drivers/media/platform/st/stm32/stm32-dcmipp/dcmipp-bytecap.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Driver for STM32 Digital Camera Memory Interface Pixel Processor
   4  *
   5  * Copyright (C) STMicroelectronics SA 2023
   6  * Authors: Hugues Fruchet <[email protected]>
   7  *          Alain Volmat <[email protected]>
   8  *          for STMicroelectronics.
   9  */
  10
  11 #include <linux/iopoll.h>
  12 #include <linux/pm_runtime.h>
  13 #include <media/v4l2-ioctl.h>
  14 #include <media/v4l2-mc.h>
  15 #include <media/videobuf2-core.h>
  16 #include <media/videobuf2-dma-contig.h>
  17
  18 #include "dcmipp-common.h"
  19
  20 #define DCMIPP_PRSR             0x1f8
  21 #define DCMIPP_CMIER            0x3f0
  22 #define DCMIPP_CMIER_P0FRAMEIE  BIT(9)
  23 #define DCMIPP_CMIER_P0VSYNCIE  BIT(10)
  24 #define DCMIPP_CMIER_P0OVRIE    BIT(15)
  25 #define DCMIPP_CMIER_P0ALL      (DCMIPP_CMIER_P0VSYNCIE |\
  26                                  DCMIPP_CMIER_P0FRAMEIE |\
  27                                  DCMIPP_CMIER_P0OVRIE)
  28 #define DCMIPP_CMSR1            0x3f4
  29 #define DCMIPP_CMSR2            0x3f8
  30 #define DCMIPP_CMSR2_P0FRAMEF   BIT(9)
  31 #define DCMIPP_CMSR2_P0VSYNCF   BIT(10)
  32 #define DCMIPP_CMSR2_P0OVRF     BIT(15)
  33 #define DCMIPP_CMFCR            0x3fc
  34 #define DCMIPP_P0FSCR           0x404
  35 #define DCMIPP_P0FSCR_PIPEN     BIT(31)
  36 #define DCMIPP_P0FCTCR          0x500
  37 #define DCMIPP_P0FCTCR_CPTREQ   BIT(3)
  38 #define DCMIPP_P0DCCNTR         0x5b0
  39 #define DCMIPP_P0DCLMTR         0x5b4
  40 #define DCMIPP_P0DCLMTR_ENABLE  BIT(31)
  41 #define DCMIPP_P0DCLMTR_LIMIT_MASK      GENMASK(23, 0)
  42 #define DCMIPP_P0PPM0AR1        0x5c4
  43 #define DCMIPP_P0SR             0x5f8
  44 #define DCMIPP_P0SR_CPTACT      BIT(23)
  45
  46 struct dcmipp_bytecap_pix_map {
  47         unsigned int code;
  48         u32 pixelformat;
  49 };
  50
  51 #define PIXMAP_MBUS_PFMT(mbus, fmt)                     \
  52         {                                               \
  53                 .code = MEDIA_BUS_FMT_##mbus,           \
  54                 .pixelformat = V4L2_PIX_FMT_##fmt       \
  55         }
  56
  57 static const struct dcmipp_bytecap_pix_map dcmipp_bytecap_pix_map_list[] = {
  58         PIXMAP_MBUS_PFMT(RGB565_2X8_LE, RGB565),
  59         PIXMAP_MBUS_PFMT(YUYV8_2X8, YUYV),
  60         PIXMAP_MBUS_PFMT(YVYU8_2X8, YVYU),
  61         PIXMAP_MBUS_PFMT(UYVY8_2X8, UYVY),
  62         PIXMAP_MBUS_PFMT(VYUY8_2X8, VYUY),
  63         PIXMAP_MBUS_PFMT(Y8_1X8, GREY),
  64         PIXMAP_MBUS_PFMT(SBGGR8_1X8, SBGGR8),
  65         PIXMAP_MBUS_PFMT(SGBRG8_1X8, SGBRG8),
  66         PIXMAP_MBUS_PFMT(SGRBG8_1X8, SGRBG8),
  67         PIXMAP_MBUS_PFMT(SRGGB8_1X8, SRGGB8),
  68         PIXMAP_MBUS_PFMT(JPEG_1X8, JPEG),
  69 };
  70
  71 static const struct dcmipp_bytecap_pix_map *
  72 dcmipp_bytecap_pix_map_by_pixelformat(u32 pixelformat)
  73 {
  74         unsigned int i;
  75
  76         for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
  77                 if (dcmipp_bytecap_pix_map_list[i].pixelformat == pixelformat)
  78                         return &dcmipp_bytecap_pix_map_list[i];
  79         }
  80
  81         return NULL;
  82 }
  83
  84 struct dcmipp_buf {
  85         struct vb2_v4l2_buffer  vb;
  86         bool                    prepared;
  87         dma_addr_t              addr;
  88         size_t                  size;
  89         struct list_head        list;
  90 };
  91
  92 enum dcmipp_state {
  93         DCMIPP_STOPPED = 0,
  94         DCMIPP_WAIT_FOR_BUFFER,
  95         DCMIPP_RUNNING,
  96 };
  97
  98 struct dcmipp_bytecap_device {
  99         struct dcmipp_ent_device ved;
 100         struct video_device vdev;
 101         struct device *dev;
 102         struct v4l2_pix_format format;
 103         struct vb2_queue queue;
 104         struct list_head buffers;
 105         /*
 106          * Protects concurrent calls of buf queue / irq handler
 107          * and buffer handling related variables / lists
 108          */
 109         spinlock_t irqlock;
 110         /* mutex used as vdev and queue lock */
 111         struct mutex lock;
 112         u32 sequence;
 113         struct media_pipeline pipe;
 114         struct v4l2_subdev *s_subdev;
 115
 116         enum dcmipp_state state;
 117
 118         /*
 119          * DCMIPP driver is handling 2 buffers
 120          * active: buffer into which DCMIPP is currently writing into
 121          * next: buffer given to the DCMIPP and which will become
 122          *       automatically active on next VSYNC
 123          */
 124         struct dcmipp_buf *active, *next;
 125
 126         void __iomem *regs;
 127
 128         u32 cmier;
 129         u32 cmsr2;
 130
 131         struct {
 132                 u32 errors;
 133                 u32 limit;
 134                 u32 overrun;
 135                 u32 buffers;
 136                 u32 vsync;
 137                 u32 frame;
 138                 u32 it;
 139                 u32 underrun;
 140                 u32 nactive;
 141         } count;
 142 };
 143
 144 static const struct v4l2_pix_format fmt_default = {
 145         .width = DCMIPP_FMT_WIDTH_DEFAULT,
 146         .height = DCMIPP_FMT_HEIGHT_DEFAULT,
 147         .pixelformat = V4L2_PIX_FMT_RGB565,
 148         .field = V4L2_FIELD_NONE,
 149         .bytesperline = DCMIPP_FMT_WIDTH_DEFAULT * 2,
 150         .sizeimage = DCMIPP_FMT_WIDTH_DEFAULT * DCMIPP_FMT_HEIGHT_DEFAULT * 2,
 151         .colorspace = DCMIPP_COLORSPACE_DEFAULT,
 152         .ycbcr_enc = DCMIPP_YCBCR_ENC_DEFAULT,
 153         .quantization = DCMIPP_QUANTIZATION_DEFAULT,
 154         .xfer_func = DCMIPP_XFER_FUNC_DEFAULT,
 155 };
 156
 157 static int dcmipp_bytecap_querycap(struct file *file, void *priv,
 158                                    struct v4l2_capability *cap)
 159 {
 160         strscpy(cap->driver, DCMIPP_PDEV_NAME, sizeof(cap->driver));
 161         strscpy(cap->card, KBUILD_MODNAME, sizeof(cap->card));
 162
 163         return 0;
 164 }
 165
 166 static int dcmipp_bytecap_g_fmt_vid_cap(struct file *file, void *priv,
 167                                         struct v4l2_format *f)
 168 {
 169         struct dcmipp_bytecap_device *vcap = video_drvdata(file);
 170
 171         f->fmt.pix = vcap->format;
 172
 173         return 0;
 174 }
 175
 176 static int dcmipp_bytecap_try_fmt_vid_cap(struct file *file, void *priv,
 177                                           struct v4l2_format *f)
 178 {
 179         struct dcmipp_bytecap_device *vcap = video_drvdata(file);
 180         struct v4l2_pix_format *format = &f->fmt.pix;
 181         const struct dcmipp_bytecap_pix_map *vpix;
 182         u32 in_w, in_h;
 183
 184         /* Don't accept a pixelformat that is not on the table */
 185         vpix = dcmipp_bytecap_pix_map_by_pixelformat(format->pixelformat);
 186         if (!vpix)
 187                 format->pixelformat = fmt_default.pixelformat;
 188
 189         /* Adjust width & height */
 190         in_w = format->width;
 191         in_h = format->height;
 192         v4l_bound_align_image(&format->width, DCMIPP_FRAME_MIN_WIDTH,
 193                               DCMIPP_FRAME_MAX_WIDTH, 0, &format->height,
 194                               DCMIPP_FRAME_MIN_HEIGHT, DCMIPP_FRAME_MAX_HEIGHT,
 195                               0, 0);
 196         if (format->width != in_w || format->height != in_h)
 197                 dev_dbg(vcap->dev, "resolution updated: %dx%d -> %dx%d\n",
 198                         in_w, in_h, format->width, format->height);
 199
 200         if (format->pixelformat == V4L2_PIX_FMT_JPEG) {
 201                 format->bytesperline = format->width;
 202                 format->sizeimage = format->bytesperline * format->height;
 203         } else {
 204                 v4l2_fill_pixfmt(format, format->pixelformat,
 205                                  format->width, format->height);
 206         }
 207
 208         if (format->field == V4L2_FIELD_ANY)
 209                 format->field = fmt_default.field;
 210
 211         dcmipp_colorimetry_clamp(format);
 212
 213         return 0;
 214 }
 215
 216 static int dcmipp_bytecap_s_fmt_vid_cap(struct file *file, void *priv,
 217                                         struct v4l2_format *f)
 218 {
 219         struct dcmipp_bytecap_device *vcap = video_drvdata(file);
 220         int ret;
 221
 222         /* Do not change the format while stream is on */
 223         if (vb2_is_busy(&vcap->queue))
 224                 return -EBUSY;
 225
 226         ret = dcmipp_bytecap_try_fmt_vid_cap(file, priv, f);
 227         if (ret)
 228                 return ret;
 229
 230         dev_dbg(vcap->dev, "%s: format update: old:%ux%u (0x%p4cc, %u, %u, %u, %u) new:%ux%d (0x%p4cc, %u, %u, %u, %u)\n",
 231                 vcap->vdev.name,
 232                 /* old */
 233                 vcap->format.width, vcap->format.height,
 234                 &vcap->format.pixelformat, vcap->format.colorspace,
 235                 vcap->format.quantization, vcap->format.xfer_func,
 236                 vcap->format.ycbcr_enc,
 237                 /* new */
 238                 f->fmt.pix.width, f->fmt.pix.height,
 239                 &f->fmt.pix.pixelformat, f->fmt.pix.colorspace,
 240                 f->fmt.pix.quantization, f->fmt.pix.xfer_func,
 241                 f->fmt.pix.ycbcr_enc);
 242
 243         vcap->format = f->fmt.pix;
 244
 245         return 0;
 246 }
 247
 248 static int dcmipp_bytecap_enum_fmt_vid_cap(struct file *file, void *priv,
 249                                            struct v4l2_fmtdesc *f)
 250 {
 251         const struct dcmipp_bytecap_pix_map *vpix;
 252         unsigned int index = f->index;
 253         unsigned int i;
 254
 255         if (f->mbus_code) {
 256                 /*
 257                  * If a media bus code is specified, only enumerate formats
 258                  * compatible with it.
 259                  */
 260                 for (i = 0; i < ARRAY_SIZE(dcmipp_bytecap_pix_map_list); i++) {
 261                         vpix = &dcmipp_bytecap_pix_map_list[i];
 262                         if (vpix->code != f->mbus_code)
 263                                 continue;
 264
 265                         if (index == 0)
 266                                 break;
 267
 268                         index--;
 269                 }
 270
 271                 if (i == ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
 272                         return -EINVAL;
 273         } else {
 274                 /* Otherwise, enumerate all formats. */
 275                 if (f->index >= ARRAY_SIZE(dcmipp_bytecap_pix_map_list))
 276                         return -EINVAL;
 277
 278                 vpix = &dcmipp_bytecap_pix_map_list[f->index];
 279         }
 280
 281         f->pixelformat = vpix->pixelformat;
 282
 283         return 0;
 284 }
 285
 286 static int dcmipp_bytecap_enum_framesizes(struct file *file, void *fh,
 287                                           struct v4l2_frmsizeenum *fsize)
 288 {
 289         const struct dcmipp_bytecap_pix_map *vpix;
 290
 291         if (fsize->index)
 292                 return -EINVAL;
 293
 294         /* Only accept code in the pix map table */
 295         vpix = dcmipp_bytecap_pix_map_by_pixelformat(fsize->pixel_format);
 296         if (!vpix)
 297                 return -EINVAL;
 298
 299         fsize->type = V4L2_FRMSIZE_TYPE_CONTINUOUS;
 300         fsize->stepwise.min_width = DCMIPP_FRAME_MIN_WIDTH;
 301         fsize->stepwise.max_width = DCMIPP_FRAME_MAX_WIDTH;
 302         fsize->stepwise.min_height = DCMIPP_FRAME_MIN_HEIGHT;
 303         fsize->stepwise.max_height = DCMIPP_FRAME_MAX_HEIGHT;
 304         fsize->stepwise.step_width = 1;
 305         fsize->stepwise.step_height = 1;
 306
 307         return 0;
 308 }
 309
 310 static const struct v4l2_file_operations dcmipp_bytecap_fops = {
 311         .owner          = THIS_MODULE,
 312         .open           = v4l2_fh_open,
 313         .release        = vb2_fop_release,
 314         .read           = vb2_fop_read,
 315         .poll           = vb2_fop_poll,
 316         .unlocked_ioctl = video_ioctl2,
 317         .mmap           = vb2_fop_mmap,
 318 };
 319
 320 static const struct v4l2_ioctl_ops dcmipp_bytecap_ioctl_ops = {
 321         .vidioc_querycap = dcmipp_bytecap_querycap,
 322
 323         .vidioc_g_fmt_vid_cap = dcmipp_bytecap_g_fmt_vid_cap,
 324         .vidioc_s_fmt_vid_cap = dcmipp_bytecap_s_fmt_vid_cap,
 325         .vidioc_try_fmt_vid_cap = dcmipp_bytecap_try_fmt_vid_cap,
 326         .vidioc_enum_fmt_vid_cap = dcmipp_bytecap_enum_fmt_vid_cap,
 327         .vidioc_enum_framesizes = dcmipp_bytecap_enum_framesizes,
 328
 329         .vidioc_reqbufs = vb2_ioctl_reqbufs,
 330         .vidioc_create_bufs = vb2_ioctl_create_bufs,
 331         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
 332         .vidioc_querybuf = vb2_ioctl_querybuf,
 333         .vidioc_qbuf = vb2_ioctl_qbuf,
 334         .vidioc_dqbuf = vb2_ioctl_dqbuf,
 335         .vidioc_expbuf = vb2_ioctl_expbuf,
 336         .vidioc_streamon = vb2_ioctl_streamon,
 337         .vidioc_streamoff = vb2_ioctl_streamoff,
 338 };
 339
 340 static int dcmipp_pipeline_s_stream(struct dcmipp_bytecap_device *vcap,
 341                                     int state)
 342 {
 343         struct media_pad *pad;
 344         int ret;
 345
 346         /*
 347          * Get source subdev - since link is IMMUTABLE, pointer is cached
 348          * within the dcmipp_bytecap_device structure
 349          */
 350         if (!vcap->s_subdev) {
 351                 pad = media_pad_remote_pad_first(&vcap->vdev.entity.pads[0]);
 352                 if (!pad || !is_media_entity_v4l2_subdev(pad->entity))
 353                         return -EINVAL;
 354                 vcap->s_subdev = media_entity_to_v4l2_subdev(pad->entity);
 355         }
 356
 357         ret = v4l2_subdev_call(vcap->s_subdev, video, s_stream, state);
 358         if (ret < 0) {
 359                 dev_err(vcap->dev, "failed to %s streaming (%d)\n",
 360                         state ? "start" : "stop", ret);
 361                 return ret;
 362         }
 363
 364         return 0;
 365 }
 366
 367 static void dcmipp_start_capture(struct dcmipp_bytecap_device *vcap,
 368                                  struct dcmipp_buf *buf)
 369 {
 370         /* Set buffer address */
 371         reg_write(vcap, DCMIPP_P0PPM0AR1, buf->addr);
 372
 373         /* Set buffer size */
 374         reg_write(vcap, DCMIPP_P0DCLMTR, DCMIPP_P0DCLMTR_ENABLE |
 375                   ((buf->size / 4) & DCMIPP_P0DCLMTR_LIMIT_MASK));
 376
 377         /* Capture request */
 378         reg_set(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
 379 }
 380
 381 static void dcmipp_bytecap_all_buffers_done(struct dcmipp_bytecap_device *vcap,
 382                                             enum vb2_buffer_state state)
 383 {
 384         struct dcmipp_buf *buf, *node;
 385
 386         list_for_each_entry_safe(buf, node, &vcap->buffers, list) {
 387                 list_del_init(&buf->list);
 388                 vb2_buffer_done(&buf->vb.vb2_buf, state);
 389         }
 390 }
 391
 392 static int dcmipp_bytecap_start_streaming(struct vb2_queue *vq,
 393                                           unsigned int count)
 394 {
 395         struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
 396         struct media_entity *entity = &vcap->vdev.entity;
 397         struct dcmipp_buf *buf;
 398         int ret;
 399
 400         vcap->sequence = 0;
 401         memset(&vcap->count, 0, sizeof(vcap->count));
 402
 403         ret = pm_runtime_resume_and_get(vcap->dev);
 404         if (ret < 0) {
 405                 dev_err(vcap->dev, "%s: Failed to start streaming, cannot get sync (%d)\n",
 406                         __func__, ret);
 407                 goto err_buffer_done;
 408         }
 409
 410         ret = media_pipeline_start(entity->pads, &vcap->pipe);
 411         if (ret) {
 412                 dev_dbg(vcap->dev, "%s: Failed to start streaming, media pipeline start error (%d)\n",
 413                         __func__, ret);
 414                 goto err_pm_put;
 415         }
 416
 417         ret = dcmipp_pipeline_s_stream(vcap, 1);
 418         if (ret)
 419                 goto err_media_pipeline_stop;
 420
 421         spin_lock_irq(&vcap->irqlock);
 422
 423         /* Enable pipe at the end of programming */
 424         reg_set(vcap, DCMIPP_P0FSCR, DCMIPP_P0FSCR_PIPEN);
 425
 426         /*
 427          * vb2 framework guarantee that we have at least 'min_queued_buffers'
 428          * buffers in the list at this moment
 429          */
 430         vcap->next = list_first_entry(&vcap->buffers, typeof(*buf), list);
 431         dev_dbg(vcap->dev, "Start with next [%d] %p phy=%pad\n",
 432                 vcap->next->vb.vb2_buf.index, vcap->next, &vcap->next->addr);
 433
 434         dcmipp_start_capture(vcap, vcap->next);
 435
 436         /* Enable interruptions */
 437         vcap->cmier |= DCMIPP_CMIER_P0ALL;
 438         reg_set(vcap, DCMIPP_CMIER, vcap->cmier);
 439
 440         vcap->state = DCMIPP_RUNNING;
 441
 442         spin_unlock_irq(&vcap->irqlock);
 443
 444         return 0;
 445
 446 err_media_pipeline_stop:
 447         media_pipeline_stop(entity->pads);
 448 err_pm_put:
 449         pm_runtime_put(vcap->dev);
 450 err_buffer_done:
 451         spin_lock_irq(&vcap->irqlock);
 452         /*
 453          * Return all buffers to vb2 in QUEUED state.
 454          * This will give ownership back to userspace
 455          */
 456         dcmipp_bytecap_all_buffers_done(vcap, VB2_BUF_STATE_QUEUED);
 457         vcap->active = NULL;
 458         spin_unlock_irq(&vcap->irqlock);
 459
 460         return ret;
 461 }
 462
 463 static void dcmipp_dump_status(struct dcmipp_bytecap_device *vcap)
 464 {
 465         struct device *dev = vcap->dev;
 466
 467         dev_dbg(dev, "[DCMIPP_PRSR]  =%#10.8x\n", reg_read(vcap, DCMIPP_PRSR));
 468         dev_dbg(dev, "[DCMIPP_P0SR] =%#10.8x\n", reg_read(vcap, DCMIPP_P0SR));
 469         dev_dbg(dev, "[DCMIPP_P0DCCNTR]=%#10.8x\n",
 470                 reg_read(vcap, DCMIPP_P0DCCNTR));
 471         dev_dbg(dev, "[DCMIPP_CMSR1] =%#10.8x\n", reg_read(vcap, DCMIPP_CMSR1));
 472         dev_dbg(dev, "[DCMIPP_CMSR2] =%#10.8x\n", reg_read(vcap, DCMIPP_CMSR2));
 473 }
 474
 475 /*
 476  * Stop the stream engine. Any remaining buffers in the stream queue are
 477  * dequeued and passed on to the vb2 framework marked as STATE_ERROR.
 478  */
 479 static void dcmipp_bytecap_stop_streaming(struct vb2_queue *vq)
 480 {
 481         struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
 482         int ret;
 483         u32 status;
 484
 485         dcmipp_pipeline_s_stream(vcap, 0);
 486
 487         /* Stop the media pipeline */
 488         media_pipeline_stop(vcap->vdev.entity.pads);
 489
 490         /* Disable interruptions */
 491         reg_clear(vcap, DCMIPP_CMIER, vcap->cmier);
 492
 493         /* Stop capture */
 494         reg_clear(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
 495
 496         /* Wait until CPTACT become 0 */
 497         ret = readl_relaxed_poll_timeout(vcap->regs + DCMIPP_P0SR, status,
 498                                          !(status & DCMIPP_P0SR_CPTACT),
 499                                          20 * USEC_PER_MSEC,
 500                                          1000 * USEC_PER_MSEC);
 501         if (ret)
 502                 dev_warn(vcap->dev, "Timeout when stopping\n");
 503
 504         /* Disable pipe */
 505         reg_clear(vcap, DCMIPP_P0FSCR, DCMIPP_P0FSCR_PIPEN);
 506
 507         spin_lock_irq(&vcap->irqlock);
 508
 509         /* Return all queued buffers to vb2 in ERROR state */
 510         dcmipp_bytecap_all_buffers_done(vcap, VB2_BUF_STATE_ERROR);
 511         INIT_LIST_HEAD(&vcap->buffers);
 512
 513         vcap->active = NULL;
 514         vcap->state = DCMIPP_STOPPED;
 515
 516         spin_unlock_irq(&vcap->irqlock);
 517
 518         dcmipp_dump_status(vcap);
 519
 520         pm_runtime_put(vcap->dev);
 521
 522         if (vcap->count.errors)
 523                 dev_warn(vcap->dev, "Some errors found while streaming: errors=%d (overrun=%d, limit=%d, nactive=%d), underrun=%d, buffers=%d\n",
 524                          vcap->count.errors, vcap->count.overrun,
 525                          vcap->count.limit, vcap->count.nactive,
 526                          vcap->count.underrun, vcap->count.buffers);
 527 }
 528
 529 static int dcmipp_bytecap_buf_prepare(struct vb2_buffer *vb)
 530 {
 531         struct dcmipp_bytecap_device *vcap =  vb2_get_drv_priv(vb->vb2_queue);
 532         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 533         struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
 534         unsigned long size;
 535
 536         size = vcap->format.sizeimage;
 537
 538         if (vb2_plane_size(vb, 0) < size) {
 539                 dev_err(vcap->dev, "%s data will not fit into plane (%lu < %lu)\n",
 540                         __func__, vb2_plane_size(vb, 0), size);
 541                 return -EINVAL;
 542         }
 543
 544         vb2_set_plane_payload(vb, 0, size);
 545
 546         if (!buf->prepared) {
 547                 /* Get memory addresses */
 548                 buf->addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
 549                 buf->size = vb2_plane_size(&buf->vb.vb2_buf, 0);
 550                 buf->prepared = true;
 551
 552                 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, buf->size);
 553
 554                 dev_dbg(vcap->dev, "Setup [%d] phy=%pad size=%zu\n",
 555                         vb->index, &buf->addr, buf->size);
 556         }
 557
 558         return 0;
 559 }
 560
 561 static void dcmipp_bytecap_buf_queue(struct vb2_buffer *vb2_buf)
 562 {
 563         struct dcmipp_bytecap_device *vcap =
 564                 vb2_get_drv_priv(vb2_buf->vb2_queue);
 565         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb2_buf);
 566         struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
 567
 568         dev_dbg(vcap->dev, "Queue [%d] %p phy=%pad\n", buf->vb.vb2_buf.index,
 569                 buf, &buf->addr);
 570
 571         spin_lock_irq(&vcap->irqlock);
 572         list_add_tail(&buf->list, &vcap->buffers);
 573
 574         if (vcap->state == DCMIPP_WAIT_FOR_BUFFER) {
 575                 vcap->next = buf;
 576                 dev_dbg(vcap->dev, "Restart with next [%d] %p phy=%pad\n",
 577                         buf->vb.vb2_buf.index, buf, &buf->addr);
 578
 579                 dcmipp_start_capture(vcap, buf);
 580
 581                 vcap->state = DCMIPP_RUNNING;
 582         }
 583
 584         spin_unlock_irq(&vcap->irqlock);
 585 }
 586
 587 static int dcmipp_bytecap_queue_setup(struct vb2_queue *vq,
 588                                       unsigned int *nbuffers,
 589                                       unsigned int *nplanes,
 590                                       unsigned int sizes[],
 591                                       struct device *alloc_devs[])
 592 {
 593         struct dcmipp_bytecap_device *vcap = vb2_get_drv_priv(vq);
 594         unsigned int size;
 595
 596         size = vcap->format.sizeimage;
 597
 598         /* Make sure the image size is large enough */
 599         if (*nplanes)
 600                 return sizes[0] < vcap->format.sizeimage ? -EINVAL : 0;
 601
 602         *nplanes = 1;
 603         sizes[0] = vcap->format.sizeimage;
 604
 605         dev_dbg(vcap->dev, "Setup queue, count=%d, size=%d\n",
 606                 *nbuffers, size);
 607
 608         return 0;
 609 }
 610
 611 static int dcmipp_bytecap_buf_init(struct vb2_buffer *vb)
 612 {
 613         struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
 614         struct dcmipp_buf *buf = container_of(vbuf, struct dcmipp_buf, vb);
 615
 616         INIT_LIST_HEAD(&buf->list);
 617
 618         return 0;
 619 }
 620
 621 static const struct vb2_ops dcmipp_bytecap_qops = {
 622         .start_streaming        = dcmipp_bytecap_start_streaming,
 623         .stop_streaming         = dcmipp_bytecap_stop_streaming,
 624         .buf_init               = dcmipp_bytecap_buf_init,
 625         .buf_prepare            = dcmipp_bytecap_buf_prepare,
 626         .buf_queue              = dcmipp_bytecap_buf_queue,
 627         .queue_setup            = dcmipp_bytecap_queue_setup,
 628 };
 629
 630 static void dcmipp_bytecap_release(struct video_device *vdev)
 631 {
 632         struct dcmipp_bytecap_device *vcap =
 633                 container_of(vdev, struct dcmipp_bytecap_device, vdev);
 634
 635         dcmipp_pads_cleanup(vcap->ved.pads);
 636         mutex_destroy(&vcap->lock);
 637
 638         kfree(vcap);
 639 }
 640
 641 void dcmipp_bytecap_ent_release(struct dcmipp_ent_device *ved)
 642 {
 643         struct dcmipp_bytecap_device *vcap =
 644                 container_of(ved, struct dcmipp_bytecap_device, ved);
 645
 646         media_entity_cleanup(ved->ent);
 647         vb2_video_unregister_device(&vcap->vdev);
 648 }
 649
 650 static void dcmipp_buffer_done(struct dcmipp_bytecap_device *vcap,
 651                                struct dcmipp_buf *buf,
 652                                size_t bytesused,
 653                                int err)
 654 {
 655         struct vb2_v4l2_buffer *vbuf;
 656
 657         list_del_init(&buf->list);
 658
 659         vbuf = &buf->vb;
 660
 661         vbuf->sequence = vcap->sequence++;
 662         vbuf->field = V4L2_FIELD_NONE;
 663         vbuf->vb2_buf.timestamp = ktime_get_ns();
 664         vb2_set_plane_payload(&vbuf->vb2_buf, 0, bytesused);
 665         vb2_buffer_done(&vbuf->vb2_buf,
 666                         err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
 667         dev_dbg(vcap->dev, "Done  [%d] %p phy=%pad\n", buf->vb.vb2_buf.index,
 668                 buf, &buf->addr);
 669         vcap->count.buffers++;
 670 }
 671
 672 /* irqlock must be held */
 673 static void
 674 dcmipp_bytecap_set_next_frame_or_stop(struct dcmipp_bytecap_device *vcap)
 675 {
 676         if (!vcap->next && list_is_singular(&vcap->buffers)) {
 677                 /*
 678                  * If there is no available buffer (none or a single one in the
 679                  * list while two are expected), stop the capture (effective
 680                  * for next frame). On-going frame capture will continue until
 681                  * FRAME END but no further capture will be done.
 682                  */
 683                 reg_clear(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
 684
 685                 dev_dbg(vcap->dev, "Capture restart is deferred to next buffer queueing\n");
 686                 vcap->next = NULL;
 687                 vcap->state = DCMIPP_WAIT_FOR_BUFFER;
 688                 return;
 689         }
 690
 691         /* If we don't have buffer yet, pick the one after active */
 692         if (!vcap->next)
 693                 vcap->next = list_next_entry(vcap->active, list);
 694
 695         /*
 696          * Set buffer address
 697          * This register is shadowed and will be taken into
 698          * account on next VSYNC (start of next frame)
 699          */
 700         reg_write(vcap, DCMIPP_P0PPM0AR1, vcap->next->addr);
 701         dev_dbg(vcap->dev, "Write [%d] %p phy=%pad\n",
 702                 vcap->next->vb.vb2_buf.index, vcap->next, &vcap->next->addr);
 703 }
 704
 705 /* irqlock must be held */
 706 static void dcmipp_bytecap_process_frame(struct dcmipp_bytecap_device *vcap,
 707                                          size_t bytesused)
 708 {
 709         int err = 0;
 710         struct dcmipp_buf *buf = vcap->active;
 711
 712         if (!buf) {
 713                 vcap->count.nactive++;
 714                 vcap->count.errors++;
 715                 return;
 716         }
 717
 718         if (bytesused > buf->size) {
 719                 dev_dbg(vcap->dev, "frame larger than expected (%zu > %zu)\n",
 720                         bytesused, buf->size);
 721                 /* Clip to buffer size and return buffer to V4L2 in error */
 722                 bytesused = buf->size;
 723                 vcap->count.limit++;
 724                 vcap->count.errors++;
 725                 err = -EOVERFLOW;
 726         }
 727
 728         dcmipp_buffer_done(vcap, buf, bytesused, err);
 729         vcap->active = NULL;
 730 }
 731
 732 static irqreturn_t dcmipp_bytecap_irq_thread(int irq, void *arg)
 733 {
 734         struct dcmipp_bytecap_device *vcap =
 735                         container_of(arg, struct dcmipp_bytecap_device, ved);
 736         size_t bytesused = 0;
 737         u32 cmsr2;
 738
 739         spin_lock_irq(&vcap->irqlock);
 740
 741         cmsr2 = vcap->cmsr2 & vcap->cmier;
 742
 743         /*
 744          * If we have an overrun, a frame-end will probably not be generated,
 745          * in that case the active buffer will be recycled as next buffer by
 746          * the VSYNC handler
 747          */
 748         if (cmsr2 & DCMIPP_CMSR2_P0OVRF) {
 749                 vcap->count.errors++;
 750                 vcap->count.overrun++;
 751         }
 752
 753         if (cmsr2 & DCMIPP_CMSR2_P0FRAMEF) {
 754                 vcap->count.frame++;
 755
 756                 /* Read captured buffer size */
 757                 bytesused = reg_read(vcap, DCMIPP_P0DCCNTR);
 758                 dcmipp_bytecap_process_frame(vcap, bytesused);
 759         }
 760
 761         if (cmsr2 & DCMIPP_CMSR2_P0VSYNCF) {
 762                 vcap->count.vsync++;
 763                 if (vcap->state == DCMIPP_WAIT_FOR_BUFFER) {
 764                         vcap->count.underrun++;
 765                         goto out;
 766                 }
 767
 768                 /*
 769                  * On VSYNC, the previously set next buffer is going to become
 770                  * active thanks to the shadowing mechanism of the DCMIPP. In
 771                  * most of the cases, since a FRAMEEND has already come,
 772                  * pointer next is NULL since active is reset during the
 773                  * FRAMEEND handling. However, in case of framerate adjustment,
 774                  * there are more VSYNC than FRAMEEND. Thus we recycle the
 775                  * active (but not used) buffer and put it back into next.
 776                  */
 777                 swap(vcap->active, vcap->next);
 778                 dcmipp_bytecap_set_next_frame_or_stop(vcap);
 779         }
 780
 781 out:
 782         spin_unlock_irq(&vcap->irqlock);
 783         return IRQ_HANDLED;
 784 }
 785
 786 static irqreturn_t dcmipp_bytecap_irq_callback(int irq, void *arg)
 787 {
 788         struct dcmipp_bytecap_device *vcap =
 789                         container_of(arg, struct dcmipp_bytecap_device, ved);
 790
 791         /* Store interrupt status register */
 792         vcap->cmsr2 = reg_read(vcap, DCMIPP_CMSR2) & vcap->cmier;
 793         vcap->count.it++;
 794
 795         /* Clear interrupt */
 796         reg_write(vcap, DCMIPP_CMFCR, vcap->cmsr2);
 797
 798         return IRQ_WAKE_THREAD;
 799 }
 800
 801 static int dcmipp_bytecap_link_validate(struct media_link *link)
 802 {
 803         struct media_entity *entity = link->sink->entity;
 804         struct video_device *vd = media_entity_to_video_device(entity);
 805         struct dcmipp_bytecap_device *vcap = container_of(vd,
 806                                         struct dcmipp_bytecap_device, vdev);
 807         struct v4l2_subdev *source_sd =
 808                 media_entity_to_v4l2_subdev(link->source->entity);
 809         struct v4l2_subdev_format source_fmt = {
 810                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 811                 .pad = link->source->index,
 812         };
 813         const struct dcmipp_bytecap_pix_map *vpix;
 814         int ret;
 815
 816         ret = v4l2_subdev_call(source_sd, pad, get_fmt, NULL, &source_fmt);
 817         if (ret < 0)
 818                 return 0;
 819
 820         if (source_fmt.format.width != vcap->format.width ||
 821             source_fmt.format.height != vcap->format.height) {
 822                 dev_err(vcap->dev, "Wrong width or height %ux%u (%ux%u expected)\n",
 823                         vcap->format.width, vcap->format.height,
 824                         source_fmt.format.width, source_fmt.format.height);
 825                 return -EINVAL;
 826         }
 827
 828         vpix = dcmipp_bytecap_pix_map_by_pixelformat(vcap->format.pixelformat);
 829         if (source_fmt.format.code != vpix->code) {
 830                 dev_err(vcap->dev, "Wrong mbus_code 0x%x, (0x%x expected)\n",
 831                         vpix->code, source_fmt.format.code);
 832                 return -EINVAL;
 833         }
 834
 835         return 0;
 836 }
 837
 838 static const struct media_entity_operations dcmipp_bytecap_entity_ops = {
 839         .link_validate = dcmipp_bytecap_link_validate,
 840 };
 841
 842 struct dcmipp_ent_device *dcmipp_bytecap_ent_init(struct device *dev,
 843                                                   const char *entity_name,
 844                                                   struct v4l2_device *v4l2_dev,
 845                                                   void __iomem *regs)
 846 {
 847         struct dcmipp_bytecap_device *vcap;
 848         struct video_device *vdev;
 849         struct vb2_queue *q;
 850         const unsigned long pad_flag = MEDIA_PAD_FL_SINK;
 851         int ret = 0;
 852
 853         /* Allocate the dcmipp_bytecap_device struct */
 854         vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
 855         if (!vcap)
 856                 return ERR_PTR(-ENOMEM);
 857
 858         /* Allocate the pads */
 859         vcap->ved.pads = dcmipp_pads_init(1, &pad_flag);
 860         if (IS_ERR(vcap->ved.pads)) {
 861                 ret = PTR_ERR(vcap->ved.pads);
 862                 goto err_free_vcap;
 863         }
 864
 865         /* Initialize the media entity */
 866         vcap->vdev.entity.name = entity_name;
 867         vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
 868         vcap->vdev.entity.ops = &dcmipp_bytecap_entity_ops;
 869         ret = media_entity_pads_init(&vcap->vdev.entity, 1, vcap->ved.pads);
 870         if (ret)
 871                 goto err_clean_pads;
 872
 873         /* Initialize the lock */
 874         mutex_init(&vcap->lock);
 875
 876         /* Initialize the vb2 queue */
 877         q = &vcap->queue;
 878         q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 879         q->io_modes = VB2_MMAP | VB2_DMABUF;
 880         q->lock = &vcap->lock;
 881         q->drv_priv = vcap;
 882         q->buf_struct_size = sizeof(struct dcmipp_buf);
 883         q->ops = &dcmipp_bytecap_qops;
 884         q->mem_ops = &vb2_dma_contig_memops;
 885         q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 886         q->min_queued_buffers = 1;
 887         q->dev = dev;
 888
 889         /* DCMIPP requires 16 bytes aligned buffers */
 890         ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32) & ~0x0f);
 891         if (ret) {
 892                 dev_err(dev, "Failed to set DMA mask\n");
 893                 goto err_mutex_destroy;
 894         }
 895
 896         ret = vb2_queue_init(q);
 897         if (ret) {
 898                 dev_err(dev, "%s: vb2 queue init failed (err=%d)\n",
 899                         entity_name, ret);
 900                 goto err_clean_m_ent;
 901         }
 902
 903         /* Initialize buffer list and its lock */
 904         INIT_LIST_HEAD(&vcap->buffers);
 905         spin_lock_init(&vcap->irqlock);
 906
 907         /* Set default frame format */
 908         vcap->format = fmt_default;
 909
 910         /* Fill the dcmipp_ent_device struct */
 911         vcap->ved.ent = &vcap->vdev.entity;
 912         vcap->ved.handler = dcmipp_bytecap_irq_callback;
 913         vcap->ved.thread_fn = dcmipp_bytecap_irq_thread;
 914         vcap->dev = dev;
 915         vcap->regs = regs;
 916
 917         /* Initialize the video_device struct */
 918         vdev = &vcap->vdev;
 919         vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
 920                             V4L2_CAP_IO_MC;
 921         vdev->release = dcmipp_bytecap_release;
 922         vdev->fops = &dcmipp_bytecap_fops;
 923         vdev->ioctl_ops = &dcmipp_bytecap_ioctl_ops;
 924         vdev->lock = &vcap->lock;
 925         vdev->queue = q;
 926         vdev->v4l2_dev = v4l2_dev;
 927         strscpy(vdev->name, entity_name, sizeof(vdev->name));
 928         video_set_drvdata(vdev, &vcap->ved);
 929
 930         /* Register the video_device with the v4l2 and the media framework */
 931         ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
 932         if (ret) {
 933                 dev_err(dev, "%s: video register failed (err=%d)\n",
 934                         vcap->vdev.name, ret);
 935                 goto err_clean_m_ent;
 936         }
 937
 938         return &vcap->ved;
 939
 940 err_clean_m_ent:
 941         media_entity_cleanup(&vcap->vdev.entity);
 942 err_mutex_destroy:
 943         mutex_destroy(&vcap->lock);
 944 err_clean_pads:
 945         dcmipp_pads_cleanup(vcap->ved.pads);
 946 err_free_vcap:
 947         kfree(vcap);
 948
 949         return ERR_PTR(ret);
 950 }