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