drivers/media/platform/starfive/v4l2_driver/stf_video.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (C) 2021 StarFive Technology Co., Ltd.
   4  */
   5 #include "stfcamss.h"
   6 #include "stf_video.h"
   7 #include <media/media-entity.h>
   8 #include <media/v4l2-mc.h>
   9 #include <media/videobuf2-dma-sg.h>
  10 #include <media/videobuf2-vmalloc.h>
  11 #include <media/videobuf2-dma-contig.h>
  12
  13 #define USE_MEDIA_PIPELINE
  14
  15 static const struct stfcamss_format_info formats_pix_st7110_wr[] = {
  16         { MEDIA_BUS_FMT_YUYV8_2X8, V4L2_PIX_FMT_YUYV, 1,
  17           { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  18         { MEDIA_BUS_FMT_RGB565_2X8_LE, V4L2_PIX_FMT_RGB565, 1,
  19           { { 1, 1 } }, { { 1, 1 } }, { 16 } },
  20         { MEDIA_BUS_FMT_SRGGB8_1X8, V4L2_PIX_FMT_SRGGB8, 1,
  21           { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  22         { MEDIA_BUS_FMT_SGRBG8_1X8, V4L2_PIX_FMT_SGRBG8, 1,
  23           { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  24         { MEDIA_BUS_FMT_SGBRG8_1X8, V4L2_PIX_FMT_SGBRG8, 1,
  25           { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  26         { MEDIA_BUS_FMT_SBGGR8_1X8, V4L2_PIX_FMT_SBGGR8, 1,
  27           { { 1, 1 } }, { { 1, 1 } }, { 8 } },
  28         { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
  29           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  30         { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
  31           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  32         { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
  33           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  34         { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
  35           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  36 };
  37
  38 static const struct stfcamss_format_info formats_raw_st7110_isp[] = {
  39         { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12, 1,
  40           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  41         { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12, 1,
  42           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  43         { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12, 1,
  44           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  45         { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12, 1,
  46           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  47 };
  48
  49 static const struct stfcamss_format_info formats_pix_st7110_isp[] = {
  50         // { MEDIA_BUS_FMT_YUYV12_2X12, V4L2_PIX_FMT_NV12M, 2,
  51         //  { { 1, 1 }, { 1, 1 } }, { { 1, 1 }, { 1, 1 } }, { 8 , 4 } },
  52         { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
  53           { { 1, 1 } }, { { 2, 3 } }, { 8 } },
  54         { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV21, 1,
  55           { { 1, 1 } }, { { 2, 3 } }, { 8 } },
  56 };
  57
  58 static const struct stfcamss_format_info formats_st7110_isp_iti[] = {
  59         //  raw format
  60         { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
  61           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  62         { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
  63           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  64         { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
  65           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  66         { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
  67           { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  68         { MEDIA_BUS_FMT_SRGGB12_1X12, V4L2_PIX_FMT_SRGGB12, 1,
  69           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  70         { MEDIA_BUS_FMT_SGRBG12_1X12, V4L2_PIX_FMT_SGRBG12, 1,
  71           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  72         { MEDIA_BUS_FMT_SGBRG12_1X12, V4L2_PIX_FMT_SGBRG12, 1,
  73           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  74         { MEDIA_BUS_FMT_SBGGR12_1X12, V4L2_PIX_FMT_SBGGR12, 1,
  75           { { 1, 1 } }, { { 1, 1 } }, { 12 } },
  76
  77         // YUV420
  78         { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
  79           { { 1, 1 } }, { { 2, 3 } }, { 8 } },
  80         { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV21, 1,
  81           { { 1, 1 } }, { { 2, 3 } }, { 8 } },
  82
  83         // YUV444
  84         { MEDIA_BUS_FMT_YUV8_1X24, V4L2_PIX_FMT_NV24, 1,
  85           { { 1, 1 } }, { { 1, 3 } }, { 8 } },
  86         { MEDIA_BUS_FMT_VUY8_1X24, V4L2_PIX_FMT_NV42, 1,
  87           { { 1, 1 } }, { { 1, 3 } }, { 8 } },
  88 };
  89
  90 static int video_find_format(u32 code, u32 pixelformat,
  91                                 const struct stfcamss_format_info *formats,
  92                                 unsigned int nformats)
  93 {
  94         int i;
  95
  96         for (i = 0; i < nformats; i++) {
  97                 if (formats[i].code == code &&
  98                         formats[i].pixelformat == pixelformat)
  99                         return i;
 100         }
 101
 102         for (i = 0; i < nformats; i++)
 103                 if (formats[i].code == code)
 104                         return i;
 105
 106         for (i = 0; i < nformats; i++)
 107                 if (formats[i].pixelformat == pixelformat)
 108                         return i;
 109
 110         return -EINVAL;
 111 }
 112
 113 static int __video_try_fmt(struct stfcamss_video *video,
 114                 struct v4l2_format *f, int is_mp)
 115 {
 116         struct v4l2_pix_format *pix;
 117         struct v4l2_pix_format_mplane *pix_mp;
 118         const struct stfcamss_format_info *fi;
 119         u32 width, height;
 120         u32 bpl;
 121         int i, j;
 122
 123         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
 124         pix = &f->fmt.pix;
 125         pix_mp = &f->fmt.pix_mp;
 126
 127         if (is_mp) {
 128                 for (i = 0; i < video->nformats; i++)
 129                         if (pix_mp->pixelformat
 130                                 == video->formats[i].pixelformat)
 131                                 break;
 132
 133                 if (i == video->nformats)
 134                         i = 0; /* default format */
 135
 136                 fi = &video->formats[i];
 137                 width = pix_mp->width;
 138                 height = pix_mp->height;
 139
 140                 memset(pix_mp, 0, sizeof(*pix_mp));
 141
 142                 pix_mp->pixelformat = fi->pixelformat;
 143                 pix_mp->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
 144                                 STFCAMSS_FRAME_MAX_WIDTH);
 145                 pix_mp->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
 146                                 STFCAMSS_FRAME_MAX_HEIGHT);
 147                 pix_mp->num_planes = fi->planes;
 148                 for (j = 0; j < pix_mp->num_planes; j++) {
 149                         bpl = pix_mp->width / fi->hsub[j].numerator *
 150                                 fi->hsub[j].denominator * fi->bpp[j] / 8;
 151                         bpl = ALIGN(bpl, video->bpl_alignment);
 152                         pix_mp->plane_fmt[j].bytesperline = bpl;
 153                         pix_mp->plane_fmt[j].sizeimage = pix_mp->height /
 154                                 fi->vsub[j].numerator
 155                                 * fi->vsub[j].denominator * bpl;
 156                 }
 157
 158                 pix_mp->field = V4L2_FIELD_NONE;
 159                 pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
 160                 pix_mp->flags = 0;
 161                 pix_mp->ycbcr_enc =
 162                         V4L2_MAP_YCBCR_ENC_DEFAULT(pix_mp->colorspace);
 163                 pix_mp->quantization =
 164                         V4L2_MAP_QUANTIZATION_DEFAULT(true,
 165                                 pix_mp->colorspace, pix_mp->ycbcr_enc);
 166                 pix_mp->xfer_func =
 167                         V4L2_MAP_XFER_FUNC_DEFAULT(pix_mp->colorspace);
 168
 169                 st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d\n", pix_mp->width,
 170                                 pix_mp->height, fi->bpp[0]);
 171                 st_info(ST_VIDEO, "i = %d, p = %d, s = 0x%x\n", i,
 172                                 pix_mp->num_planes, pix_mp->plane_fmt[0].sizeimage);
 173
 174         } else {
 175                 for (i = 0; i < video->nformats; i++)
 176                         if (pix->pixelformat == video->formats[i].pixelformat)
 177                                 break;
 178
 179                 if (i == video->nformats)
 180                         i = 0; /* default format */
 181
 182                 fi = &video->formats[i];
 183                 width = pix->width;
 184                 height = pix->height;
 185
 186                 memset(pix, 0, sizeof(*pix));
 187
 188                 pix->pixelformat = fi->pixelformat;
 189                 pix->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
 190                                 STFCAMSS_FRAME_MAX_WIDTH);
 191                 pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
 192                                 STFCAMSS_FRAME_MAX_HEIGHT);
 193                 bpl = pix->width / fi->hsub[0].numerator *
 194                         fi->hsub[0].denominator * fi->bpp[0] / 8;
 195                 bpl = ALIGN(bpl, video->bpl_alignment);
 196                 pix->bytesperline = bpl;
 197                 pix->sizeimage = pix->height /
 198                         fi->vsub[0].numerator
 199                         * fi->vsub[0].denominator * bpl;
 200
 201                 pix->field = V4L2_FIELD_NONE;
 202                 pix->colorspace = V4L2_COLORSPACE_SRGB;
 203                 pix->flags = 0;
 204                 pix->ycbcr_enc =
 205                         V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
 206                 pix->quantization =
 207                         V4L2_MAP_QUANTIZATION_DEFAULT(true,
 208                                 pix->colorspace, pix->ycbcr_enc);
 209                 pix->xfer_func =
 210                         V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
 211
 212                 st_info(ST_VIDEO, "w, h = %d, %d, bpp = %d\n", pix->width,
 213                                 pix->height, fi->bpp[0]);
 214                 st_info(ST_VIDEO, "i = %d, s = 0x%x\n", i, pix->sizeimage);
 215         }
 216         return 0;
 217 }
 218
 219 static int stf_video_init_format(struct stfcamss_video *video, int is_mp)
 220 {
 221         int ret;
 222         struct v4l2_format format = {
 223                 .type = video->type,
 224                 .fmt.pix = {
 225                         .width = 1920,
 226                         .height = 1080,
 227                         .pixelformat = V4L2_PIX_FMT_RGB565,
 228                 },
 229         };
 230
 231         ret = __video_try_fmt(video, &format, is_mp);
 232
 233         if (ret < 0)
 234                 return ret;
 235
 236         video->active_fmt = format;
 237
 238         return 0;
 239 }
 240
 241 static int video_queue_setup(struct vb2_queue *q,
 242         unsigned int *num_buffers, unsigned int *num_planes,
 243         unsigned int sizes[], struct device *alloc_devs[])
 244 {
 245         struct stfcamss_video *video = vb2_get_drv_priv(q);
 246         const struct v4l2_pix_format *format =
 247                         &video->active_fmt.fmt.pix;
 248         const struct v4l2_pix_format_mplane *format_mp =
 249                         &video->active_fmt.fmt.pix_mp;
 250         unsigned int i;
 251
 252         st_debug(ST_VIDEO, "%s, planes = %d\n", __func__, *num_planes);
 253
 254         if (video->is_mp) {
 255                 if (*num_planes) {
 256                         if (*num_planes != format_mp->num_planes)
 257                                 return -EINVAL;
 258
 259                         for (i = 0; i < *num_planes; i++)
 260                                 if (sizes[i] <
 261                                         format_mp->plane_fmt[i].sizeimage)
 262                                         return -EINVAL;
 263
 264                         return 0;
 265                 }
 266
 267                 *num_planes = format_mp->num_planes;
 268
 269                 for (i = 0; i < *num_planes; i++)
 270                         sizes[i] = format_mp->plane_fmt[i].sizeimage;
 271         } else {
 272                 if (*num_planes) {
 273                         if (*num_planes != 1)
 274                                 return -EINVAL;
 275
 276                         if (sizes[0] < format->sizeimage)
 277                                 return -EINVAL;
 278                 }
 279
 280                 *num_planes  = 1;
 281                 sizes[0] = format->sizeimage;
 282                 if (!sizes[0])
 283                         st_err(ST_VIDEO, "%s: error size is zero!!!\n", __func__);
 284         }
 285         if ((stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC)
 286                 == STF_ISP_PAD_SRC_SCD_Y) &&
 287                 sizes[0] < ISP_SCD_Y_BUFFER_SIZE) {
 288                 sizes[0] = ISP_SCD_Y_BUFFER_SIZE;
 289         }
 290
 291         st_info(ST_VIDEO, "%s, planes = %d, size = %d\n",
 292                         __func__, *num_planes, sizes[0]);
 293         return 0;
 294 }
 295
 296 static int video_buf_init(struct vb2_buffer *vb)
 297 {
 298         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 299         struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 300         struct stfcamss_buffer *buffer =
 301                 container_of(vbuf, struct stfcamss_buffer, vb);
 302         const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
 303         const struct v4l2_pix_format_mplane *fmt_mp =
 304                                 &video->active_fmt.fmt.pix_mp;
 305         //struct sg_table *sgt;
 306         dma_addr_t *paddr;
 307         unsigned int i;
 308         buffer->sizeimage = 0;
 309
 310         if (video->is_mp) {
 311                 for (i = 0; i < fmt_mp->num_planes; i++) {
 312                         paddr = vb2_plane_cookie(vb, i);
 313                         buffer->addr[i] = *paddr;
 314                 buffer->sizeimage += vb2_plane_size(vb, i);
 315                 }
 316
 317                 if (fmt_mp->num_planes == 1
 318                         && (fmt_mp->pixelformat == V4L2_PIX_FMT_NV12
 319                         || fmt_mp->pixelformat == V4L2_PIX_FMT_NV21
 320                         || fmt_mp->pixelformat == V4L2_PIX_FMT_NV16
 321                         || fmt_mp->pixelformat == V4L2_PIX_FMT_NV61))
 322                         buffer->addr[1] = buffer->addr[0] +
 323                                         fmt_mp->plane_fmt[0].bytesperline *
 324                                         fmt_mp->height;
 325         } else {
 326                 paddr = vb2_plane_cookie(vb, 0);
 327                 buffer->sizeimage = vb2_plane_size(vb, 0);
 328                 buffer->addr[0] = *paddr;
 329                 if (fmt->pixelformat == V4L2_PIX_FMT_NV12
 330                         || fmt->pixelformat == V4L2_PIX_FMT_NV21
 331                         || fmt->pixelformat == V4L2_PIX_FMT_NV16
 332                         || fmt->pixelformat == V4L2_PIX_FMT_NV61)
 333                         buffer->addr[1] = buffer->addr[0] +
 334                                 fmt->bytesperline *
 335                                 fmt->height;
 336         }
 337
 338         if (stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC)
 339                 == STF_ISP_PAD_SRC_SCD_Y)
 340                 buffer->addr[1] = buffer->addr[0] + ISP_YHIST_BUFFER_SIZE;
 341
 342         return 0;
 343 }
 344
 345 static int video_buf_prepare(struct vb2_buffer *vb)
 346 {
 347         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 348         struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 349         const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
 350         const struct v4l2_pix_format_mplane *fmt_mp =
 351                                         &video->active_fmt.fmt.pix_mp;
 352         unsigned int i;
 353
 354         if (video->is_mp) {
 355                 for (i = 0; i < fmt_mp->num_planes; i++) {
 356                         if (fmt_mp->plane_fmt[i].sizeimage
 357                                         > vb2_plane_size(vb, i))
 358                                 return -EINVAL;
 359
 360                         vb2_set_plane_payload(vb, i,
 361                                         fmt_mp->plane_fmt[i].sizeimage);
 362                 }
 363         } else {
 364                 if (fmt->sizeimage > vb2_plane_size(vb, 0)) {
 365                         st_err(ST_VIDEO, "sizeimage = %d, plane size = %d\n",
 366                                 fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0));
 367                         return -EINVAL;
 368                 }
 369                 vb2_set_plane_payload(vb, 0, fmt->sizeimage);
 370         }
 371
 372         vbuf->field = V4L2_FIELD_NONE;
 373
 374         return 0;
 375 }
 376
 377 static void video_buf_queue(struct vb2_buffer *vb)
 378 {
 379         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 380         struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 381         struct stfcamss_buffer *buffer =
 382                 container_of(vbuf, struct stfcamss_buffer, vb);
 383
 384         video->ops->queue_buffer(video, buffer);
 385 }
 386
 387 static int video_mbus_to_pix_mp(const struct v4l2_mbus_framefmt *mbus,
 388                                 struct v4l2_pix_format_mplane *pix,
 389                                 const struct stfcamss_format_info *f,
 390                                 unsigned int alignment)
 391 {
 392         unsigned int i;
 393         u32 bytesperline;
 394
 395         memset(pix, 0, sizeof(*pix));
 396         v4l2_fill_pix_format_mplane(pix, mbus);
 397         pix->pixelformat = f->pixelformat;
 398         pix->num_planes = f->planes;
 399         for (i = 0; i < pix->num_planes; i++) {
 400                 bytesperline = pix->width / f->hsub[i].numerator *
 401                         f->hsub[i].denominator * f->bpp[i] / 8;
 402                 bytesperline = ALIGN(bytesperline, alignment);
 403                 pix->plane_fmt[i].bytesperline = bytesperline;
 404                 pix->plane_fmt[i].sizeimage = pix->height /
 405                                 f->vsub[i].numerator * f->vsub[i].denominator *
 406                                 bytesperline;
 407         }
 408
 409         return 0;
 410 }
 411
 412 static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
 413                         struct v4l2_pix_format *pix,
 414                         const struct stfcamss_format_info *f,
 415                         unsigned int alignment)
 416 {
 417         u32 bytesperline;
 418
 419         memset(pix, 0, sizeof(*pix));
 420         v4l2_fill_pix_format(pix, mbus);
 421         pix->pixelformat = f->pixelformat;
 422         bytesperline = pix->width / f->hsub[0].numerator *
 423                 f->hsub[0].denominator * f->bpp[0] / 8;
 424         bytesperline = ALIGN(bytesperline, alignment);
 425         pix->bytesperline = bytesperline;
 426         pix->sizeimage = pix->height /
 427                         f->vsub[0].numerator * f->vsub[0].denominator *
 428                         bytesperline;
 429         return 0;
 430 }
 431
 432 static struct v4l2_subdev *video_remote_subdev(
 433                 struct stfcamss_video *video, u32 *pad)
 434 {
 435         struct media_pad *remote;
 436
 437         remote = media_entity_remote_pad(&video->pad);
 438
 439         if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 440                 return NULL;
 441
 442         if (pad)
 443                 *pad = remote->index;
 444
 445         return media_entity_to_v4l2_subdev(remote->entity);
 446 }
 447
 448 static int video_get_subdev_format(struct stfcamss_video *video,
 449                 struct v4l2_format *format)
 450 {
 451         struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
 452         struct v4l2_pix_format_mplane *pix_mp =
 453                                 &video->active_fmt.fmt.pix_mp;
 454         struct v4l2_subdev_format fmt;
 455         struct v4l2_subdev *subdev;
 456         u32 pixelformat;
 457         u32 pad;
 458         int ret;
 459
 460         subdev = video_remote_subdev(video, &pad);
 461         if (subdev == NULL)
 462                 return -EPIPE;
 463
 464         fmt.pad = pad;
 465         fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 466
 467         ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 468         if (ret)
 469                 return ret;
 470
 471         if (video->is_mp)
 472                 pixelformat = pix_mp->pixelformat;
 473         else
 474                 pixelformat = pix->pixelformat;
 475         ret = video_find_format(fmt.format.code, pixelformat,
 476                                 video->formats, video->nformats);
 477         if (ret < 0)
 478                 return ret;
 479
 480         format->type = video->type;
 481
 482         if (video->is_mp)
 483                 return video_mbus_to_pix_mp(&fmt.format, &format->fmt.pix_mp,
 484                                 &video->formats[ret], video->bpl_alignment);
 485         else
 486                 return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
 487                                 &video->formats[ret], video->bpl_alignment);
 488 }
 489
 490 static int video_check_format(struct stfcamss_video *video)
 491 {
 492         struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
 493         struct v4l2_pix_format_mplane *pix_mp =
 494                                 &video->active_fmt.fmt.pix_mp;
 495         struct v4l2_format format;
 496         struct v4l2_pix_format *sd_pix = &format.fmt.pix;
 497         struct v4l2_pix_format_mplane *sd_pix_mp = &format.fmt.pix_mp;
 498         int ret;
 499
 500         if (video->is_mp) {
 501                 sd_pix_mp->pixelformat = pix_mp->pixelformat;
 502                 ret = video_get_subdev_format(video, &format);
 503                 if (ret < 0)
 504                         return ret;
 505
 506                 if (pix_mp->pixelformat != sd_pix_mp->pixelformat ||
 507                         pix_mp->height > sd_pix_mp->height ||
 508                         pix_mp->width > sd_pix_mp->width ||
 509                         pix_mp->num_planes != sd_pix_mp->num_planes ||
 510                         pix_mp->field != format.fmt.pix_mp.field) {
 511                         st_err(ST_VIDEO,
 512                                 "%s, not match:\n"
 513                                 "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
 514                                 __func__,
 515                                 pix_mp->pixelformat, sd_pix_mp->pixelformat,
 516                                 pix_mp->height, sd_pix_mp->height,
 517                                 pix_mp->field, format.fmt.pix_mp.field);
 518                         return -EPIPE;
 519                 }
 520
 521         } else {
 522                 sd_pix->pixelformat = pix->pixelformat;
 523                 ret = video_get_subdev_format(video, &format);
 524                 if (ret < 0)
 525                         return ret;
 526
 527                 if (pix->pixelformat != sd_pix->pixelformat ||
 528                         pix->height > sd_pix->height ||
 529                         pix->width > sd_pix->width ||
 530                         pix->field != format.fmt.pix.field) {
 531                         st_err(ST_VIDEO,
 532                                 "%s, not match:\n"
 533                                 "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
 534                                 __func__,
 535                                 pix->pixelformat, sd_pix->pixelformat,
 536                                 pix->height, sd_pix->height,
 537                                 pix->field, format.fmt.pix.field);
 538                         return -EPIPE;
 539                 }
 540         }
 541         return 0;
 542 }
 543
 544 static int video_start_streaming(struct vb2_queue *q, unsigned int count)
 545 {
 546         struct stfcamss_video *video = vb2_get_drv_priv(q);
 547         struct video_device *vdev = &video->vdev;
 548         struct media_entity *entity;
 549         struct media_pad *pad;
 550         struct v4l2_subdev *subdev;
 551         int ret;
 552
 553 #ifdef USE_MEDIA_PIPELINE
 554         // ret = media_pipeline_start(&vdev->entity, &video->pipe);
 555         ret = media_pipeline_start(&vdev->entity, &video->stfcamss->pipe);
 556         if (ret < 0) {
 557                 st_err(ST_VIDEO,
 558                         "Failed to media_pipeline_start: %d\n", ret);
 559                 return ret;
 560         }
 561 #endif
 562         ret = video_check_format(video);
 563         if (ret < 0)
 564                 goto error;
 565         entity = &vdev->entity;
 566         while (1) {
 567                 pad = &entity->pads[0];
 568                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 569                         break;
 570
 571                 pad = media_entity_remote_pad(pad);
 572                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 573                         break;
 574
 575                 entity = pad->entity;
 576                 subdev = media_entity_to_v4l2_subdev(entity);
 577
 578                 ret = v4l2_subdev_call(subdev, video, s_stream, 1);
 579                 if (ret < 0 && ret != -ENOIOCTLCMD)
 580                         goto error;
 581         }
 582         return 0;
 583
 584 error:
 585 #ifdef USE_MEDIA_PIPELINE
 586         media_pipeline_stop(&vdev->entity);
 587 #endif
 588         video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
 589         return ret;
 590 }
 591
 592 static void video_stop_streaming(struct vb2_queue *q)
 593 {
 594         struct stfcamss_video *video = vb2_get_drv_priv(q);
 595         struct video_device *vdev = &video->vdev;
 596         struct media_entity *entity;
 597         struct media_pad *pad;
 598         struct v4l2_subdev *subdev;
 599
 600         entity = &vdev->entity;
 601         while (1) {
 602                 pad = &entity->pads[0];
 603                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 604                         break;
 605
 606                 pad = media_entity_remote_pad(pad);
 607                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 608                         break;
 609
 610                 entity = pad->entity;
 611                 subdev = media_entity_to_v4l2_subdev(entity);
 612
 613                 v4l2_subdev_call(subdev, video, s_stream, 0);
 614         }
 615
 616 #ifdef USE_MEDIA_PIPELINE
 617         media_pipeline_stop(&vdev->entity);
 618 #endif
 619         video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
 620 }
 621
 622 static const struct vb2_ops stf_video_vb2_q_ops = {
 623         .queue_setup     = video_queue_setup,
 624         .wait_prepare    = vb2_ops_wait_prepare,
 625         .wait_finish     = vb2_ops_wait_finish,
 626         .buf_init        = video_buf_init,
 627         .buf_prepare     = video_buf_prepare,
 628         .buf_queue       = video_buf_queue,
 629         .start_streaming = video_start_streaming,
 630         .stop_streaming  = video_stop_streaming,
 631 };
 632
 633 /* -----------------------------------------------------
 634  * V4L2 ioctls
 635  */
 636
 637 static int getcrop_pad_id(int video_id)
 638 {
 639         return stf_vin_map_isp_pad(video_id, STF_ISP_PAD_SRC);
 640 }
 641
 642 static int video_querycap(struct file *file, void *fh,
 643                         struct v4l2_capability *cap)
 644 {
 645         struct stfcamss_video *video = video_drvdata(file);
 646
 647         strscpy(cap->driver, "stf camss", sizeof(cap->driver));
 648         strscpy(cap->card, "Starfive Camera Subsystem", sizeof(cap->card));
 649         snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 650                 dev_name(video->stfcamss->dev));
 651         return 0;
 652 }
 653
 654 static int video_get_unique_pixelformat_by_index(struct stfcamss_video *video,
 655                                                 int ndx)
 656 {
 657         int i, j, k;
 658
 659         /* find index "i" of "k"th unique pixelformat in formats array */
 660         k = -1;
 661         for (i = 0; i < video->nformats; i++) {
 662                 for (j = 0; j < i; j++) {
 663                         if (video->formats[i].pixelformat ==
 664                                 video->formats[j].pixelformat)
 665                                 break;
 666                 }
 667
 668                 if (j == i)
 669                         k++;
 670
 671                 if (k == ndx)
 672                         return i;
 673         }
 674
 675         return -EINVAL;
 676 }
 677
 678 static int video_get_pixelformat_by_mbus_code(struct stfcamss_video *video,
 679                                                 u32 mcode)
 680 {
 681         int i;
 682
 683         for (i = 0; i < video->nformats; i++) {
 684                 if (video->formats[i].code == mcode)
 685                         return i;
 686         }
 687
 688         return -EINVAL;
 689 }
 690
 691 static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 692 {
 693         struct stfcamss_video *video = video_drvdata(file);
 694         int i;
 695
 696         st_debug(ST_VIDEO, "%s:\n0x%x 0x%x\n 0x%x, 0x%x\n0x%x\n",
 697                 __func__,
 698                 f->type, video->type,
 699                 f->index, video->nformats,
 700                 f->mbus_code);
 701
 702         if (f->type != video->type)
 703                 return -EINVAL;
 704         if (f->index >= video->nformats)
 705                 return -EINVAL;
 706
 707         if (f->mbus_code) {
 708                 /* Each entry in formats[] table has unique mbus_code */
 709                 if (f->index > 0)
 710                         return -EINVAL;
 711
 712                 i = video_get_pixelformat_by_mbus_code(video, f->mbus_code);
 713         } else {
 714                 i = video_get_unique_pixelformat_by_index(video, f->index);
 715         }
 716
 717         if (i < 0)
 718                 return -EINVAL;
 719
 720         f->pixelformat = video->formats[i].pixelformat;
 721
 722         return 0;
 723 }
 724
 725 static struct v4l2_subdev *get_senname(struct file *file, const char *name)
 726 {
 727         struct stfcamss_video *video = video_drvdata(file);
 728         struct video_device *vdev = &video->vdev;
 729         struct media_entity *entity = &vdev->entity;
 730         struct v4l2_subdev *subdev;
 731         struct media_pad *pad;
 732         char vin_name[40];
 733
 734         strcpy(vin_name, entity->name);
 735         while (1) {
 736                 pad = &entity->pads[0];
 737                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 738                         break;
 739                 pad = media_entity_remote_pad(pad);
 740                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 741                         break;
 742                 entity = pad->entity;
 743         }
 744         if (!strncmp(vin_name, entity->name, 13)) {
 745                 st_err(ST_VIDEO, "===== [%s] Please configure pipeline first =====\n", name);
 746                 return NULL;
 747         }
 748         subdev = media_entity_to_v4l2_subdev(entity);
 749
 750         return subdev;
 751 }
 752
 753 static int video_enum_framesizes(struct file *file, void *fh,
 754                                 struct v4l2_frmsizeenum *fsize)
 755 {
 756         struct v4l2_subdev_frame_size_enum fse = {0};
 757         struct v4l2_subdev_mbus_code_enum code = {0};
 758         struct stfcamss_video *video = video_drvdata(file);
 759         struct video_device *vdev = &video->vdev;
 760         struct media_entity *entity = &vdev->entity;
 761         struct media_entity *sensor;
 762         struct v4l2_subdev *subdev;
 763         struct media_pad *pad;
 764         bool support_selection = false;
 765         int i;
 766         int ret;
 767
 768         for (i = 0; i < video->nformats; i++) {
 769                 if (video->formats[i].pixelformat == fsize->pixel_format)
 770                         break;
 771         }
 772
 773         if (i == video->nformats)
 774                 return -EINVAL;
 775
 776         entity = &vdev->entity;
 777         while (1) {
 778                 pad = &entity->pads[0];
 779                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
 780                         break;
 781
 782                 pad = media_entity_remote_pad(pad);
 783                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 784                         break;
 785
 786                 entity = pad->entity;
 787                 subdev = media_entity_to_v4l2_subdev(entity);
 788
 789                 if (subdev->ops->pad->set_selection) {
 790                         support_selection = true;
 791                         break;
 792                 }
 793         }
 794
 795         if (support_selection) {
 796                 if (fsize->index)
 797                         return -ENOTTY;
 798                 fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 799                 fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
 800                 fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
 801                 fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
 802                 fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT;
 803                 fsize->stepwise.step_width = 1;
 804                 fsize->stepwise.step_height = 1;
 805         } else {
 806                 entity = &vdev->entity;
 807                 sensor = stfcamss_find_sensor(entity);
 808                 if (!sensor)
 809                         return -ENOTTY;
 810
 811                 subdev = media_entity_to_v4l2_subdev(sensor);
 812                 code.index = 0;
 813                 code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 814                 ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
 815                 if (ret < 0)
 816                         return -EINVAL;
 817                 fse.index = fsize->index;
 818                 fse.code = code.code;
 819                 fse.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 820                 ret = v4l2_subdev_call(subdev, pad, enum_frame_size, NULL, &fse);
 821                 if (ret < 0)
 822                         return -EINVAL;
 823                 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 824                 fsize->discrete.width = fse.min_width;
 825                 fsize->discrete.height = fse.min_height;
 826         }
 827
 828         return 0;
 829 }
 830
 831 static int video_enum_frameintervals(struct file *file, void *fh,
 832                                 struct v4l2_frmivalenum *fival)
 833 {
 834         int ret = 0;
 835         struct stfcamss_video *video = video_drvdata(file);
 836         struct video_device *vdev = &video->vdev;
 837         struct media_entity *entity = &vdev->entity;
 838         struct media_entity *sensor;
 839         struct v4l2_subdev *subdev;
 840         struct v4l2_subdev_mbus_code_enum code = {0};
 841         struct v4l2_subdev_frame_interval_enum fie = {0};
 842
 843         sensor = stfcamss_find_sensor(entity);
 844         if (!sensor)
 845                 return -EINVAL;
 846         fie.index = fival->index;
 847         fie.width = fival->width;
 848         fie.height = fival->height;
 849         fie.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 850         subdev = media_entity_to_v4l2_subdev(sensor);
 851
 852         code.index = 0;
 853         code.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 854
 855         ret = v4l2_subdev_call(subdev, pad, enum_mbus_code, NULL, &code);
 856         if (ret < 0)
 857                 return -EINVAL;
 858
 859         fie.code = code.code;
 860         ret = v4l2_subdev_call(subdev, pad, enum_frame_interval, NULL, &fie);
 861         if (ret < 0)
 862                 return ret;
 863
 864         fival->type = V4L2_FRMSIZE_TYPE_DISCRETE;
 865         fival->discrete = fie.interval;
 866
 867         return 0;
 868 }
 869
 870 static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 871 {
 872         struct stfcamss_video *video = video_drvdata(file);
 873
 874         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
 875         st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x,0x%x\n",
 876                         __func__, video->active_fmt.type,
 877                         video->active_fmt.fmt.pix.pixelformat);
 878         *f = video->active_fmt;
 879         return 0;
 880 }
 881
 882 static int video_g_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
 883 {
 884         struct stfcamss_video *video = video_drvdata(file);
 885
 886         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
 887         st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
 888                         __func__, video->active_fmt.type);
 889         *f = video->active_fmt;
 890         return 0;
 891 }
 892
 893 static int video_entity_s_fmt(struct stfcamss_video *video,
 894                         struct media_entity *entity,
 895                         struct v4l2_subdev_state *state,
 896                         struct v4l2_subdev_format *fmt, u32 dst_code)
 897 {
 898         struct v4l2_subdev *subdev;
 899         struct media_pad *pad;
 900         struct v4l2_mbus_framefmt *mf = &fmt->format;
 901         u32 width, height, code;
 902         int ret, index = 0;
 903
 904         code = mf->code;
 905         width = mf->width;
 906         height = mf->height;
 907         subdev = media_entity_to_v4l2_subdev(entity);
 908         while (1) {
 909                 if (index >= entity->num_pads)
 910                         break;
 911                 pad = &entity->pads[index];
 912                 pad = media_entity_remote_pad(pad);
 913                 if (pad && is_media_entity_v4l2_subdev(pad->entity)) {
 914                         fmt->pad = index;
 915                         if (index)
 916                                 mf->code = dst_code;
 917                         ret = v4l2_subdev_call(subdev, pad, set_fmt, state, fmt);
 918                         st_warn(ST_VIDEO,
 919                                 "\"%s\":%d pad fmt set to 0x%x %ux%u, dst_code = 0x%x, ret=%d\n",
 920                                 subdev->name, fmt->pad, mf->code,
 921                                 mf->width, mf->height, dst_code, ret);
 922                         if (mf->code != code ||
 923                                 mf->width != width || mf->height != height) {
 924                                 st_warn(ST_VIDEO,
 925                                         "\"%s\":%d pad fmt has been"
 926                                         " changed to 0x%x %ux%u\n",
 927                                         subdev->name, fmt->pad, mf->code,
 928                                         mf->width, mf->height);
 929                         }
 930                         if (index)
 931                                 ret = video_entity_s_fmt(video, pad->entity, state, fmt, dst_code);
 932                 }
 933
 934                 if (ret < 0 && ret != -ENOIOCTLCMD)
 935                         break;
 936                 index++;
 937         }
 938         return ret;
 939 }
 940
 941 static int video_pipeline_s_fmt(struct stfcamss_video *video,
 942                         struct v4l2_subdev_state *state,
 943                         struct v4l2_format *f)
 944 {
 945         struct video_device *vdev = &video->vdev;
 946         struct media_entity *entity = &vdev->entity;
 947         struct v4l2_subdev *subdev;
 948         int ret, index;
 949         struct v4l2_subdev_format fmt = {
 950                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 951                 .reserved = {getcrop_pad_id(video->id)}
 952         };
 953         struct v4l2_mbus_framefmt *mf = &fmt.format;
 954         struct v4l2_pix_format *pix = &f->fmt.pix;
 955         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 956         struct media_entity *sensor;
 957         u32 width, height, code;
 958         struct media_pad *pad;
 959
 960         /* pix to mbus format */
 961         if (video->is_mp) {
 962                 index = video_find_format(mf->code,
 963                                         pix_mp->pixelformat,
 964                                         video->formats, video->nformats);
 965                 if (index < 0)
 966                         return index;
 967                 v4l2_fill_mbus_format_mplane(mf, pix_mp);
 968                 mf->code = video->formats[index].code;
 969         } else {
 970                 index = video_find_format(mf->code,
 971                                         pix->pixelformat,
 972                                         video->formats, video->nformats);
 973                 if (index < 0)
 974                         return index;
 975                 v4l2_fill_mbus_format(mf, pix, video->formats[index].code);
 976         }
 977         code = mf->code;
 978         width = mf->width;
 979         height = mf->height;
 980         sensor = stfcamss_find_sensor(entity);
 981         if (sensor) {
 982                 subdev = media_entity_to_v4l2_subdev(sensor);
 983                 ret = v4l2_subdev_call(subdev, pad, set_fmt, state, &fmt);
 984                 st_warn(ST_VIDEO,
 985                         "\"%s\":%d pad fmt set to 0x%x %ux%u\n",
 986                         subdev->name, fmt.pad, mf->code,
 987                         mf->width, mf->height);
 988                 if (mf->code != code ||
 989                         mf->width != width || mf->height != height) {
 990                         st_warn(ST_VIDEO,
 991                                 "\"%s\":%d pad fmt has been"
 992                                 " changed to 0x%x %ux%u\n",
 993                                 subdev->name, fmt.pad, mf->code,
 994                                 mf->width, mf->height);
 995                 }
 996         } else {
 997                 st_err(ST_VIDEO, "Can't find sensor\n");
 998                 return -EINVAL;
 999         }
1000         /*
1001          * Starting from sensor subdevice, walk within
1002          * pipeline and set format on each subdevice
1003          */
1004         sensor = stfcamss_find_sensor(entity);
1005         pad = media_entity_remote_pad(&sensor->pads[0]);
1006         ret = video_entity_s_fmt(video, pad->entity, state, &fmt, code);
1007
1008         if (ret < 0 && ret != -ENOIOCTLCMD) {
1009                 return ret;
1010         }
1011
1012         index = video_find_format(mf->code,
1013                                 video->formats[index].pixelformat,
1014                                 video->formats, video->nformats);
1015         st_debug(ST_VIDEO, "%s, code=%x, index=%d\n",
1016                         __func__, mf->code, index);
1017
1018         if (index < 0)
1019                 return index;
1020
1021         if (video->is_mp)
1022                 video_mbus_to_pix_mp(mf, pix_mp,
1023                                 &video->formats[index], video->bpl_alignment);
1024         else
1025                 video_mbus_to_pix(mf, pix,
1026                                 &video->formats[index], video->bpl_alignment);
1027
1028         ret = __video_try_fmt(video, f, video->is_mp);
1029         if (ret < 0)
1030                 return ret;
1031
1032         return 0;
1033 }
1034
1035 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
1036 {
1037         struct stfcamss_video *video = video_drvdata(file);
1038         int ret;
1039
1040         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x, v4l2fmt=%x\n",
1041                         __func__, f->type, f->fmt.pix.pixelformat);
1042
1043         if (vb2_is_busy(&video->vb2_q))
1044                 return -EBUSY;
1045
1046         ret = __video_try_fmt(video, f, false);
1047         if (ret < 0)
1048                 return ret;
1049
1050         ret = video_pipeline_s_fmt(video, NULL, f);
1051
1052         st_debug(ST_VIDEO, "%s, pixelformat=0x%x, ret=%d\n",
1053                         __func__, f->fmt.pix.pixelformat, ret);
1054         if (ret < 0)
1055                 return ret;
1056
1057         video->active_fmt = *f;
1058
1059         return 0;
1060 }
1061
1062 static int video_s_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
1063 {
1064         struct stfcamss_video *video = video_drvdata(file);
1065         int ret;
1066
1067         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
1068         if (vb2_is_busy(&video->vb2_q))
1069                 return -EBUSY;
1070
1071         ret = __video_try_fmt(video, f, true);
1072         if (ret < 0)
1073                 return ret;
1074
1075         ret = video_pipeline_s_fmt(video, NULL, f);
1076         if (ret < 0)
1077                 return ret;
1078
1079         video->active_fmt = *f;
1080
1081         return 0;
1082 }
1083
1084 static int video_try_fmt(struct file *file,
1085                 void *fh, struct v4l2_format *f)
1086 {
1087         struct stfcamss_video *video = video_drvdata(file);
1088
1089         return __video_try_fmt(video, f, false);
1090 }
1091
1092 static int video_try_fmt_mp(struct file *file,
1093                 void *fh, struct v4l2_format *f)
1094 {
1095         struct stfcamss_video *video = video_drvdata(file);
1096
1097         return __video_try_fmt(video, f, true);
1098 }
1099
1100 static int video_enum_input(struct file *file, void *fh,
1101                         struct v4l2_input *input)
1102 {
1103         if (input->index > 0)
1104                 return -EINVAL;
1105
1106         strscpy(input->name, "camera", sizeof(input->name));
1107         input->type = V4L2_INPUT_TYPE_CAMERA;
1108
1109         return 0;
1110 }
1111
1112 static int video_g_input(struct file *file, void *fh, unsigned int *input)
1113 {
1114         *input = 0;
1115
1116         return 0;
1117 }
1118
1119 static int video_s_input(struct file *file, void *fh, unsigned int input)
1120 {
1121         return input == 0 ? 0 : -EINVAL;
1122 }
1123
1124 static int video_g_parm(struct file *file, void *priv,
1125                         struct v4l2_streamparm *p)
1126 {
1127         struct stfcamss_video *video = video_drvdata(file);
1128         struct video_device *vdev = &video->vdev;
1129         struct media_entity *entity;
1130         struct v4l2_subdev *subdev;
1131         struct media_pad *pad;
1132         int ret, is_support = 0;
1133
1134         entity = &vdev->entity;
1135         while (1) {
1136                 pad = &entity->pads[0];
1137                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1138                         break;
1139
1140                 pad = media_entity_remote_pad(pad);
1141                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1142                         break;
1143
1144                 entity = pad->entity;
1145                 subdev = media_entity_to_v4l2_subdev(entity);
1146
1147                 ret = v4l2_g_parm_cap(vdev, subdev, p);
1148                 if (ret < 0 && ret != -ENOIOCTLCMD)
1149                         break;
1150                 if (!ret)
1151                         is_support = 1;
1152         }
1153
1154         return is_support ? 0 : ret;
1155 }
1156
1157 static int video_s_parm(struct file *file, void *priv,
1158                         struct v4l2_streamparm *p)
1159 {
1160         struct stfcamss_video *video = video_drvdata(file);
1161         struct video_device *vdev = &video->vdev;
1162         struct media_entity *entity;
1163         struct v4l2_subdev *subdev;
1164         struct media_pad *pad;
1165         struct v4l2_streamparm tmp_p;
1166         int ret, is_support = 0;
1167
1168         entity = &vdev->entity;
1169         while (1) {
1170                 pad = &entity->pads[0];
1171                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1172                         break;
1173
1174                 pad = media_entity_remote_pad(pad);
1175                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1176                         break;
1177
1178                 entity = pad->entity;
1179                 subdev = media_entity_to_v4l2_subdev(entity);
1180
1181                 tmp_p = *p;
1182                 ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
1183                 if (ret < 0 && ret != -ENOIOCTLCMD)
1184                         break;
1185                 if (!ret) {
1186                         is_support = 1;
1187                         *p = tmp_p;
1188                 }
1189         }
1190
1191         return is_support ? 0 : ret;
1192 }
1193
1194 /* Crop ioctls */
1195 int video_g_pixelaspect(struct file *file, void *fh,
1196                             int buf_type, struct v4l2_fract *aspect)
1197 {
1198         return 0;
1199 }
1200
1201 int video_g_selection(struct file *file, void *fh,
1202                           struct v4l2_selection *s)
1203 {
1204         struct stfcamss_video *video = video_drvdata(file);
1205         struct video_device *vdev = &video->vdev;
1206         struct media_entity *entity;
1207         struct v4l2_subdev *subdev;
1208         struct media_pad *pad;
1209         struct v4l2_subdev_selection sel = {
1210                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1211                 .pad = getcrop_pad_id(video->id),
1212                 .target = s->target,
1213                 .r = s->r,
1214                 .flags = s->flags,
1215         };
1216         int ret;
1217
1218         st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
1219                         __func__, sel.target, s->target);
1220         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
1221                 && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1222                 return -EINVAL;
1223
1224         entity = &vdev->entity;
1225         while (1) {
1226                 pad = &entity->pads[0];
1227                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1228                         break;
1229
1230                 pad = media_entity_remote_pad(pad);
1231                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1232                         break;
1233
1234                 entity = pad->entity;
1235                 subdev = media_entity_to_v4l2_subdev(entity);
1236
1237                 ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel);
1238                 if (!ret) {
1239                         s->r = sel.r;
1240                         s->flags = sel.flags;
1241                         break;
1242                 }
1243                 if (ret != -ENOIOCTLCMD)
1244                         break;
1245         }
1246
1247         return ret;
1248 }
1249
1250 int video_s_selection(struct file *file, void *fh,
1251                         struct v4l2_selection *s)
1252 {
1253         struct stfcamss_video *video = video_drvdata(file);
1254         struct video_device *vdev = &video->vdev;
1255         struct media_entity *entity;
1256         struct v4l2_subdev *subdev;
1257         struct media_pad *pad;
1258         struct v4l2_subdev_selection sel = {
1259                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1260                 .pad = getcrop_pad_id(video->id),
1261                 .target = s->target,
1262                 .r = s->r,
1263                 .flags = s->flags,
1264         };
1265         struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
1266         struct v4l2_pix_format_mplane *format_mp =
1267                                                 &video->active_fmt.fmt.pix_mp;
1268         int ret;
1269
1270         st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
1271                         __func__, sel.target, s->target);
1272         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
1273                 && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1274                 return -EINVAL;
1275
1276         entity = &vdev->entity;
1277         while (1) {
1278                 pad = &entity->pads[0];
1279                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1280                         break;
1281
1282                 pad = media_entity_remote_pad(pad);
1283                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1284                         break;
1285
1286                 entity = pad->entity;
1287                 subdev = media_entity_to_v4l2_subdev(entity);
1288
1289                 ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel);
1290                 if (!ret) {
1291                         s->r = sel.r;
1292                         s->flags = sel.flags;
1293                         format->width = s->r.width;
1294                         format->height = s->r.height;
1295                         format_mp->width = s->r.width;
1296                         format_mp->height = s->r.height;
1297                         ret = __video_try_fmt(video, &video->active_fmt,
1298                                         video->is_mp);
1299                         if (ret < 0)
1300                                 return ret;
1301                         break;
1302                 }
1303                 if (ret != -ENOIOCTLCMD)
1304                         break;
1305         }
1306
1307         st_debug(ST_VIDEO, "ret = 0x%x, -EINVAL = 0x%x\n", ret, -EINVAL);
1308
1309         return ret;
1310 }
1311
1312 int video_g_ctrl(struct file *file, void *fh,
1313                                 struct v4l2_control *ctrls)
1314 {
1315         struct v4l2_subdev *subdev;
1316         int ret;
1317
1318         subdev = get_senname(file, (char *)__func__);
1319         if (!subdev)
1320                 return -EINVAL;
1321
1322         ret = v4l2_g_ctrl(subdev->ctrl_handler, ctrls);
1323
1324         return ret;
1325 }
1326
1327 static int video_s_ctrl(struct file *file, void *fh,
1328                                 struct v4l2_control *ctrl)
1329 {
1330         struct v4l2_subdev *subdev;
1331         struct v4l2_fh *vfh;
1332         int ret;
1333
1334         subdev = get_senname(file, (char *)__func__);
1335         if (!subdev)
1336                 return -EINVAL;
1337
1338         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1339         if (!vfh->ctrl_handler)
1340                 return -ENOTTY;
1341
1342         ret = v4l2_s_ctrl(vfh, subdev->ctrl_handler, ctrl);
1343
1344         return ret;
1345 }
1346
1347 #ifdef UNUSED_CODE
1348 static int video_query_ext_ctrl(struct file *file, void *fh,
1349                                     struct v4l2_query_ext_ctrl *qec)
1350 {
1351         struct v4l2_subdev *subdev;
1352         int ret;
1353
1354         subdev = get_senname(file, (char *)__func__);
1355         if (!subdev)
1356                 return -EINVAL;
1357
1358         ret = v4l2_query_ext_ctrl(subdev->ctrl_handler, qec);
1359
1360         return ret;
1361 }
1362 #endif
1363
1364 static int video_g_ext_ctrls(struct file *file, void *fh,
1365                                  struct v4l2_ext_controls *ctrls)
1366 {
1367         struct stfcamss_video *video = video_drvdata(file);
1368         struct video_device *vdev = &video->vdev;
1369         struct v4l2_subdev *subdev;
1370         int ret;
1371
1372         subdev = get_senname(file, (char *)__func__);
1373         if (!subdev)
1374                 return -EINVAL;
1375
1376         ret = v4l2_g_ext_ctrls(subdev->ctrl_handler,
1377                                                 vdev, subdev->v4l2_dev->mdev, ctrls);
1378
1379         return ret;
1380 }
1381
1382 static int video_queryctrl(struct file *file, void *fh,
1383                                struct v4l2_queryctrl *qc)
1384 {
1385         struct stfcamss_video *video = video_drvdata(file);
1386         struct video_device *vdev = &video->vdev;
1387         struct media_entity *entity = &vdev->entity;
1388         struct media_entity *sensor;
1389         struct v4l2_subdev *subdev;
1390         int ret = 0;
1391
1392         sensor = stfcamss_find_sensor(entity);
1393         if (sensor) {
1394                 subdev = media_entity_to_v4l2_subdev(sensor);
1395                 ret = v4l2_queryctrl(subdev->ctrl_handler, qc);
1396         } else {
1397         //      st_err(ST_VIDEO, "== [%s] Please configure pipeline first ==\n", __func__);
1398                 return -EINVAL;
1399         }
1400
1401         return ret;
1402 }
1403
1404 static int video_s_ext_ctrls(struct file *file, void *fh,
1405                                  struct v4l2_ext_controls *ctrls)
1406 {
1407         struct stfcamss_video *video = video_drvdata(file);
1408         struct video_device *vdev = &video->vdev;
1409         struct v4l2_subdev *subdev;
1410         struct v4l2_fh *vfh;
1411         int ret;
1412
1413         subdev = get_senname(file, (char *)__func__);
1414         if (!subdev)
1415                 return -EINVAL;
1416
1417         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1418         if (!vfh->ctrl_handler)
1419                 return -ENOTTY;
1420         ret = v4l2_s_ext_ctrls(vfh, subdev->ctrl_handler,
1421                                         vdev, subdev->v4l2_dev->mdev, ctrls);
1422
1423         return ret;
1424 }
1425
1426 static int video_try_ext_ctrls(struct file *file, void *fh,
1427                                    struct v4l2_ext_controls *ctrls)
1428 {
1429         struct stfcamss_video *video = video_drvdata(file);
1430         struct video_device *vdev = &video->vdev;
1431         struct v4l2_subdev *subdev;
1432         struct v4l2_fh *vfh;
1433         int ret;
1434
1435         subdev = get_senname(file, (char *)__func__);
1436         if (!subdev)
1437                 return -EINVAL;
1438
1439         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1440         if (!vfh->ctrl_handler)
1441                 return -ENOTTY;
1442         ret = v4l2_try_ext_ctrls(vfh->ctrl_handler,
1443                                           vdev, subdev->v4l2_dev->mdev, ctrls);
1444
1445         return ret;
1446 }
1447
1448
1449 #ifdef UNUSED_CODE
1450 static int video_querymenu(struct file *file, void *fh,
1451                                struct v4l2_querymenu *qm)
1452 {
1453         struct v4l2_subdev *subdev;
1454         int ret;
1455
1456         subdev = get_senname(file, (char *)__func__);
1457         if (!subdev)
1458                 return -EINVAL;
1459
1460         ret = v4l2_querymenu(subdev->ctrl_handler, qm);
1461
1462         return ret;
1463 }
1464 #endif
1465
1466 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops = {
1467         .vidioc_querycap                = video_querycap,
1468         .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
1469         .vidioc_enum_framesizes         = video_enum_framesizes,
1470         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1471         .vidioc_g_fmt_vid_cap           = video_g_fmt,
1472         .vidioc_s_fmt_vid_cap           = video_s_fmt,
1473         .vidioc_try_fmt_vid_cap         = video_try_fmt,
1474         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1475         .vidioc_querybuf                = vb2_ioctl_querybuf,
1476         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1477         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1478         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1479         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1480         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1481         .vidioc_streamon                = vb2_ioctl_streamon,
1482         .vidioc_streamoff               = vb2_ioctl_streamoff,
1483         .vidioc_enum_input              = video_enum_input,
1484         .vidioc_g_input                 = video_g_input,
1485         .vidioc_s_input                 = video_s_input,
1486         .vidioc_g_parm                  = video_g_parm,
1487         .vidioc_s_parm                  = video_s_parm,
1488         .vidioc_s_selection             = video_s_selection,
1489         .vidioc_g_selection             = video_g_selection,
1490         .vidioc_g_ctrl                  = video_g_ctrl,
1491         .vidioc_s_ctrl                  = video_s_ctrl,
1492         .vidioc_g_ext_ctrls             = video_g_ext_ctrls,
1493         .vidioc_queryctrl               = video_queryctrl,
1494         .vidioc_s_ext_ctrls             = video_s_ext_ctrls,
1495         .vidioc_try_ext_ctrls           = video_try_ext_ctrls,
1496 };
1497
1498 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_mp = {
1499         .vidioc_querycap                = video_querycap,
1500         .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
1501         .vidioc_enum_framesizes         = video_enum_framesizes,
1502         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1503         .vidioc_g_fmt_vid_cap_mplane    = video_g_fmt_mp,
1504         .vidioc_s_fmt_vid_cap_mplane    = video_s_fmt_mp,
1505         .vidioc_try_fmt_vid_cap_mplane  = video_try_fmt_mp,
1506         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1507         .vidioc_querybuf                = vb2_ioctl_querybuf,
1508         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1509         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1510         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1511         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1512         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1513         .vidioc_streamon                = vb2_ioctl_streamon,
1514         .vidioc_streamoff               = vb2_ioctl_streamoff,
1515         .vidioc_enum_input              = video_enum_input,
1516         .vidioc_g_input                 = video_g_input,
1517         .vidioc_s_input                 = video_s_input,
1518         .vidioc_g_parm                  = video_g_parm,
1519         .vidioc_s_parm                  = video_s_parm,
1520         .vidioc_s_selection             = video_s_selection,
1521         .vidioc_g_selection             = video_g_selection,
1522         .vidioc_g_ctrl                  = video_g_ctrl,
1523         .vidioc_s_ctrl                  = video_s_ctrl,
1524         .vidioc_g_ext_ctrls             = video_g_ext_ctrls,
1525         .vidioc_queryctrl               = video_queryctrl,
1526         .vidioc_s_ext_ctrls             = video_s_ext_ctrls,
1527         .vidioc_try_ext_ctrls           = video_try_ext_ctrls,
1528 };
1529
1530 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_out = {
1531         .vidioc_querycap                = video_querycap,
1532         .vidioc_enum_fmt_vid_out        = video_enum_fmt,
1533         .vidioc_enum_framesizes         = video_enum_framesizes,
1534         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1535         .vidioc_g_fmt_vid_out           = video_g_fmt,
1536         .vidioc_s_fmt_vid_out           = video_s_fmt,
1537         .vidioc_try_fmt_vid_out         = video_try_fmt,
1538         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1539         .vidioc_querybuf                = vb2_ioctl_querybuf,
1540         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1541         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1542         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1543         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1544         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1545         .vidioc_streamon                = vb2_ioctl_streamon,
1546         .vidioc_streamoff               = vb2_ioctl_streamoff,
1547 };
1548
1549 static int video_open(struct file *file)
1550 {
1551         struct video_device *vdev = video_devdata(file);
1552         struct stfcamss_video *video = video_drvdata(file);
1553         struct v4l2_fh *vfh;
1554         int ret;
1555
1556         mutex_lock(&video->lock);
1557
1558         vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
1559         if (vfh == NULL) {
1560                 ret = -ENOMEM;
1561                 goto error_alloc;
1562         }
1563
1564         v4l2_fh_init(vfh, vdev);
1565         v4l2_fh_add(vfh);
1566
1567         file->private_data = vfh;
1568
1569 #ifdef USE_MEDIA_PIPELINE
1570         ret = v4l2_pipeline_pm_get(&vdev->entity);
1571         if (ret < 0) {
1572                 st_err(ST_VIDEO,
1573                         "Failed to power up pipeline: %d\n", ret);
1574                 goto error_pm_use;
1575         }
1576 #else
1577         struct media_entity *entity;
1578         struct media_pad *pad;
1579         struct v4l2_subdev *subdev;
1580         int i = 0;
1581
1582         entity = &vdev->entity;
1583         while (1) {
1584                 pad = &entity->pads[0];
1585                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1586                         break;
1587
1588                 pad = media_entity_remote_pad(pad);
1589                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1590                         break;
1591
1592                 entity = pad->entity;
1593                 subdev = media_entity_to_v4l2_subdev(entity);
1594
1595                 ret = v4l2_subdev_call(subdev, core, s_power, 1);
1596                 if (ret < 0 && ret != -ENOIOCTLCMD)
1597                         goto error_power;
1598                 i++;
1599         }
1600 #endif
1601         mutex_unlock(&video->lock);
1602
1603         return 0;
1604 #ifndef USE_MEDIA_PIPELINE
1605 error_power:
1606         entity = &vdev->entity;
1607         while (i--) {
1608                 pad = &entity->pads[0];
1609                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1610                         break;
1611
1612                 pad = media_entity_remote_pad(pad);
1613                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1614                         break;
1615
1616                 entity = pad->entity;
1617                 subdev = media_entity_to_v4l2_subdev(entity);
1618
1619                 v4l2_subdev_call(subdev, core, s_power, 0);
1620         }
1621 #endif
1622 error_pm_use:
1623         v4l2_fh_release(file);
1624 error_alloc:
1625         mutex_unlock(&video->lock);
1626         return ret;
1627 }
1628
1629 static int video_release(struct file *file)
1630 {
1631         struct video_device *vdev = video_devdata(file);
1632
1633         vb2_fop_release(file);
1634 #ifdef USE_MEDIA_PIPELINE
1635         v4l2_pipeline_pm_put(&vdev->entity);
1636 #else
1637         struct media_entity *entity;
1638         struct media_pad *pad;
1639         struct v4l2_subdev *subdev;
1640
1641         entity = &vdev->entity;
1642         while (1) {
1643                 pad = &entity->pads[0];
1644                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1645                         break;
1646
1647                 pad = media_entity_remote_pad(pad);
1648                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1649                         break;
1650
1651                 entity = pad->entity;
1652                 subdev = media_entity_to_v4l2_subdev(entity);
1653
1654                 v4l2_subdev_call(subdev, core, s_power, 0);
1655         }
1656 #endif
1657         file->private_data = NULL;
1658
1659         return 0;
1660 }
1661
1662 static const struct v4l2_file_operations stf_vid_fops = {
1663         .owner          = THIS_MODULE,
1664         .unlocked_ioctl = video_ioctl2,
1665         .open           = video_open,
1666         .release        = video_release,
1667         .poll           = vb2_fop_poll,
1668         .mmap           = vb2_fop_mmap,
1669         .read           = vb2_fop_read,
1670 };
1671
1672 static void stf_video_release(struct video_device *vdev)
1673 {
1674         struct stfcamss_video *video = video_get_drvdata(vdev);
1675
1676         media_entity_cleanup(&vdev->entity);
1677
1678         mutex_destroy(&video->q_lock);
1679         mutex_destroy(&video->lock);
1680 }
1681
1682 int stf_video_register(struct stfcamss_video *video,
1683                         struct v4l2_device *v4l2_dev,
1684                         const char *name, int is_mp)
1685 {
1686         struct video_device *vdev;
1687         struct vb2_queue *q;
1688         struct media_pad *pad = &video->pad;
1689         int ret;
1690         enum isp_pad_id isp_pad;
1691
1692         vdev = &video->vdev;
1693
1694         mutex_init(&video->q_lock);
1695
1696         q = &video->vb2_q;
1697         q->drv_priv = video;
1698         q->mem_ops = &vb2_dma_contig_memops;
1699         q->ops = &stf_video_vb2_q_ops;
1700         //q->type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
1701         //      V4L2_BUF_TYPE_VIDEO_CAPTURE;
1702         q->type = video->type;
1703         q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
1704         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1705         q->buf_struct_size = sizeof(struct stfcamss_buffer);
1706         q->dev = video->stfcamss->dev;
1707         q->lock = &video->q_lock;
1708         q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
1709         ret = vb2_queue_init(q);
1710         if (ret < 0) {
1711                 st_err(ST_VIDEO,
1712                         "Failed to init vb2 queue: %d\n", ret);
1713                 goto err_vb2_init;
1714         }
1715
1716         pad->flags = MEDIA_PAD_FL_SINK;
1717         ret = media_entity_pads_init(&vdev->entity, 1, pad);
1718         if (ret < 0) {
1719                 st_err(ST_VIDEO,
1720                         "Failed to init video entity: %d\n",
1721                         ret);
1722                 goto err_vb2_init;
1723         }
1724
1725         mutex_init(&video->lock);
1726
1727         isp_pad = stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC);
1728         if (video->id == VIN_LINE_WR) {
1729                 video->formats = formats_pix_st7110_wr;
1730                 video->nformats = ARRAY_SIZE(formats_pix_st7110_wr);
1731                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1732         } else if (isp_pad == STF_ISP_PAD_SRC
1733                 || isp_pad == STF_ISP_PAD_SRC_SS0
1734                 || isp_pad == STF_ISP_PAD_SRC_SS1) {
1735                 video->formats = formats_pix_st7110_isp;
1736                 video->nformats = ARRAY_SIZE(formats_pix_st7110_isp);
1737                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1738         } else if (isp_pad == STF_ISP_PAD_SRC_ITIW
1739                 || isp_pad == STF_ISP_PAD_SRC_ITIR) {
1740                 video->formats = formats_st7110_isp_iti;
1741                 video->nformats = ARRAY_SIZE(formats_st7110_isp_iti);
1742                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1743         } else { // raw/scdump/yhist
1744                 video->formats = formats_raw_st7110_isp;
1745                 video->nformats = ARRAY_SIZE(formats_raw_st7110_isp);
1746                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_128;
1747         }
1748         video->is_mp = is_mp;
1749
1750         ret = stf_video_init_format(video, is_mp);
1751         if (ret < 0) {
1752                 st_err(ST_VIDEO, "Failed to init format: %d\n", ret);
1753                 goto err_vid_init_format;
1754         }
1755
1756         vdev->fops = &stf_vid_fops;
1757         if (isp_pad == STF_ISP_PAD_SRC_ITIR) {
1758                 vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT;
1759                 vdev->vfl_dir = VFL_DIR_TX;
1760         } else {
1761                 vdev->device_caps = is_mp ? V4L2_CAP_VIDEO_CAPTURE_MPLANE :
1762                         V4L2_CAP_VIDEO_CAPTURE;
1763                 vdev->vfl_dir = VFL_DIR_RX;
1764         }
1765         vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
1766         if (video->type == V4L2_CAP_VIDEO_OUTPUT)
1767                 vdev->ioctl_ops = &stf_vid_ioctl_ops_out;
1768         else
1769                 vdev->ioctl_ops = is_mp ? &stf_vid_ioctl_ops_mp : &stf_vid_ioctl_ops;
1770         vdev->release = stf_video_release;
1771         vdev->v4l2_dev = v4l2_dev;
1772         vdev->queue = &video->vb2_q;
1773         vdev->lock = &video->lock;
1774         //strlcpy(vdev->name, name, sizeof(vdev->name));
1775         strscpy(vdev->name, name, sizeof(vdev->name));
1776
1777         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1778         if (ret < 0) {
1779                 st_err(ST_VIDEO,
1780                         "Failed to register video device: %d\n",
1781                         ret);
1782                 goto err_vid_reg;
1783         }
1784
1785         video_set_drvdata(vdev, video);
1786         return 0;
1787
1788 err_vid_reg:
1789 err_vid_init_format:
1790         media_entity_cleanup(&vdev->entity);
1791         mutex_destroy(&video->lock);
1792 err_vb2_init:
1793         mutex_destroy(&video->q_lock);
1794         return ret;
1795 }
1796
1797 void stf_video_unregister(struct stfcamss_video *video)
1798 {
1799         vb2_video_unregister_device(&video->vdev);
1800 }