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\n",
 876                         __func__, video->active_fmt.type);
 877         *f = video->active_fmt;
 878         return 0;
 879 }
 880
 881 static int video_g_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
 882 {
 883         struct stfcamss_video *video = video_drvdata(file);
 884
 885         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
 886         st_debug(ST_VIDEO, "%s, active_fmt.type = 0x%x\n",
 887                         __func__, video->active_fmt.type);
 888         *f = video->active_fmt;
 889         return 0;
 890 }
 891
 892 static int video_entity_s_fmt(struct stfcamss_video *video,
 893                         struct media_entity *entity,
 894                         struct v4l2_subdev_state *state,
 895                         struct v4l2_subdev_format *fmt, u32 dst_code)
 896 {
 897         struct v4l2_subdev *subdev;
 898         struct media_pad *pad;
 899         struct v4l2_mbus_framefmt *mf = &fmt->format;
 900         u32 width, height, code;
 901         int ret, index = 0;
 902
 903         code = mf->code;
 904         width = mf->width;
 905         height = mf->height;
 906         subdev = media_entity_to_v4l2_subdev(entity);
 907         while (1) {
 908                 if (index >= entity->num_pads)
 909                         break;
 910                 pad = &entity->pads[index];
 911                 pad = media_entity_remote_pad(pad);
 912                 if (pad && is_media_entity_v4l2_subdev(pad->entity)) {
 913                         fmt->pad = index;
 914                         if (index)
 915                                 mf->code = dst_code;
 916                         ret = v4l2_subdev_call(subdev, pad, set_fmt, state, fmt);
 917                         st_warn(ST_VIDEO,
 918                                 "\"%s\":%d pad fmt set to 0x%x %ux%u, dst_code = 0x%x\n",
 919                                 subdev->name, fmt->pad, mf->code,
 920                                 mf->width, mf->height, dst_code);
 921                         if (mf->code != code ||
 922                                 mf->width != width || mf->height != height) {
 923                                 st_warn(ST_VIDEO,
 924                                         "\"%s\":%d pad fmt has been"
 925                                         " changed to 0x%x %ux%u\n",
 926                                         subdev->name, fmt->pad, mf->code,
 927                                         mf->width, mf->height);
 928                         }
 929                         if (index)
 930                                 ret = video_entity_s_fmt(video, pad->entity, state, fmt, dst_code);
 931                 }
 932
 933                 if (ret < 0 && ret != -ENOIOCTLCMD)
 934                         break;
 935                 index++;
 936         }
 937         return ret;
 938 }
 939
 940 static int video_pipeline_s_fmt(struct stfcamss_video *video,
 941                         struct v4l2_subdev_state *state,
 942                         struct v4l2_format *f)
 943 {
 944         struct video_device *vdev = &video->vdev;
 945         struct media_entity *entity = &vdev->entity;
 946         struct v4l2_subdev *subdev;
 947         int ret, index;
 948         struct v4l2_subdev_format fmt = {
 949                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 950                 .reserved = {getcrop_pad_id(video->id)}
 951         };
 952         struct v4l2_mbus_framefmt *mf = &fmt.format;
 953         struct v4l2_pix_format *pix = &f->fmt.pix;
 954         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 955         struct media_entity *sensor;
 956         u32 width, height, code;
 957         struct media_pad *pad;
 958
 959         /* pix to mbus format */
 960         if (video->is_mp) {
 961                 index = video_find_format(mf->code,
 962                                         pix_mp->pixelformat,
 963                                         video->formats, video->nformats);
 964                 if (index < 0)
 965                         return index;
 966                 v4l2_fill_mbus_format_mplane(mf, pix_mp);
 967                 mf->code = video->formats[index].code;
 968         } else {
 969                 index = video_find_format(mf->code,
 970                                         pix->pixelformat,
 971                                         video->formats, video->nformats);
 972                 if (index < 0)
 973                         return index;
 974                 v4l2_fill_mbus_format(mf, pix, video->formats[index].code);
 975         }
 976         code = mf->code;
 977         width = mf->width;
 978         height = mf->height;
 979         sensor = stfcamss_find_sensor(entity);
 980         if (sensor) {
 981                 subdev = media_entity_to_v4l2_subdev(sensor);
 982                 ret = v4l2_subdev_call(subdev, pad, set_fmt, state, &fmt);
 983                 st_warn(ST_VIDEO,
 984                         "\"%s\":%d pad fmt set to 0x%x %ux%u\n",
 985                         subdev->name, fmt.pad, mf->code,
 986                         mf->width, mf->height);
 987                 if (mf->code != code ||
 988                         mf->width != width || mf->height != height) {
 989                         st_warn(ST_VIDEO,
 990                                 "\"%s\":%d pad fmt has been"
 991                                 " changed to 0x%x %ux%u\n",
 992                                 subdev->name, fmt.pad, mf->code,
 993                                 mf->width, mf->height);
 994                 }
 995         } else {
 996                 st_err(ST_VIDEO, "Can't find sensor\n");
 997                 return -EINVAL;
 998         }
 999         /*
1000          * Starting from sensor subdevice, walk within
1001          * pipeline and set format on each subdevice
1002          */
1003         sensor = stfcamss_find_sensor(entity);
1004         pad = media_entity_remote_pad(&sensor->pads[0]);
1005         ret = video_entity_s_fmt(video, pad->entity, state, &fmt, code);
1006
1007         if (ret < 0 && ret != -ENOIOCTLCMD) {
1008                 return ret;
1009         }
1010
1011         index = video_find_format(mf->code,
1012                                 video->formats[index].pixelformat,
1013                                 video->formats, video->nformats);
1014         if (index < 0)
1015                 return index;
1016
1017         if (video->is_mp)
1018                 video_mbus_to_pix_mp(mf, pix_mp,
1019                                 &video->formats[index], video->bpl_alignment);
1020         else
1021                 video_mbus_to_pix(mf, pix,
1022                                 &video->formats[index], video->bpl_alignment);
1023
1024         ret = __video_try_fmt(video, f, video->is_mp);
1025         if (ret < 0)
1026                 return ret;
1027
1028         return 0;
1029 }
1030
1031 static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
1032 {
1033         struct stfcamss_video *video = video_drvdata(file);
1034         int ret;
1035
1036         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
1037
1038         if (vb2_is_busy(&video->vb2_q))
1039                 return -EBUSY;
1040
1041         ret = __video_try_fmt(video, f, false);
1042         if (ret < 0)
1043                 return ret;
1044
1045         ret = video_pipeline_s_fmt(video, NULL, f);
1046         if (ret < 0)
1047                 return ret;
1048
1049         video->active_fmt = *f;
1050
1051         return 0;
1052 }
1053
1054 static int video_s_fmt_mp(struct file *file, void *fh, struct v4l2_format *f)
1055 {
1056         struct stfcamss_video *video = video_drvdata(file);
1057         int ret;
1058
1059         st_debug(ST_VIDEO, "%s, fmt.type = 0x%x\n", __func__, f->type);
1060         if (vb2_is_busy(&video->vb2_q))
1061                 return -EBUSY;
1062
1063         ret = __video_try_fmt(video, f, true);
1064         if (ret < 0)
1065                 return ret;
1066
1067         ret = video_pipeline_s_fmt(video, NULL, f);
1068         if (ret < 0)
1069                 return ret;
1070
1071         video->active_fmt = *f;
1072
1073         return 0;
1074 }
1075
1076 static int video_try_fmt(struct file *file,
1077                 void *fh, struct v4l2_format *f)
1078 {
1079         struct stfcamss_video *video = video_drvdata(file);
1080
1081         return __video_try_fmt(video, f, false);
1082 }
1083
1084 static int video_try_fmt_mp(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, true);
1090 }
1091
1092 static int video_enum_input(struct file *file, void *fh,
1093                         struct v4l2_input *input)
1094 {
1095         if (input->index > 0)
1096                 return -EINVAL;
1097
1098         strscpy(input->name, "camera", sizeof(input->name));
1099         input->type = V4L2_INPUT_TYPE_CAMERA;
1100
1101         return 0;
1102 }
1103
1104 static int video_g_input(struct file *file, void *fh, unsigned int *input)
1105 {
1106         *input = 0;
1107
1108         return 0;
1109 }
1110
1111 static int video_s_input(struct file *file, void *fh, unsigned int input)
1112 {
1113         return input == 0 ? 0 : -EINVAL;
1114 }
1115
1116 static int video_g_parm(struct file *file, void *priv,
1117                         struct v4l2_streamparm *p)
1118 {
1119         struct stfcamss_video *video = video_drvdata(file);
1120         struct video_device *vdev = &video->vdev;
1121         struct media_entity *entity;
1122         struct v4l2_subdev *subdev;
1123         struct media_pad *pad;
1124         int ret, is_support = 0;
1125
1126         entity = &vdev->entity;
1127         while (1) {
1128                 pad = &entity->pads[0];
1129                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1130                         break;
1131
1132                 pad = media_entity_remote_pad(pad);
1133                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1134                         break;
1135
1136                 entity = pad->entity;
1137                 subdev = media_entity_to_v4l2_subdev(entity);
1138
1139                 ret = v4l2_g_parm_cap(vdev, subdev, p);
1140                 if (ret < 0 && ret != -ENOIOCTLCMD)
1141                         break;
1142                 if (!ret)
1143                         is_support = 1;
1144         }
1145
1146         return is_support ? 0 : ret;
1147 }
1148
1149 static int video_s_parm(struct file *file, void *priv,
1150                         struct v4l2_streamparm *p)
1151 {
1152         struct stfcamss_video *video = video_drvdata(file);
1153         struct video_device *vdev = &video->vdev;
1154         struct media_entity *entity;
1155         struct v4l2_subdev *subdev;
1156         struct media_pad *pad;
1157         struct v4l2_streamparm tmp_p;
1158         int ret, is_support = 0;
1159
1160         entity = &vdev->entity;
1161         while (1) {
1162                 pad = &entity->pads[0];
1163                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1164                         break;
1165
1166                 pad = media_entity_remote_pad(pad);
1167                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1168                         break;
1169
1170                 entity = pad->entity;
1171                 subdev = media_entity_to_v4l2_subdev(entity);
1172
1173                 tmp_p = *p;
1174                 ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
1175                 if (ret < 0 && ret != -ENOIOCTLCMD)
1176                         break;
1177                 if (!ret) {
1178                         is_support = 1;
1179                         *p = tmp_p;
1180                 }
1181         }
1182
1183         return is_support ? 0 : ret;
1184 }
1185
1186 /* Crop ioctls */
1187 int video_g_pixelaspect(struct file *file, void *fh,
1188                             int buf_type, struct v4l2_fract *aspect)
1189 {
1190         return 0;
1191 }
1192
1193 int video_g_selection(struct file *file, void *fh,
1194                           struct v4l2_selection *s)
1195 {
1196         struct stfcamss_video *video = video_drvdata(file);
1197         struct video_device *vdev = &video->vdev;
1198         struct media_entity *entity;
1199         struct v4l2_subdev *subdev;
1200         struct media_pad *pad;
1201         struct v4l2_subdev_selection sel = {
1202                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1203                 .pad = getcrop_pad_id(video->id),
1204                 .target = s->target,
1205                 .r = s->r,
1206                 .flags = s->flags,
1207         };
1208         int ret;
1209
1210         st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
1211                         __func__, sel.target, s->target);
1212         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
1213                 && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1214                 return -EINVAL;
1215
1216         entity = &vdev->entity;
1217         while (1) {
1218                 pad = &entity->pads[0];
1219                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1220                         break;
1221
1222                 pad = media_entity_remote_pad(pad);
1223                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1224                         break;
1225
1226                 entity = pad->entity;
1227                 subdev = media_entity_to_v4l2_subdev(entity);
1228
1229                 ret = v4l2_subdev_call(subdev, pad, get_selection, NULL, &sel);
1230                 if (!ret) {
1231                         s->r = sel.r;
1232                         s->flags = sel.flags;
1233                         break;
1234                 }
1235                 if (ret != -ENOIOCTLCMD)
1236                         break;
1237         }
1238
1239         return ret;
1240 }
1241
1242 int video_s_selection(struct file *file, void *fh,
1243                         struct v4l2_selection *s)
1244 {
1245         struct stfcamss_video *video = video_drvdata(file);
1246         struct video_device *vdev = &video->vdev;
1247         struct media_entity *entity;
1248         struct v4l2_subdev *subdev;
1249         struct media_pad *pad;
1250         struct v4l2_subdev_selection sel = {
1251                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
1252                 .pad = getcrop_pad_id(video->id),
1253                 .target = s->target,
1254                 .r = s->r,
1255                 .flags = s->flags,
1256         };
1257         struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
1258         struct v4l2_pix_format_mplane *format_mp =
1259                                                 &video->active_fmt.fmt.pix_mp;
1260         int ret;
1261
1262         st_debug(ST_VIDEO, "%s, target = 0x%x, 0x%x\n",
1263                         __func__, sel.target, s->target);
1264         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE
1265                 && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1266                 return -EINVAL;
1267
1268         entity = &vdev->entity;
1269         while (1) {
1270                 pad = &entity->pads[0];
1271                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1272                         break;
1273
1274                 pad = media_entity_remote_pad(pad);
1275                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1276                         break;
1277
1278                 entity = pad->entity;
1279                 subdev = media_entity_to_v4l2_subdev(entity);
1280
1281                 ret = v4l2_subdev_call(subdev, pad, set_selection, NULL, &sel);
1282                 if (!ret) {
1283                         s->r = sel.r;
1284                         s->flags = sel.flags;
1285                         format->width = s->r.width;
1286                         format->height = s->r.height;
1287                         format_mp->width = s->r.width;
1288                         format_mp->height = s->r.height;
1289                         ret = __video_try_fmt(video, &video->active_fmt,
1290                                         video->is_mp);
1291                         if (ret < 0)
1292                                 return ret;
1293                         break;
1294                 }
1295                 if (ret != -ENOIOCTLCMD)
1296                         break;
1297         }
1298
1299         st_debug(ST_VIDEO, "ret = 0x%x, -EINVAL = 0x%x\n", ret, -EINVAL);
1300
1301         return ret;
1302 }
1303
1304 int video_g_ctrl(struct file *file, void *fh,
1305                                 struct v4l2_control *ctrls)
1306 {
1307         struct v4l2_subdev *subdev;
1308         int ret;
1309
1310         subdev = get_senname(file, (char *)__func__);
1311         if (!subdev)
1312                 return -EINVAL;
1313
1314         ret = v4l2_g_ctrl(subdev->ctrl_handler, ctrls);
1315
1316         return ret;
1317 }
1318
1319 static int video_s_ctrl(struct file *file, void *fh,
1320                                 struct v4l2_control *ctrl)
1321 {
1322         struct v4l2_subdev *subdev;
1323         struct v4l2_fh *vfh;
1324         int ret;
1325
1326         subdev = get_senname(file, (char *)__func__);
1327         if (!subdev)
1328                 return -EINVAL;
1329
1330         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1331         if (!vfh->ctrl_handler)
1332                 return -ENOTTY;
1333
1334         ret = v4l2_s_ctrl(vfh, subdev->ctrl_handler, ctrl);
1335
1336         return ret;
1337 }
1338
1339 #ifdef UNUSED_CODE
1340 static int video_query_ext_ctrl(struct file *file, void *fh,
1341                                     struct v4l2_query_ext_ctrl *qec)
1342 {
1343         struct v4l2_subdev *subdev;
1344         int ret;
1345
1346         subdev = get_senname(file, (char *)__func__);
1347         if (!subdev)
1348                 return -EINVAL;
1349
1350         ret = v4l2_query_ext_ctrl(subdev->ctrl_handler, qec);
1351
1352         return ret;
1353 }
1354 #endif
1355
1356 static int video_g_ext_ctrls(struct file *file, void *fh,
1357                                  struct v4l2_ext_controls *ctrls)
1358 {
1359         struct stfcamss_video *video = video_drvdata(file);
1360         struct video_device *vdev = &video->vdev;
1361         struct v4l2_subdev *subdev;
1362         int ret;
1363
1364         subdev = get_senname(file, (char *)__func__);
1365         if (!subdev)
1366                 return -EINVAL;
1367
1368         ret = v4l2_g_ext_ctrls(subdev->ctrl_handler,
1369                                                 vdev, subdev->v4l2_dev->mdev, ctrls);
1370
1371         return ret;
1372 }
1373
1374 static int video_queryctrl(struct file *file, void *fh,
1375                                struct v4l2_queryctrl *qc)
1376 {
1377         struct stfcamss_video *video = video_drvdata(file);
1378         struct video_device *vdev = &video->vdev;
1379         struct media_entity *entity = &vdev->entity;
1380         struct media_entity *sensor;
1381         struct v4l2_subdev *subdev;
1382         int ret = 0;
1383
1384         sensor = stfcamss_find_sensor(entity);
1385         if (sensor) {
1386                 subdev = media_entity_to_v4l2_subdev(sensor);
1387                 ret = v4l2_queryctrl(subdev->ctrl_handler, qc);
1388         } else {
1389         //      st_err(ST_VIDEO, "== [%s] Please configure pipeline first ==\n", __func__);
1390                 return -EINVAL;
1391         }
1392
1393         return ret;
1394 }
1395
1396 static int video_s_ext_ctrls(struct file *file, void *fh,
1397                                  struct v4l2_ext_controls *ctrls)
1398 {
1399         struct stfcamss_video *video = video_drvdata(file);
1400         struct video_device *vdev = &video->vdev;
1401         struct v4l2_subdev *subdev;
1402         struct v4l2_fh *vfh;
1403         int ret;
1404
1405         subdev = get_senname(file, (char *)__func__);
1406         if (!subdev)
1407                 return -EINVAL;
1408
1409         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1410         if (!vfh->ctrl_handler)
1411                 return -ENOTTY;
1412         ret = v4l2_s_ext_ctrls(vfh, subdev->ctrl_handler,
1413                                         vdev, subdev->v4l2_dev->mdev, ctrls);
1414
1415         return ret;
1416 }
1417
1418 static int video_try_ext_ctrls(struct file *file, void *fh,
1419                                    struct v4l2_ext_controls *ctrls)
1420 {
1421         struct stfcamss_video *video = video_drvdata(file);
1422         struct video_device *vdev = &video->vdev;
1423         struct v4l2_subdev *subdev;
1424         struct v4l2_fh *vfh;
1425         int ret;
1426
1427         subdev = get_senname(file, (char *)__func__);
1428         if (!subdev)
1429                 return -EINVAL;
1430
1431         vfh = container_of(&subdev->ctrl_handler, struct v4l2_fh, ctrl_handler);
1432         if (!vfh->ctrl_handler)
1433                 return -ENOTTY;
1434         ret = v4l2_try_ext_ctrls(vfh->ctrl_handler,
1435                                           vdev, subdev->v4l2_dev->mdev, ctrls);
1436
1437         return ret;
1438 }
1439
1440
1441 #ifdef UNUSED_CODE
1442 static int video_querymenu(struct file *file, void *fh,
1443                                struct v4l2_querymenu *qm)
1444 {
1445         struct v4l2_subdev *subdev;
1446         int ret;
1447
1448         subdev = get_senname(file, (char *)__func__);
1449         if (!subdev)
1450                 return -EINVAL;
1451
1452         ret = v4l2_querymenu(subdev->ctrl_handler, qm);
1453
1454         return ret;
1455 }
1456 #endif
1457
1458 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops = {
1459         .vidioc_querycap                = video_querycap,
1460         .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
1461         .vidioc_enum_framesizes         = video_enum_framesizes,
1462         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1463         .vidioc_g_fmt_vid_cap           = video_g_fmt,
1464         .vidioc_s_fmt_vid_cap           = video_s_fmt,
1465         .vidioc_try_fmt_vid_cap         = video_try_fmt,
1466         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1467         .vidioc_querybuf                = vb2_ioctl_querybuf,
1468         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1469         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1470         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1471         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1472         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1473         .vidioc_streamon                = vb2_ioctl_streamon,
1474         .vidioc_streamoff               = vb2_ioctl_streamoff,
1475         .vidioc_enum_input              = video_enum_input,
1476         .vidioc_g_input                 = video_g_input,
1477         .vidioc_s_input                 = video_s_input,
1478         .vidioc_g_parm                  = video_g_parm,
1479         .vidioc_s_parm                  = video_s_parm,
1480         .vidioc_s_selection             = video_s_selection,
1481         .vidioc_g_selection             = video_g_selection,
1482         .vidioc_g_ctrl                  = video_g_ctrl,
1483         .vidioc_s_ctrl                  = video_s_ctrl,
1484         .vidioc_g_ext_ctrls             = video_g_ext_ctrls,
1485         .vidioc_queryctrl               = video_queryctrl,
1486         .vidioc_s_ext_ctrls             = video_s_ext_ctrls,
1487         .vidioc_try_ext_ctrls           = video_try_ext_ctrls,
1488 };
1489
1490 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_mp = {
1491         .vidioc_querycap                = video_querycap,
1492         .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
1493         .vidioc_enum_framesizes         = video_enum_framesizes,
1494         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1495         .vidioc_g_fmt_vid_cap_mplane    = video_g_fmt_mp,
1496         .vidioc_s_fmt_vid_cap_mplane    = video_s_fmt_mp,
1497         .vidioc_try_fmt_vid_cap_mplane  = video_try_fmt_mp,
1498         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1499         .vidioc_querybuf                = vb2_ioctl_querybuf,
1500         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1501         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1502         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1503         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1504         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1505         .vidioc_streamon                = vb2_ioctl_streamon,
1506         .vidioc_streamoff               = vb2_ioctl_streamoff,
1507         .vidioc_enum_input              = video_enum_input,
1508         .vidioc_g_input                 = video_g_input,
1509         .vidioc_s_input                 = video_s_input,
1510         .vidioc_g_parm                  = video_g_parm,
1511         .vidioc_s_parm                  = video_s_parm,
1512         .vidioc_s_selection             = video_s_selection,
1513         .vidioc_g_selection             = video_g_selection,
1514         .vidioc_g_ctrl                  = video_g_ctrl,
1515         .vidioc_s_ctrl                  = video_s_ctrl,
1516         .vidioc_g_ext_ctrls             = video_g_ext_ctrls,
1517         .vidioc_queryctrl               = video_queryctrl,
1518         .vidioc_s_ext_ctrls             = video_s_ext_ctrls,
1519         .vidioc_try_ext_ctrls           = video_try_ext_ctrls,
1520 };
1521
1522 static const struct v4l2_ioctl_ops stf_vid_ioctl_ops_out = {
1523         .vidioc_querycap                = video_querycap,
1524         .vidioc_enum_fmt_vid_out        = video_enum_fmt,
1525         .vidioc_enum_framesizes         = video_enum_framesizes,
1526         .vidioc_enum_frameintervals     = video_enum_frameintervals,
1527         .vidioc_g_fmt_vid_out           = video_g_fmt,
1528         .vidioc_s_fmt_vid_out           = video_s_fmt,
1529         .vidioc_try_fmt_vid_out         = video_try_fmt,
1530         .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
1531         .vidioc_querybuf                = vb2_ioctl_querybuf,
1532         .vidioc_qbuf                    = vb2_ioctl_qbuf,
1533         .vidioc_expbuf                  = vb2_ioctl_expbuf,
1534         .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
1535         .vidioc_create_bufs             = vb2_ioctl_create_bufs,
1536         .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
1537         .vidioc_streamon                = vb2_ioctl_streamon,
1538         .vidioc_streamoff               = vb2_ioctl_streamoff,
1539 };
1540
1541 static int video_open(struct file *file)
1542 {
1543         struct video_device *vdev = video_devdata(file);
1544         struct stfcamss_video *video = video_drvdata(file);
1545         struct v4l2_fh *vfh;
1546         int ret;
1547
1548         mutex_lock(&video->lock);
1549
1550         vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
1551         if (vfh == NULL) {
1552                 ret = -ENOMEM;
1553                 goto error_alloc;
1554         }
1555
1556         v4l2_fh_init(vfh, vdev);
1557         v4l2_fh_add(vfh);
1558
1559         file->private_data = vfh;
1560
1561 #ifdef USE_MEDIA_PIPELINE
1562         ret = v4l2_pipeline_pm_get(&vdev->entity);
1563         if (ret < 0) {
1564                 st_err(ST_VIDEO,
1565                         "Failed to power up pipeline: %d\n", ret);
1566                 goto error_pm_use;
1567         }
1568 #else
1569         struct media_entity *entity;
1570         struct media_pad *pad;
1571         struct v4l2_subdev *subdev;
1572         int i = 0;
1573
1574         entity = &vdev->entity;
1575         while (1) {
1576                 pad = &entity->pads[0];
1577                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1578                         break;
1579
1580                 pad = media_entity_remote_pad(pad);
1581                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1582                         break;
1583
1584                 entity = pad->entity;
1585                 subdev = media_entity_to_v4l2_subdev(entity);
1586
1587                 ret = v4l2_subdev_call(subdev, core, s_power, 1);
1588                 if (ret < 0 && ret != -ENOIOCTLCMD)
1589                         goto error_power;
1590                 i++;
1591         }
1592 #endif
1593         mutex_unlock(&video->lock);
1594
1595         return 0;
1596 #ifndef USE_MEDIA_PIPELINE
1597 error_power:
1598         entity = &vdev->entity;
1599         while (i--) {
1600                 pad = &entity->pads[0];
1601                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1602                         break;
1603
1604                 pad = media_entity_remote_pad(pad);
1605                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1606                         break;
1607
1608                 entity = pad->entity;
1609                 subdev = media_entity_to_v4l2_subdev(entity);
1610
1611                 v4l2_subdev_call(subdev, core, s_power, 0);
1612         }
1613 #endif
1614 error_pm_use:
1615         v4l2_fh_release(file);
1616 error_alloc:
1617         mutex_unlock(&video->lock);
1618         return ret;
1619 }
1620
1621 static int video_release(struct file *file)
1622 {
1623         struct video_device *vdev = video_devdata(file);
1624
1625         vb2_fop_release(file);
1626 #ifdef USE_MEDIA_PIPELINE
1627         v4l2_pipeline_pm_put(&vdev->entity);
1628 #else
1629         struct media_entity *entity;
1630         struct media_pad *pad;
1631         struct v4l2_subdev *subdev;
1632
1633         entity = &vdev->entity;
1634         while (1) {
1635                 pad = &entity->pads[0];
1636                 if (!(pad->flags & MEDIA_PAD_FL_SINK))
1637                         break;
1638
1639                 pad = media_entity_remote_pad(pad);
1640                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
1641                         break;
1642
1643                 entity = pad->entity;
1644                 subdev = media_entity_to_v4l2_subdev(entity);
1645
1646                 v4l2_subdev_call(subdev, core, s_power, 0);
1647         }
1648 #endif
1649         file->private_data = NULL;
1650
1651         return 0;
1652 }
1653
1654 static const struct v4l2_file_operations stf_vid_fops = {
1655         .owner          = THIS_MODULE,
1656         .unlocked_ioctl = video_ioctl2,
1657         .open           = video_open,
1658         .release        = video_release,
1659         .poll           = vb2_fop_poll,
1660         .mmap           = vb2_fop_mmap,
1661         .read           = vb2_fop_read,
1662 };
1663
1664 static void stf_video_release(struct video_device *vdev)
1665 {
1666         struct stfcamss_video *video = video_get_drvdata(vdev);
1667
1668         media_entity_cleanup(&vdev->entity);
1669
1670         mutex_destroy(&video->q_lock);
1671         mutex_destroy(&video->lock);
1672 }
1673
1674 int stf_video_register(struct stfcamss_video *video,
1675                         struct v4l2_device *v4l2_dev,
1676                         const char *name, int is_mp)
1677 {
1678         struct video_device *vdev;
1679         struct vb2_queue *q;
1680         struct media_pad *pad = &video->pad;
1681         int ret;
1682         enum isp_pad_id isp_pad;
1683
1684         vdev = &video->vdev;
1685
1686         mutex_init(&video->q_lock);
1687
1688         q = &video->vb2_q;
1689         q->drv_priv = video;
1690         q->mem_ops = &vb2_dma_contig_memops;
1691         q->ops = &stf_video_vb2_q_ops;
1692         //q->type = is_mp ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
1693         //      V4L2_BUF_TYPE_VIDEO_CAPTURE;
1694         q->type = video->type;
1695         q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
1696         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1697         q->buf_struct_size = sizeof(struct stfcamss_buffer);
1698         q->dev = video->stfcamss->dev;
1699         q->lock = &video->q_lock;
1700         q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
1701         ret = vb2_queue_init(q);
1702         if (ret < 0) {
1703                 st_err(ST_VIDEO,
1704                         "Failed to init vb2 queue: %d\n", ret);
1705                 goto err_vb2_init;
1706         }
1707
1708         pad->flags = MEDIA_PAD_FL_SINK;
1709         ret = media_entity_pads_init(&vdev->entity, 1, pad);
1710         if (ret < 0) {
1711                 st_err(ST_VIDEO,
1712                         "Failed to init video entity: %d\n",
1713                         ret);
1714                 goto err_vb2_init;
1715         }
1716
1717         mutex_init(&video->lock);
1718
1719         isp_pad = stf_vin_map_isp_pad(video->id, STF_ISP_PAD_SRC);
1720         if (video->id == VIN_LINE_WR) {
1721                 video->formats = formats_pix_st7110_wr;
1722                 video->nformats = ARRAY_SIZE(formats_pix_st7110_wr);
1723                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1724         } else if (isp_pad == STF_ISP_PAD_SRC
1725                 || isp_pad == STF_ISP_PAD_SRC_SS0
1726                 || isp_pad == STF_ISP_PAD_SRC_SS1) {
1727                 video->formats = formats_pix_st7110_isp;
1728                 video->nformats = ARRAY_SIZE(formats_pix_st7110_isp);
1729                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1730         } else if (isp_pad == STF_ISP_PAD_SRC_ITIW
1731                 || isp_pad == STF_ISP_PAD_SRC_ITIR) {
1732                 video->formats = formats_st7110_isp_iti;
1733                 video->nformats = ARRAY_SIZE(formats_st7110_isp_iti);
1734                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
1735         } else { // raw/scdump/yhist
1736                 video->formats = formats_raw_st7110_isp;
1737                 video->nformats = ARRAY_SIZE(formats_raw_st7110_isp);
1738                 video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_128;
1739         }
1740         video->is_mp = is_mp;
1741
1742         ret = stf_video_init_format(video, is_mp);
1743         if (ret < 0) {
1744                 st_err(ST_VIDEO, "Failed to init format: %d\n", ret);
1745                 goto err_vid_init_format;
1746         }
1747
1748         vdev->fops = &stf_vid_fops;
1749         if (isp_pad == STF_ISP_PAD_SRC_ITIR) {
1750                 vdev->device_caps = V4L2_CAP_VIDEO_OUTPUT;
1751                 vdev->vfl_dir = VFL_DIR_TX;
1752         } else {
1753                 vdev->device_caps = is_mp ? V4L2_CAP_VIDEO_CAPTURE_MPLANE :
1754                         V4L2_CAP_VIDEO_CAPTURE;
1755                 vdev->vfl_dir = VFL_DIR_RX;
1756         }
1757         vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
1758         if (video->type == V4L2_CAP_VIDEO_OUTPUT)
1759                 vdev->ioctl_ops = &stf_vid_ioctl_ops_out;
1760         else
1761                 vdev->ioctl_ops = is_mp ? &stf_vid_ioctl_ops_mp : &stf_vid_ioctl_ops;
1762         vdev->release = stf_video_release;
1763         vdev->v4l2_dev = v4l2_dev;
1764         vdev->queue = &video->vb2_q;
1765         vdev->lock = &video->lock;
1766         //strlcpy(vdev->name, name, sizeof(vdev->name));
1767         strscpy(vdev->name, name, sizeof(vdev->name));
1768
1769         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
1770         if (ret < 0) {
1771                 st_err(ST_VIDEO,
1772                         "Failed to register video device: %d\n",
1773                         ret);
1774                 goto err_vid_reg;
1775         }
1776
1777         video_set_drvdata(vdev, video);
1778         return 0;
1779
1780 err_vid_reg:
1781 err_vid_init_format:
1782         media_entity_cleanup(&vdev->entity);
1783         mutex_destroy(&video->lock);
1784 err_vb2_init:
1785         mutex_destroy(&video->q_lock);
1786         return ret;
1787 }
1788
1789 void stf_video_unregister(struct stfcamss_video *video)
1790 {
1791         vb2_video_unregister_device(&video->vdev);
1792 }