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(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 *
dcmipp_bytecap_pix_map_by_pixelformat(u32 pixelformat)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
dcmipp_bytecap_querycap(struct file * file,void * priv,struct v4l2_capability * cap)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
dcmipp_bytecap_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
dcmipp_bytecap_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
dcmipp_bytecap_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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
dcmipp_bytecap_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)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
dcmipp_bytecap_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)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
dcmipp_pipeline_s_stream(struct dcmipp_bytecap_device * vcap,int state)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
dcmipp_start_capture(struct dcmipp_bytecap_device * vcap,struct dcmipp_buf * buf)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
dcmipp_bytecap_all_buffers_done(struct dcmipp_bytecap_device * vcap,enum vb2_buffer_state state)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
dcmipp_bytecap_start_streaming(struct vb2_queue * vq,unsigned int count)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
dcmipp_dump_status(struct dcmipp_bytecap_device * vcap)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 */
dcmipp_bytecap_stop_streaming(struct vb2_queue * vq)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
dcmipp_bytecap_buf_prepare(struct vb2_buffer * vb)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
dcmipp_bytecap_buf_queue(struct vb2_buffer * vb2_buf)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
dcmipp_bytecap_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])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
dcmipp_bytecap_buf_init(struct vb2_buffer * vb)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 * Since q->lock is set we can use the standard
630 * vb2_ops_wait_prepare/finish helper functions.
631 */
632 .wait_prepare = vb2_ops_wait_prepare,
633 .wait_finish = vb2_ops_wait_finish,
634 };
635
dcmipp_bytecap_release(struct video_device * vdev)636 static void dcmipp_bytecap_release(struct video_device *vdev)
637 {
638 struct dcmipp_bytecap_device *vcap =
639 container_of(vdev, struct dcmipp_bytecap_device, vdev);
640
641 dcmipp_pads_cleanup(vcap->ved.pads);
642 mutex_destroy(&vcap->lock);
643
644 kfree(vcap);
645 }
646
dcmipp_bytecap_ent_release(struct dcmipp_ent_device * ved)647 void dcmipp_bytecap_ent_release(struct dcmipp_ent_device *ved)
648 {
649 struct dcmipp_bytecap_device *vcap =
650 container_of(ved, struct dcmipp_bytecap_device, ved);
651
652 media_entity_cleanup(ved->ent);
653 vb2_video_unregister_device(&vcap->vdev);
654 }
655
dcmipp_buffer_done(struct dcmipp_bytecap_device * vcap,struct dcmipp_buf * buf,size_t bytesused,int err)656 static void dcmipp_buffer_done(struct dcmipp_bytecap_device *vcap,
657 struct dcmipp_buf *buf,
658 size_t bytesused,
659 int err)
660 {
661 struct vb2_v4l2_buffer *vbuf;
662
663 list_del_init(&buf->list);
664
665 vbuf = &buf->vb;
666
667 vbuf->sequence = vcap->sequence++;
668 vbuf->field = V4L2_FIELD_NONE;
669 vbuf->vb2_buf.timestamp = ktime_get_ns();
670 vb2_set_plane_payload(&vbuf->vb2_buf, 0, bytesused);
671 vb2_buffer_done(&vbuf->vb2_buf,
672 err ? VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
673 dev_dbg(vcap->dev, "Done [%d] %p phy=%pad\n", buf->vb.vb2_buf.index,
674 buf, &buf->addr);
675 vcap->count.buffers++;
676 }
677
678 /* irqlock must be held */
679 static void
dcmipp_bytecap_set_next_frame_or_stop(struct dcmipp_bytecap_device * vcap)680 dcmipp_bytecap_set_next_frame_or_stop(struct dcmipp_bytecap_device *vcap)
681 {
682 if (!vcap->next && list_is_singular(&vcap->buffers)) {
683 /*
684 * If there is no available buffer (none or a single one in the
685 * list while two are expected), stop the capture (effective
686 * for next frame). On-going frame capture will continue until
687 * FRAME END but no further capture will be done.
688 */
689 reg_clear(vcap, DCMIPP_P0FCTCR, DCMIPP_P0FCTCR_CPTREQ);
690
691 dev_dbg(vcap->dev, "Capture restart is deferred to next buffer queueing\n");
692 vcap->next = NULL;
693 vcap->state = DCMIPP_WAIT_FOR_BUFFER;
694 return;
695 }
696
697 /* If we don't have buffer yet, pick the one after active */
698 if (!vcap->next)
699 vcap->next = list_next_entry(vcap->active, list);
700
701 /*
702 * Set buffer address
703 * This register is shadowed and will be taken into
704 * account on next VSYNC (start of next frame)
705 */
706 reg_write(vcap, DCMIPP_P0PPM0AR1, vcap->next->addr);
707 dev_dbg(vcap->dev, "Write [%d] %p phy=%pad\n",
708 vcap->next->vb.vb2_buf.index, vcap->next, &vcap->next->addr);
709 }
710
711 /* irqlock must be held */
dcmipp_bytecap_process_frame(struct dcmipp_bytecap_device * vcap,size_t bytesused)712 static void dcmipp_bytecap_process_frame(struct dcmipp_bytecap_device *vcap,
713 size_t bytesused)
714 {
715 int err = 0;
716 struct dcmipp_buf *buf = vcap->active;
717
718 if (!buf) {
719 vcap->count.nactive++;
720 vcap->count.errors++;
721 return;
722 }
723
724 if (bytesused > buf->size) {
725 dev_dbg(vcap->dev, "frame larger than expected (%zu > %zu)\n",
726 bytesused, buf->size);
727 /* Clip to buffer size and return buffer to V4L2 in error */
728 bytesused = buf->size;
729 vcap->count.limit++;
730 vcap->count.errors++;
731 err = -EOVERFLOW;
732 }
733
734 dcmipp_buffer_done(vcap, buf, bytesused, err);
735 vcap->active = NULL;
736 }
737
dcmipp_bytecap_irq_thread(int irq,void * arg)738 static irqreturn_t dcmipp_bytecap_irq_thread(int irq, void *arg)
739 {
740 struct dcmipp_bytecap_device *vcap =
741 container_of(arg, struct dcmipp_bytecap_device, ved);
742 size_t bytesused = 0;
743 u32 cmsr2;
744
745 spin_lock_irq(&vcap->irqlock);
746
747 cmsr2 = vcap->cmsr2 & vcap->cmier;
748
749 /*
750 * If we have an overrun, a frame-end will probably not be generated,
751 * in that case the active buffer will be recycled as next buffer by
752 * the VSYNC handler
753 */
754 if (cmsr2 & DCMIPP_CMSR2_P0OVRF) {
755 vcap->count.errors++;
756 vcap->count.overrun++;
757 }
758
759 if (cmsr2 & DCMIPP_CMSR2_P0FRAMEF) {
760 vcap->count.frame++;
761
762 /* Read captured buffer size */
763 bytesused = reg_read(vcap, DCMIPP_P0DCCNTR);
764 dcmipp_bytecap_process_frame(vcap, bytesused);
765 }
766
767 if (cmsr2 & DCMIPP_CMSR2_P0VSYNCF) {
768 vcap->count.vsync++;
769 if (vcap->state == DCMIPP_WAIT_FOR_BUFFER) {
770 vcap->count.underrun++;
771 goto out;
772 }
773
774 /*
775 * On VSYNC, the previously set next buffer is going to become
776 * active thanks to the shadowing mechanism of the DCMIPP. In
777 * most of the cases, since a FRAMEEND has already come,
778 * pointer next is NULL since active is reset during the
779 * FRAMEEND handling. However, in case of framerate adjustment,
780 * there are more VSYNC than FRAMEEND. Thus we recycle the
781 * active (but not used) buffer and put it back into next.
782 */
783 swap(vcap->active, vcap->next);
784 dcmipp_bytecap_set_next_frame_or_stop(vcap);
785 }
786
787 out:
788 spin_unlock_irq(&vcap->irqlock);
789 return IRQ_HANDLED;
790 }
791
dcmipp_bytecap_irq_callback(int irq,void * arg)792 static irqreturn_t dcmipp_bytecap_irq_callback(int irq, void *arg)
793 {
794 struct dcmipp_bytecap_device *vcap =
795 container_of(arg, struct dcmipp_bytecap_device, ved);
796
797 /* Store interrupt status register */
798 vcap->cmsr2 = reg_read(vcap, DCMIPP_CMSR2) & vcap->cmier;
799 vcap->count.it++;
800
801 /* Clear interrupt */
802 reg_write(vcap, DCMIPP_CMFCR, vcap->cmsr2);
803
804 return IRQ_WAKE_THREAD;
805 }
806
dcmipp_bytecap_link_validate(struct media_link * link)807 static int dcmipp_bytecap_link_validate(struct media_link *link)
808 {
809 struct media_entity *entity = link->sink->entity;
810 struct video_device *vd = media_entity_to_video_device(entity);
811 struct dcmipp_bytecap_device *vcap = container_of(vd,
812 struct dcmipp_bytecap_device, vdev);
813 struct v4l2_subdev *source_sd =
814 media_entity_to_v4l2_subdev(link->source->entity);
815 struct v4l2_subdev_format source_fmt = {
816 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
817 .pad = link->source->index,
818 };
819 const struct dcmipp_bytecap_pix_map *vpix;
820 int ret;
821
822 ret = v4l2_subdev_call(source_sd, pad, get_fmt, NULL, &source_fmt);
823 if (ret < 0)
824 return 0;
825
826 if (source_fmt.format.width != vcap->format.width ||
827 source_fmt.format.height != vcap->format.height) {
828 dev_err(vcap->dev, "Wrong width or height %ux%u (%ux%u expected)\n",
829 vcap->format.width, vcap->format.height,
830 source_fmt.format.width, source_fmt.format.height);
831 return -EINVAL;
832 }
833
834 vpix = dcmipp_bytecap_pix_map_by_pixelformat(vcap->format.pixelformat);
835 if (source_fmt.format.code != vpix->code) {
836 dev_err(vcap->dev, "Wrong mbus_code 0x%x, (0x%x expected)\n",
837 vpix->code, source_fmt.format.code);
838 return -EINVAL;
839 }
840
841 return 0;
842 }
843
844 static const struct media_entity_operations dcmipp_bytecap_entity_ops = {
845 .link_validate = dcmipp_bytecap_link_validate,
846 };
847
dcmipp_bytecap_ent_init(struct device * dev,const char * entity_name,struct v4l2_device * v4l2_dev,void __iomem * regs)848 struct dcmipp_ent_device *dcmipp_bytecap_ent_init(struct device *dev,
849 const char *entity_name,
850 struct v4l2_device *v4l2_dev,
851 void __iomem *regs)
852 {
853 struct dcmipp_bytecap_device *vcap;
854 struct video_device *vdev;
855 struct vb2_queue *q;
856 const unsigned long pad_flag = MEDIA_PAD_FL_SINK;
857 int ret = 0;
858
859 /* Allocate the dcmipp_bytecap_device struct */
860 vcap = kzalloc(sizeof(*vcap), GFP_KERNEL);
861 if (!vcap)
862 return ERR_PTR(-ENOMEM);
863
864 /* Allocate the pads */
865 vcap->ved.pads = dcmipp_pads_init(1, &pad_flag);
866 if (IS_ERR(vcap->ved.pads)) {
867 ret = PTR_ERR(vcap->ved.pads);
868 goto err_free_vcap;
869 }
870
871 /* Initialize the media entity */
872 vcap->vdev.entity.name = entity_name;
873 vcap->vdev.entity.function = MEDIA_ENT_F_IO_V4L;
874 vcap->vdev.entity.ops = &dcmipp_bytecap_entity_ops;
875 ret = media_entity_pads_init(&vcap->vdev.entity, 1, vcap->ved.pads);
876 if (ret)
877 goto err_clean_pads;
878
879 /* Initialize the lock */
880 mutex_init(&vcap->lock);
881
882 /* Initialize the vb2 queue */
883 q = &vcap->queue;
884 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
885 q->io_modes = VB2_MMAP | VB2_DMABUF;
886 q->lock = &vcap->lock;
887 q->drv_priv = vcap;
888 q->buf_struct_size = sizeof(struct dcmipp_buf);
889 q->ops = &dcmipp_bytecap_qops;
890 q->mem_ops = &vb2_dma_contig_memops;
891 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
892 q->min_queued_buffers = 1;
893 q->dev = dev;
894
895 /* DCMIPP requires 16 bytes aligned buffers */
896 ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32) & ~0x0f);
897 if (ret) {
898 dev_err(dev, "Failed to set DMA mask\n");
899 goto err_mutex_destroy;
900 }
901
902 ret = vb2_queue_init(q);
903 if (ret) {
904 dev_err(dev, "%s: vb2 queue init failed (err=%d)\n",
905 entity_name, ret);
906 goto err_clean_m_ent;
907 }
908
909 /* Initialize buffer list and its lock */
910 INIT_LIST_HEAD(&vcap->buffers);
911 spin_lock_init(&vcap->irqlock);
912
913 /* Set default frame format */
914 vcap->format = fmt_default;
915
916 /* Fill the dcmipp_ent_device struct */
917 vcap->ved.ent = &vcap->vdev.entity;
918 vcap->ved.handler = dcmipp_bytecap_irq_callback;
919 vcap->ved.thread_fn = dcmipp_bytecap_irq_thread;
920 vcap->dev = dev;
921 vcap->regs = regs;
922
923 /* Initialize the video_device struct */
924 vdev = &vcap->vdev;
925 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
926 V4L2_CAP_IO_MC;
927 vdev->release = dcmipp_bytecap_release;
928 vdev->fops = &dcmipp_bytecap_fops;
929 vdev->ioctl_ops = &dcmipp_bytecap_ioctl_ops;
930 vdev->lock = &vcap->lock;
931 vdev->queue = q;
932 vdev->v4l2_dev = v4l2_dev;
933 strscpy(vdev->name, entity_name, sizeof(vdev->name));
934 video_set_drvdata(vdev, &vcap->ved);
935
936 /* Register the video_device with the v4l2 and the media framework */
937 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
938 if (ret) {
939 dev_err(dev, "%s: video register failed (err=%d)\n",
940 vcap->vdev.name, ret);
941 goto err_clean_m_ent;
942 }
943
944 return &vcap->ved;
945
946 err_clean_m_ent:
947 media_entity_cleanup(&vcap->vdev.entity);
948 err_mutex_destroy:
949 mutex_destroy(&vcap->lock);
950 err_clean_pads:
951 dcmipp_pads_cleanup(vcap->ved.pads);
952 err_free_vcap:
953 kfree(vcap);
954
955 return ERR_PTR(ret);
956 }
957