target/linux/starfive/patches-6.1/0084-media-starfive-Add-video-driver.patch

   1 From 70a588f7daefe34697ccd04fa6ac3a6d4b63be88 Mon Sep 17 00:00:00 2001
   2 From: Jack Zhu <jack.zhu@starfivetech.com>
   3 Date: Fri, 12 May 2023 18:28:42 +0800
   4 Subject: [PATCH 084/122] media: starfive: Add video driver
   5
   6 Add video driver for StarFive Camera Subsystem.
   7
   8 Signed-off-by: Jack Zhu <jack.zhu@starfivetech.com>
   9 ---
  10  drivers/media/platform/starfive/Makefile    |   3 +-
  11  drivers/media/platform/starfive/stf_video.c | 864 ++++++++++++++++++++
  12  drivers/media/platform/starfive/stf_video.h |  95 +++
  13  3 files changed, 961 insertions(+), 1 deletion(-)
  14  create mode 100644 drivers/media/platform/starfive/stf_video.c
  15  create mode 100644 drivers/media/platform/starfive/stf_video.h
  16
  17 --- a/drivers/media/platform/starfive/Makefile
  18 +++ b/drivers/media/platform/starfive/Makefile
  19 @@ -4,6 +4,7 @@
  20  #
  21
  22  starfive-camss-objs += \
  23 -               stf_camss.o
  24 +               stf_camss.o \
  25 +               stf_video.o
  26
  27  obj-$(CONFIG_VIDEO_STARFIVE_CAMSS) += starfive-camss.o \
  28 --- /dev/null
  29 +++ b/drivers/media/platform/starfive/stf_video.c
  30 @@ -0,0 +1,864 @@
  31 +// SPDX-License-Identifier: GPL-2.0
  32 +/*
  33 + * stf_video.c
  34 + *
  35 + * StarFive Camera Subsystem - V4L2 device node
  36 + *
  37 + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
  38 + */
  39 +
  40 +#include <media/media-entity.h>
  41 +#include <media/v4l2-ctrls.h>
  42 +#include <media/v4l2-event.h>
  43 +#include <media/v4l2-mc.h>
  44 +#include <media/videobuf2-dma-contig.h>
  45 +
  46 +#include "stf_camss.h"
  47 +#include "stf_video.h"
  48 +
  49 +static const struct stfcamss_format_info formats_pix_wr[] = {
  50 +       { MEDIA_BUS_FMT_SRGGB10_1X10, V4L2_PIX_FMT_SRGGB10, 1,
  51 +         { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  52 +       { MEDIA_BUS_FMT_SGRBG10_1X10, V4L2_PIX_FMT_SGRBG10, 1,
  53 +         { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  54 +       { MEDIA_BUS_FMT_SGBRG10_1X10, V4L2_PIX_FMT_SGBRG10, 1,
  55 +         { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  56 +       { MEDIA_BUS_FMT_SBGGR10_1X10, V4L2_PIX_FMT_SBGGR10, 1,
  57 +         { { 1, 1 } }, { { 1, 1 } }, { 10 } },
  58 +};
  59 +
  60 +static const struct stfcamss_format_info formats_pix_isp[] = {
  61 +       { MEDIA_BUS_FMT_Y12_1X12, V4L2_PIX_FMT_NV12, 1,
  62 +         { { 1, 1 } }, { { 2, 3 } }, { 8 } },
  63 +};
  64 +
  65 +/* -----------------------------------------------------------------------------
  66 + * Helper functions
  67 + */
  68 +
  69 +static int video_find_format(u32 code, u32 pixelformat,
  70 +                            const struct stfcamss_format_info *formats,
  71 +                            unsigned int nformats)
  72 +{
  73 +       int i;
  74 +
  75 +       for (i = 0; i < nformats; i++) {
  76 +               if (formats[i].code == code &&
  77 +                   formats[i].pixelformat == pixelformat)
  78 +                       return i;
  79 +       }
  80 +
  81 +       for (i = 0; i < nformats; i++)
  82 +               if (formats[i].code == code)
  83 +                       return i;
  84 +
  85 +       for (i = 0; i < nformats; i++)
  86 +               if (formats[i].pixelformat == pixelformat)
  87 +                       return i;
  88 +
  89 +       return -EINVAL;
  90 +}
  91 +
  92 +static int __video_try_fmt(struct stfcamss_video *video, struct v4l2_format *f)
  93 +{
  94 +       struct v4l2_pix_format *pix;
  95 +       const struct stfcamss_format_info *fi;
  96 +       u32 width, height;
  97 +       u32 bpl;
  98 +       int i;
  99 +
 100 +       pix = &f->fmt.pix;
 101 +
 102 +       for (i = 0; i < video->nformats; i++)
 103 +               if (pix->pixelformat == video->formats[i].pixelformat)
 104 +                       break;
 105 +
 106 +       if (i == video->nformats)
 107 +               i = 0; /* default format */
 108 +
 109 +       fi = &video->formats[i];
 110 +       width = pix->width;
 111 +       height = pix->height;
 112 +
 113 +       memset(pix, 0, sizeof(*pix));
 114 +
 115 +       pix->pixelformat = fi->pixelformat;
 116 +       pix->width = clamp_t(u32, width, STFCAMSS_FRAME_MIN_WIDTH,
 117 +                            STFCAMSS_FRAME_MAX_WIDTH);
 118 +       pix->height = clamp_t(u32, height, STFCAMSS_FRAME_MIN_HEIGHT,
 119 +                             STFCAMSS_FRAME_MAX_HEIGHT);
 120 +       bpl = pix->width / fi->hsub[0].numerator *
 121 +             fi->hsub[0].denominator * fi->bpp[0] / 8;
 122 +       bpl = ALIGN(bpl, video->bpl_alignment);
 123 +       pix->bytesperline = bpl;
 124 +       pix->sizeimage = pix->height / fi->vsub[0].numerator *
 125 +                        fi->vsub[0].denominator * bpl;
 126 +
 127 +       pix->field = V4L2_FIELD_NONE;
 128 +       pix->colorspace = V4L2_COLORSPACE_SRGB;
 129 +       pix->flags = 0;
 130 +       pix->ycbcr_enc =
 131 +               V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace);
 132 +       pix->quantization = V4L2_MAP_QUANTIZATION_DEFAULT(true,
 133 +                                                         pix->colorspace,
 134 +                                                         pix->ycbcr_enc);
 135 +       pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace);
 136 +
 137 +       return 0;
 138 +}
 139 +
 140 +static int stf_video_init_format(struct stfcamss_video *video)
 141 +{
 142 +       int ret;
 143 +       struct v4l2_format format = {
 144 +               .type = video->type,
 145 +               .fmt.pix = {
 146 +                       .width = 1920,
 147 +                       .height = 1080,
 148 +                       .pixelformat = V4L2_PIX_FMT_RGB565,
 149 +               },
 150 +       };
 151 +
 152 +       ret = __video_try_fmt(video, &format);
 153 +
 154 +       if (ret < 0)
 155 +               return ret;
 156 +
 157 +       video->active_fmt = format;
 158 +
 159 +       return 0;
 160 +}
 161 +
 162 +/* -----------------------------------------------------------------------------
 163 + * Video queue operations
 164 + */
 165 +
 166 +static int video_queue_setup(struct vb2_queue *q,
 167 +                            unsigned int *num_buffers,
 168 +                            unsigned int *num_planes,
 169 +                            unsigned int sizes[],
 170 +                            struct device *alloc_devs[])
 171 +{
 172 +       struct stfcamss_video *video = vb2_get_drv_priv(q);
 173 +       const struct v4l2_pix_format *format = &video->active_fmt.fmt.pix;
 174 +
 175 +       if (*num_planes) {
 176 +               if (*num_planes != 1)
 177 +                       return -EINVAL;
 178 +
 179 +               if (sizes[0] < format->sizeimage)
 180 +                       return -EINVAL;
 181 +       }
 182 +
 183 +       *num_planes = 1;
 184 +       sizes[0] = format->sizeimage;
 185 +       if (!sizes[0])
 186 +               dev_err(video->stfcamss->dev,
 187 +                       "%s: error size is zero!!!\n", __func__);
 188 +
 189 +       dev_dbg(video->stfcamss->dev, "planes = %d, size = %d\n",
 190 +               *num_planes, sizes[0]);
 191 +
 192 +       return 0;
 193 +}
 194 +
 195 +static int video_buf_init(struct vb2_buffer *vb)
 196 +{
 197 +       struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 198 +       struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 199 +       struct stfcamss_buffer *buffer =
 200 +               container_of(vbuf, struct stfcamss_buffer, vb);
 201 +       const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
 202 +       dma_addr_t *paddr;
 203 +
 204 +       buffer->sizeimage = 0;
 205 +
 206 +       paddr = vb2_plane_cookie(vb, 0);
 207 +       buffer->sizeimage = vb2_plane_size(vb, 0);
 208 +       buffer->addr[0] = *paddr;
 209 +       if (fmt->pixelformat == V4L2_PIX_FMT_NV12 ||
 210 +           fmt->pixelformat == V4L2_PIX_FMT_NV21 ||
 211 +           fmt->pixelformat == V4L2_PIX_FMT_NV16 ||
 212 +           fmt->pixelformat == V4L2_PIX_FMT_NV61)
 213 +               buffer->addr[1] =
 214 +                       buffer->addr[0] + fmt->bytesperline * fmt->height;
 215 +
 216 +       return 0;
 217 +}
 218 +
 219 +static int video_buf_prepare(struct vb2_buffer *vb)
 220 +{
 221 +       struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 222 +       struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 223 +       const struct v4l2_pix_format *fmt = &video->active_fmt.fmt.pix;
 224 +
 225 +       if (fmt->sizeimage > vb2_plane_size(vb, 0)) {
 226 +               dev_err(video->stfcamss->dev,
 227 +                       "sizeimage = %d, plane size = %d\n",
 228 +                       fmt->sizeimage, (unsigned int)vb2_plane_size(vb, 0));
 229 +               return -EINVAL;
 230 +       }
 231 +       vb2_set_plane_payload(vb, 0, fmt->sizeimage);
 232 +
 233 +       vbuf->field = V4L2_FIELD_NONE;
 234 +
 235 +       return 0;
 236 +}
 237 +
 238 +static void video_buf_queue(struct vb2_buffer *vb)
 239 +{
 240 +       struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 241 +       struct stfcamss_video *video = vb2_get_drv_priv(vb->vb2_queue);
 242 +       struct stfcamss_buffer *buffer =
 243 +               container_of(vbuf, struct stfcamss_buffer, vb);
 244 +
 245 +       video->ops->queue_buffer(video, buffer);
 246 +}
 247 +
 248 +/*
 249 + * video_mbus_to_pix - Convert v4l2_mbus_framefmt to v4l2_pix_format
 250 + * @mbus: v4l2_mbus_framefmt format (input)
 251 + * @pix: v4l2_pix_format_mplane format (output)
 252 + * @f: a pointer to formats array element to be used for the conversion
 253 + * @alignment: bytesperline alignment value
 254 + *
 255 + * Fill the output pix structure with information from the input mbus format.
 256 + *
 257 + * Return 0 on success or a negative error code otherwise
 258 + */
 259 +static int video_mbus_to_pix(const struct v4l2_mbus_framefmt *mbus,
 260 +                            struct v4l2_pix_format *pix,
 261 +                            const struct stfcamss_format_info *f,
 262 +                            unsigned int alignment)
 263 +{
 264 +       u32 bytesperline;
 265 +
 266 +       memset(pix, 0, sizeof(*pix));
 267 +       v4l2_fill_pix_format(pix, mbus);
 268 +       pix->pixelformat = f->pixelformat;
 269 +       bytesperline = pix->width / f->hsub[0].numerator *
 270 +                      f->hsub[0].denominator * f->bpp[0] / 8;
 271 +       bytesperline = ALIGN(bytesperline, alignment);
 272 +       pix->bytesperline = bytesperline;
 273 +       pix->sizeimage = pix->height / f->vsub[0].numerator *
 274 +                        f->vsub[0].denominator * bytesperline;
 275 +       return 0;
 276 +}
 277 +
 278 +static struct v4l2_subdev *video_remote_subdev(struct stfcamss_video *video,
 279 +                                              u32 *pad)
 280 +{
 281 +       struct media_pad *remote;
 282 +
 283 +       remote = media_pad_remote_pad_first(&video->pad);
 284 +
 285 +       if (!remote || !is_media_entity_v4l2_subdev(remote->entity))
 286 +               return NULL;
 287 +
 288 +       if (pad)
 289 +               *pad = remote->index;
 290 +
 291 +       return media_entity_to_v4l2_subdev(remote->entity);
 292 +}
 293 +
 294 +static int video_get_subdev_format(struct stfcamss_video *video,
 295 +                                  struct v4l2_format *format)
 296 +{
 297 +       struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
 298 +       struct v4l2_subdev_format fmt;
 299 +       struct v4l2_subdev *subdev;
 300 +       u32 pixelformat;
 301 +       u32 pad;
 302 +       int ret;
 303 +
 304 +       subdev = video_remote_subdev(video, &pad);
 305 +       if (!subdev)
 306 +               return -EPIPE;
 307 +
 308 +       fmt.pad = pad;
 309 +       fmt.which = V4L2_SUBDEV_FORMAT_ACTIVE;
 310 +
 311 +       ret = v4l2_subdev_call(subdev, pad, get_fmt, NULL, &fmt);
 312 +       if (ret)
 313 +               return ret;
 314 +
 315 +       pixelformat = pix->pixelformat;
 316 +       ret = video_find_format(fmt.format.code, pixelformat,
 317 +                               video->formats, video->nformats);
 318 +       if (ret < 0)
 319 +               return ret;
 320 +
 321 +       format->type = video->type;
 322 +
 323 +       return video_mbus_to_pix(&fmt.format, &format->fmt.pix,
 324 +                                &video->formats[ret], video->bpl_alignment);
 325 +}
 326 +
 327 +static int video_check_format(struct stfcamss_video *video)
 328 +{
 329 +       struct v4l2_pix_format *pix = &video->active_fmt.fmt.pix;
 330 +       struct v4l2_format format;
 331 +       struct v4l2_pix_format *sd_pix = &format.fmt.pix;
 332 +       int ret;
 333 +
 334 +       sd_pix->pixelformat = pix->pixelformat;
 335 +       ret = video_get_subdev_format(video, &format);
 336 +       if (ret < 0)
 337 +               return ret;
 338 +
 339 +       if (pix->pixelformat != sd_pix->pixelformat ||
 340 +           pix->height > sd_pix->height ||
 341 +           pix->width > sd_pix->width ||
 342 +           pix->field != format.fmt.pix.field) {
 343 +               dev_err(video->stfcamss->dev,
 344 +                       "%s, not match:\n"
 345 +                       "0x%x 0x%x\n0x%x 0x%x\n0x%x 0x%x\n",
 346 +                       __func__,
 347 +                       pix->pixelformat, sd_pix->pixelformat,
 348 +                       pix->height, sd_pix->height,
 349 +                       pix->field, format.fmt.pix.field);
 350 +               return -EPIPE;
 351 +       }
 352 +
 353 +       return 0;
 354 +}
 355 +
 356 +static int video_start_streaming(struct vb2_queue *q, unsigned int count)
 357 +{
 358 +       struct stfcamss_video *video = vb2_get_drv_priv(q);
 359 +       struct video_device *vdev = &video->vdev;
 360 +       struct media_entity *entity;
 361 +       struct media_pad *pad;
 362 +       struct v4l2_subdev *subdev;
 363 +       int ret;
 364 +
 365 +       ret = video_device_pipeline_start(vdev, &video->stfcamss->pipe);
 366 +       if (ret < 0) {
 367 +               dev_err(video->stfcamss->dev,
 368 +                       "Failed to media_pipeline_start: %d\n", ret);
 369 +               return ret;
 370 +       }
 371 +
 372 +       ret = video_check_format(video);
 373 +       if (ret < 0)
 374 +               goto error;
 375 +       entity = &vdev->entity;
 376 +       while (1) {
 377 +               pad = &entity->pads[0];
 378 +               if (!(pad->flags & MEDIA_PAD_FL_SINK))
 379 +                       break;
 380 +
 381 +               pad = media_pad_remote_pad_first(pad);
 382 +               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 383 +                       break;
 384 +
 385 +               entity = pad->entity;
 386 +               subdev = media_entity_to_v4l2_subdev(entity);
 387 +
 388 +               ret = v4l2_subdev_call(subdev, video, s_stream, 1);
 389 +               if (ret < 0 && ret != -ENOIOCTLCMD)
 390 +                       goto error;
 391 +       }
 392 +       return 0;
 393 +
 394 +error:
 395 +       video_device_pipeline_stop(vdev);
 396 +       video->ops->flush_buffers(video, VB2_BUF_STATE_QUEUED);
 397 +       return ret;
 398 +}
 399 +
 400 +static void video_stop_streaming(struct vb2_queue *q)
 401 +{
 402 +       struct stfcamss_video *video = vb2_get_drv_priv(q);
 403 +       struct video_device *vdev = &video->vdev;
 404 +       struct media_entity *entity;
 405 +       struct media_pad *pad;
 406 +       struct v4l2_subdev *subdev;
 407 +
 408 +       entity = &vdev->entity;
 409 +       while (1) {
 410 +               pad = &entity->pads[0];
 411 +               if (!(pad->flags & MEDIA_PAD_FL_SINK))
 412 +                       break;
 413 +
 414 +               pad = media_pad_remote_pad_first(pad);
 415 +               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 416 +                       break;
 417 +
 418 +               entity = pad->entity;
 419 +               subdev = media_entity_to_v4l2_subdev(entity);
 420 +
 421 +               v4l2_subdev_call(subdev, video, s_stream, 0);
 422 +       }
 423 +
 424 +       video_device_pipeline_stop(vdev);
 425 +       video->ops->flush_buffers(video, VB2_BUF_STATE_ERROR);
 426 +}
 427 +
 428 +static const struct vb2_ops stf_video_vb2_q_ops = {
 429 +       .queue_setup     = video_queue_setup,
 430 +       .wait_prepare    = vb2_ops_wait_prepare,
 431 +       .wait_finish     = vb2_ops_wait_finish,
 432 +       .buf_init        = video_buf_init,
 433 +       .buf_prepare     = video_buf_prepare,
 434 +       .buf_queue       = video_buf_queue,
 435 +       .start_streaming = video_start_streaming,
 436 +       .stop_streaming  = video_stop_streaming,
 437 +};
 438 +
 439 +/* -----------------------------------------------------------------------------
 440 + * V4L2 ioctls
 441 + */
 442 +
 443 +static int video_querycap(struct file *file, void *fh,
 444 +                         struct v4l2_capability *cap)
 445 +{
 446 +       struct stfcamss_video *video = video_drvdata(file);
 447 +
 448 +       strscpy(cap->driver, "stf camss", sizeof(cap->driver));
 449 +       strscpy(cap->card, "Starfive Camera Subsystem", sizeof(cap->card));
 450 +       snprintf(cap->bus_info, sizeof(cap->bus_info), "platform:%s",
 451 +                dev_name(video->stfcamss->dev));
 452 +       return 0;
 453 +}
 454 +
 455 +static int video_get_pfmt_by_index(struct stfcamss_video *video, int ndx)
 456 +{
 457 +       int i, j, k;
 458 +
 459 +       /* find index "i" of "k"th unique pixelformat in formats array */
 460 +       k = -1;
 461 +       for (i = 0; i < video->nformats; i++) {
 462 +               for (j = 0; j < i; j++) {
 463 +                       if (video->formats[i].pixelformat ==
 464 +                           video->formats[j].pixelformat)
 465 +                               break;
 466 +               }
 467 +
 468 +               if (j == i)
 469 +                       k++;
 470 +
 471 +               if (k == ndx)
 472 +                       return i;
 473 +       }
 474 +
 475 +       return -EINVAL;
 476 +}
 477 +
 478 +static int video_get_pfmt_by_mcode(struct stfcamss_video *video, u32 mcode)
 479 +{
 480 +       int i;
 481 +
 482 +       for (i = 0; i < video->nformats; i++) {
 483 +               if (video->formats[i].code == mcode)
 484 +                       return i;
 485 +       }
 486 +
 487 +       return -EINVAL;
 488 +}
 489 +
 490 +static int video_enum_fmt(struct file *file, void *fh, struct v4l2_fmtdesc *f)
 491 +{
 492 +       struct stfcamss_video *video = video_drvdata(file);
 493 +       int i;
 494 +
 495 +       if (f->type != video->type)
 496 +               return -EINVAL;
 497 +       if (f->index >= video->nformats)
 498 +               return -EINVAL;
 499 +
 500 +       if (f->mbus_code) {
 501 +               /* Each entry in formats[] table has unique mbus_code */
 502 +               if (f->index > 0)
 503 +                       return -EINVAL;
 504 +
 505 +               i = video_get_pfmt_by_mcode(video, f->mbus_code);
 506 +       } else {
 507 +               i = video_get_pfmt_by_index(video, f->index);
 508 +       }
 509 +
 510 +       if (i < 0)
 511 +               return -EINVAL;
 512 +
 513 +       f->pixelformat = video->formats[i].pixelformat;
 514 +
 515 +       return 0;
 516 +}
 517 +
 518 +static int video_enum_framesizes(struct file *file, void *fh,
 519 +                                struct v4l2_frmsizeenum *fsize)
 520 +{
 521 +       struct stfcamss_video *video = video_drvdata(file);
 522 +       int i;
 523 +
 524 +       if (fsize->index)
 525 +               return -EINVAL;
 526 +
 527 +       for (i = 0; i < video->nformats; i++) {
 528 +               if (video->formats[i].pixelformat == fsize->pixel_format)
 529 +                       break;
 530 +       }
 531 +
 532 +       if (i == video->nformats)
 533 +               return -EINVAL;
 534 +
 535 +       fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 536 +       fsize->stepwise.min_width = STFCAMSS_FRAME_MIN_WIDTH;
 537 +       fsize->stepwise.max_width = STFCAMSS_FRAME_MAX_WIDTH;
 538 +       fsize->stepwise.min_height = STFCAMSS_FRAME_MIN_HEIGHT;
 539 +       fsize->stepwise.max_height = STFCAMSS_FRAME_MAX_HEIGHT;
 540 +       fsize->stepwise.step_width = 1;
 541 +       fsize->stepwise.step_height = 1;
 542 +
 543 +       return 0;
 544 +}
 545 +
 546 +static int video_g_fmt(struct file *file, void *fh, struct v4l2_format *f)
 547 +{
 548 +       struct stfcamss_video *video = video_drvdata(file);
 549 +
 550 +       *f = video->active_fmt;
 551 +
 552 +       return 0;
 553 +}
 554 +
 555 +static int video_s_fmt(struct file *file, void *fh, struct v4l2_format *f)
 556 +{
 557 +       struct stfcamss_video *video = video_drvdata(file);
 558 +       int ret;
 559 +
 560 +       if (vb2_is_busy(&video->vb2_q))
 561 +               return -EBUSY;
 562 +
 563 +       ret = __video_try_fmt(video, f);
 564 +       if (ret < 0)
 565 +               return ret;
 566 +
 567 +       video->active_fmt = *f;
 568 +
 569 +       return 0;
 570 +}
 571 +
 572 +static int video_try_fmt(struct file *file, void *fh, struct v4l2_format *f)
 573 +{
 574 +       struct stfcamss_video *video = video_drvdata(file);
 575 +
 576 +       return __video_try_fmt(video, f);
 577 +}
 578 +
 579 +static int video_enum_input(struct file *file, void *fh,
 580 +                           struct v4l2_input *input)
 581 +{
 582 +       if (input->index > 0)
 583 +               return -EINVAL;
 584 +
 585 +       strscpy(input->name, "camera", sizeof(input->name));
 586 +       input->type = V4L2_INPUT_TYPE_CAMERA;
 587 +
 588 +       return 0;
 589 +}
 590 +
 591 +static int video_g_input(struct file *file, void *fh, unsigned int *input)
 592 +{
 593 +       *input = 0;
 594 +
 595 +       return 0;
 596 +}
 597 +
 598 +static int video_s_input(struct file *file, void *fh, unsigned int input)
 599 +{
 600 +       return input == 0 ? 0 : -EINVAL;
 601 +}
 602 +
 603 +static int video_g_parm(struct file *file, void *priv,
 604 +                       struct v4l2_streamparm *p)
 605 +{
 606 +       struct stfcamss_video *video = video_drvdata(file);
 607 +       struct video_device *vdev = &video->vdev;
 608 +       struct media_entity *entity;
 609 +       struct v4l2_subdev *subdev;
 610 +       struct media_pad *pad;
 611 +       int ret, is_support = 0;
 612 +
 613 +       entity = &vdev->entity;
 614 +       while (1) {
 615 +               pad = &entity->pads[0];
 616 +               if (!(pad->flags & MEDIA_PAD_FL_SINK))
 617 +                       break;
 618 +
 619 +               pad = media_pad_remote_pad_first(pad);
 620 +               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 621 +                       break;
 622 +
 623 +               entity = pad->entity;
 624 +               subdev = media_entity_to_v4l2_subdev(entity);
 625 +
 626 +               ret = v4l2_g_parm_cap(vdev, subdev, p);
 627 +               if (ret < 0 && ret != -ENOIOCTLCMD)
 628 +                       break;
 629 +               if (!ret)
 630 +                       is_support = 1;
 631 +       }
 632 +
 633 +       return is_support ? 0 : ret;
 634 +}
 635 +
 636 +static int video_s_parm(struct file *file, void *priv,
 637 +                       struct v4l2_streamparm *p)
 638 +{
 639 +       struct stfcamss_video *video = video_drvdata(file);
 640 +       struct video_device *vdev = &video->vdev;
 641 +       struct media_entity *entity;
 642 +       struct v4l2_subdev *subdev;
 643 +       struct media_pad *pad;
 644 +       struct v4l2_streamparm tmp_p;
 645 +       int ret, is_support = 0;
 646 +
 647 +       entity = &vdev->entity;
 648 +       while (1) {
 649 +               pad = &entity->pads[0];
 650 +               if (!(pad->flags & MEDIA_PAD_FL_SINK))
 651 +                       break;
 652 +
 653 +               pad = media_pad_remote_pad_first(pad);
 654 +               if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 655 +                       break;
 656 +
 657 +               entity = pad->entity;
 658 +               subdev = media_entity_to_v4l2_subdev(entity);
 659 +
 660 +               tmp_p = *p;
 661 +               ret = v4l2_s_parm_cap(vdev, subdev, &tmp_p);
 662 +               if (ret < 0 && ret != -ENOIOCTLCMD)
 663 +                       break;
 664 +               if (!ret) {
 665 +                       is_support = 1;
 666 +                       *p = tmp_p;
 667 +               }
 668 +       }
 669 +
 670 +       return is_support ? 0 : ret;
 671 +}
 672 +
 673 +static const struct v4l2_ioctl_ops stf_vid_vin_ioctl_ops = {
 674 +       .vidioc_querycap                = video_querycap,
 675 +       .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
 676 +       .vidioc_enum_fmt_vid_out        = video_enum_fmt,
 677 +       .vidioc_enum_framesizes         = video_enum_framesizes,
 678 +       .vidioc_g_fmt_vid_cap           = video_g_fmt,
 679 +       .vidioc_s_fmt_vid_cap           = video_s_fmt,
 680 +       .vidioc_try_fmt_vid_cap         = video_try_fmt,
 681 +       .vidioc_g_fmt_vid_out           = video_g_fmt,
 682 +       .vidioc_s_fmt_vid_out           = video_s_fmt,
 683 +       .vidioc_try_fmt_vid_out         = video_try_fmt,
 684 +       .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 685 +       .vidioc_querybuf                = vb2_ioctl_querybuf,
 686 +       .vidioc_qbuf                    = vb2_ioctl_qbuf,
 687 +       .vidioc_expbuf                  = vb2_ioctl_expbuf,
 688 +       .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 689 +       .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 690 +       .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 691 +       .vidioc_streamon                = vb2_ioctl_streamon,
 692 +       .vidioc_streamoff               = vb2_ioctl_streamoff,
 693 +       .vidioc_enum_input              = video_enum_input,
 694 +       .vidioc_g_input                 = video_g_input,
 695 +       .vidioc_s_input                 = video_s_input,
 696 +       .vidioc_g_parm                  = video_g_parm,
 697 +       .vidioc_s_parm                  = video_s_parm,
 698 +};
 699 +
 700 +static const struct v4l2_ioctl_ops stf_vid_isp_ioctl_ops = {
 701 +       .vidioc_querycap                = video_querycap,
 702 +       .vidioc_enum_fmt_vid_cap        = video_enum_fmt,
 703 +       .vidioc_enum_fmt_vid_out        = video_enum_fmt,
 704 +       .vidioc_enum_framesizes         = video_enum_framesizes,
 705 +       .vidioc_g_fmt_vid_cap           = video_g_fmt,
 706 +       .vidioc_s_fmt_vid_cap           = video_s_fmt,
 707 +       .vidioc_try_fmt_vid_cap         = video_try_fmt,
 708 +       .vidioc_g_fmt_vid_out           = video_g_fmt,
 709 +       .vidioc_s_fmt_vid_out           = video_s_fmt,
 710 +       .vidioc_try_fmt_vid_out         = video_try_fmt,
 711 +       .vidioc_reqbufs                 = vb2_ioctl_reqbufs,
 712 +       .vidioc_querybuf                = vb2_ioctl_querybuf,
 713 +       .vidioc_qbuf                    = vb2_ioctl_qbuf,
 714 +       .vidioc_expbuf                  = vb2_ioctl_expbuf,
 715 +       .vidioc_dqbuf                   = vb2_ioctl_dqbuf,
 716 +       .vidioc_create_bufs             = vb2_ioctl_create_bufs,
 717 +       .vidioc_prepare_buf             = vb2_ioctl_prepare_buf,
 718 +       .vidioc_streamon                = vb2_ioctl_streamon,
 719 +       .vidioc_streamoff               = vb2_ioctl_streamoff,
 720 +       .vidioc_enum_input              = video_enum_input,
 721 +       .vidioc_g_input                 = video_g_input,
 722 +       .vidioc_s_input                 = video_s_input,
 723 +       .vidioc_g_parm                  = video_g_parm,
 724 +       .vidioc_s_parm                  = video_s_parm,
 725 +};
 726 +
 727 +/* -----------------------------------------------------------------------------
 728 + * V4L2 file operations
 729 + */
 730 +
 731 +static int video_open(struct file *file)
 732 +{
 733 +       struct video_device *vdev = video_devdata(file);
 734 +       struct stfcamss_video *video = video_drvdata(file);
 735 +       struct v4l2_fh *vfh;
 736 +       int ret;
 737 +
 738 +       mutex_lock(&video->lock);
 739 +
 740 +       vfh = kzalloc(sizeof(*vfh), GFP_KERNEL);
 741 +       if (!vfh) {
 742 +               ret = -ENOMEM;
 743 +               goto error_alloc;
 744 +       }
 745 +
 746 +       v4l2_fh_init(vfh, vdev);
 747 +       v4l2_fh_add(vfh);
 748 +
 749 +       file->private_data = vfh;
 750 +
 751 +       ret = v4l2_pipeline_pm_get(&vdev->entity);
 752 +       if (ret < 0) {
 753 +               dev_err(video->stfcamss->dev,
 754 +                       "Failed to power up pipeline: %d\n", ret);
 755 +               goto error_pm_use;
 756 +       }
 757 +       mutex_unlock(&video->lock);
 758 +
 759 +       return 0;
 760 +
 761 +error_pm_use:
 762 +       v4l2_fh_release(file);
 763 +error_alloc:
 764 +       mutex_unlock(&video->lock);
 765 +       return ret;
 766 +}
 767 +
 768 +static int video_release(struct file *file)
 769 +{
 770 +       struct video_device *vdev = video_devdata(file);
 771 +
 772 +       vb2_fop_release(file);
 773 +       v4l2_pipeline_pm_put(&vdev->entity);
 774 +       file->private_data = NULL;
 775 +
 776 +       return 0;
 777 +}
 778 +
 779 +static const struct v4l2_file_operations stf_vid_fops = {
 780 +       .owner          = THIS_MODULE,
 781 +       .unlocked_ioctl = video_ioctl2,
 782 +       .open           = video_open,
 783 +       .release        = video_release,
 784 +       .poll           = vb2_fop_poll,
 785 +       .mmap           = vb2_fop_mmap,
 786 +       .read           = vb2_fop_read,
 787 +};
 788 +
 789 +/* -----------------------------------------------------------------------------
 790 + * STFCAMSS video core
 791 + */
 792 +
 793 +static void stf_video_release(struct video_device *vdev)
 794 +{
 795 +       struct stfcamss_video *video = video_get_drvdata(vdev);
 796 +
 797 +       media_entity_cleanup(&vdev->entity);
 798 +
 799 +       mutex_destroy(&video->q_lock);
 800 +       mutex_destroy(&video->lock);
 801 +}
 802 +
 803 +int stf_video_register(struct stfcamss_video *video,
 804 +                      struct v4l2_device *v4l2_dev, const char *name)
 805 +{
 806 +       struct video_device *vdev;
 807 +       struct vb2_queue *q;
 808 +       struct media_pad *pad = &video->pad;
 809 +       int ret;
 810 +
 811 +       vdev = &video->vdev;
 812 +
 813 +       mutex_init(&video->q_lock);
 814 +
 815 +       q = &video->vb2_q;
 816 +       q->drv_priv = video;
 817 +       q->mem_ops = &vb2_dma_contig_memops;
 818 +       q->ops = &stf_video_vb2_q_ops;
 819 +       q->type = video->type;
 820 +       q->io_modes = VB2_DMABUF | VB2_MMAP | VB2_READ;
 821 +       q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 822 +       q->buf_struct_size = sizeof(struct stfcamss_buffer);
 823 +       q->dev = video->stfcamss->dev;
 824 +       q->lock = &video->q_lock;
 825 +       q->min_buffers_needed = STFCAMSS_MIN_BUFFERS;
 826 +       ret = vb2_queue_init(q);
 827 +       if (ret < 0) {
 828 +               dev_err(video->stfcamss->dev,
 829 +                       "Failed to init vb2 queue: %d\n", ret);
 830 +               goto err_vb2_init;
 831 +       }
 832 +
 833 +       pad->flags = MEDIA_PAD_FL_SINK;
 834 +       ret = media_entity_pads_init(&vdev->entity, 1, pad);
 835 +       if (ret < 0) {
 836 +               dev_err(video->stfcamss->dev,
 837 +                       "Failed to init video entity: %d\n", ret);
 838 +               goto err_vb2_init;
 839 +       }
 840 +
 841 +       mutex_init(&video->lock);
 842 +
 843 +       if (video->id == STF_V_LINE_WR) {
 844 +               video->formats = formats_pix_wr;
 845 +               video->nformats = ARRAY_SIZE(formats_pix_wr);
 846 +               video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
 847 +               vdev->ioctl_ops = &stf_vid_vin_ioctl_ops;
 848 +       } else {
 849 +               video->formats = formats_pix_isp;
 850 +               video->nformats = ARRAY_SIZE(formats_pix_isp);
 851 +               video->bpl_alignment = STFCAMSS_FRAME_WIDTH_ALIGN_8;
 852 +               vdev->ioctl_ops = &stf_vid_isp_ioctl_ops;
 853 +       }
 854 +
 855 +       ret = stf_video_init_format(video);
 856 +       if (ret < 0) {
 857 +               dev_err(video->stfcamss->dev,
 858 +                       "Failed to init format: %d\n", ret);
 859 +               goto err_vid_init_format;
 860 +       }
 861 +
 862 +       vdev->fops = &stf_vid_fops;
 863 +       vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE;
 864 +       vdev->vfl_dir = VFL_DIR_RX;
 865 +       vdev->device_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
 866 +       vdev->release = stf_video_release;
 867 +       vdev->v4l2_dev = v4l2_dev;
 868 +       vdev->queue = &video->vb2_q;
 869 +       vdev->lock = &video->lock;
 870 +       strscpy(vdev->name, name, sizeof(vdev->name));
 871 +
 872 +       ret = video_register_device(vdev, VFL_TYPE_VIDEO, video->id);
 873 +       if (ret < 0) {
 874 +               dev_err(video->stfcamss->dev,
 875 +                       "Failed to register video device: %d\n", ret);
 876 +               goto err_vid_reg;
 877 +       }
 878 +
 879 +       video_set_drvdata(vdev, video);
 880 +       return 0;
 881 +
 882 +err_vid_reg:
 883 +err_vid_init_format:
 884 +       media_entity_cleanup(&vdev->entity);
 885 +       mutex_destroy(&video->lock);
 886 +err_vb2_init:
 887 +       mutex_destroy(&video->q_lock);
 888 +       return ret;
 889 +}
 890 +
 891 +void stf_video_unregister(struct stfcamss_video *video)
 892 +{
 893 +       vb2_video_unregister_device(&video->vdev);
 894 +}
 895 --- /dev/null
 896 +++ b/drivers/media/platform/starfive/stf_video.h
 897 @@ -0,0 +1,95 @@
 898 +/* SPDX-License-Identifier: GPL-2.0 */
 899 +/*
 900 + * stf_video.h
 901 + *
 902 + * StarFive Camera Subsystem - V4L2 device node
 903 + *
 904 + * Copyright (C) 2021-2023 StarFive Technology Co., Ltd.
 905 + */
 906 +
 907 +#ifndef STF_VIDEO_H
 908 +#define STF_VIDEO_H
 909 +
 910 +#include <linux/mutex.h>
 911 +#include <linux/videodev2.h>
 912 +#include <media/v4l2-dev.h>
 913 +#include <media/v4l2-fh.h>
 914 +#include <media/v4l2-ioctl.h>
 915 +#include <media/videobuf2-v4l2.h>
 916 +
 917 +#define STFCAMSS_FRAME_MIN_WIDTH               64
 918 +#define STFCAMSS_FRAME_MAX_WIDTH               1920
 919 +#define STFCAMSS_FRAME_MIN_HEIGHT              64
 920 +#define STFCAMSS_FRAME_MAX_HEIGHT              1080
 921 +#define STFCAMSS_FRAME_WIDTH_ALIGN_8           8
 922 +#define STFCAMSS_FRAME_WIDTH_ALIGN_128         128
 923 +#define STFCAMSS_MIN_BUFFERS                   2
 924 +
 925 +#define STFCAMSS_MAX_ENTITY_NAME_LEN           27
 926 +
 927 +enum stf_v_line_id {
 928 +       STF_V_LINE_WR = 0,
 929 +       STF_V_LINE_ISP,
 930 +       STF_V_LINE_MAX,
 931 +};
 932 +
 933 +struct stfcamss_buffer {
 934 +       struct vb2_v4l2_buffer vb;
 935 +       dma_addr_t addr[3];
 936 +       struct list_head queue;
 937 +       int sizeimage;
 938 +};
 939 +
 940 +struct fract {
 941 +       u8 numerator;
 942 +       u8 denominator;
 943 +};
 944 +
 945 +/*
 946 + * struct stfcamss_format_info - ISP media bus format information
 947 + * @code: V4L2 media bus format code
 948 + * @pixelformat: V4L2 pixel format FCC identifier
 949 + * @planes: Number of planes
 950 + * @hsub: Horizontal subsampling (for each plane)
 951 + * @vsub: Vertical subsampling (for each plane)
 952 + * @bpp: Bits per pixel when stored in memory (for each plane)
 953 + */
 954 +struct stfcamss_format_info {
 955 +       u32 code;
 956 +       u32 pixelformat;
 957 +       u8 planes;
 958 +       struct fract hsub[3];
 959 +       struct fract vsub[3];
 960 +       u8 bpp[3];
 961 +};
 962 +
 963 +struct stfcamss_video {
 964 +       struct stfcamss *stfcamss;
 965 +       u8 id;
 966 +       struct vb2_queue vb2_q;
 967 +       struct video_device vdev;
 968 +       struct media_pad pad;
 969 +       struct media_pipeline pipe;
 970 +       struct v4l2_format active_fmt;
 971 +       enum v4l2_buf_type type;
 972 +       const struct stfcamss_video_ops *ops;
 973 +       struct mutex lock;       /* serialize device access */
 974 +       struct mutex q_lock;     /* protects the queue */
 975 +       unsigned int bpl_alignment;
 976 +       const struct stfcamss_format_info *formats;
 977 +       unsigned int nformats;
 978 +};
 979 +
 980 +struct stfcamss_video_ops {
 981 +       int (*queue_buffer)(struct stfcamss_video *vid,
 982 +                           struct stfcamss_buffer *buf);
 983 +       int (*flush_buffers)(struct stfcamss_video *vid,
 984 +                            enum vb2_buffer_state state);
 985 +};
 986 +
 987 +int stf_video_register(struct stfcamss_video *video,
 988 +                      struct v4l2_device *v4l2_dev, const char *name);
 989 +
 990 +void stf_video_unregister(struct stfcamss_video *video);
 991 +
 992 +#endif /* STF_VIDEO_H */