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