1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (c) 2020 Rockchip Electronics Co., Ltd. */
3
4 #include <linux/kfifo.h>
5 #include <media/v4l2-common.h>
6 #include <media/v4l2-ioctl.h>
7 #include <media/videobuf2-core.h>
8 #include <media/videobuf2-vmalloc.h>
9 #include <media/videobuf2-dma-contig.h>
10 #include <media/videobuf2-dma-sg.h>
11 #include <linux/of.h>
12 #include <linux/of_gpio.h>
13 #include <linux/of_graph.h>
14 #include <linux/of_platform.h>
15 #include <linux/of_reserved_mem.h>
16 #include <media/v4l2-event.h>
17 #include "dev.h"
18 #include "regs.h"
19 #include "mipi-csi2.h"
20 #include <media/v4l2-fwnode.h>
21 #include <linux/pm_runtime.h>
22 #include <linux/delay.h>
23
24 #define MEMORY_ALIGN_ROUND_UP_HEIGHT 16
25
26 #define TOOLS_MIN_WIDTH 4
27 #define TOOLS_MIN_HEIGHT 4
28 #define TOOLS_OUTPUT_STEP_WISE 1
29 #define CIF_TOOLS_REQ_BUFS_MIN 1
30
31 static const struct cif_output_fmt tools_out_fmts[] = {
32 {
33 .fourcc = V4L2_PIX_FMT_SRGGB8,
34 .cplanes = 1,
35 .mplanes = 1,
36 .bpp = { 8 },
37 .raw_bpp = 8,
38 .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
39 .fmt_type = CIF_FMT_TYPE_RAW,
40 }, {
41 .fourcc = V4L2_PIX_FMT_SGRBG8,
42 .cplanes = 1,
43 .mplanes = 1,
44 .bpp = { 8 },
45 .raw_bpp = 8,
46 .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
47 .fmt_type = CIF_FMT_TYPE_RAW,
48 }, {
49 .fourcc = V4L2_PIX_FMT_SGBRG8,
50 .cplanes = 1,
51 .mplanes = 1,
52 .bpp = { 8 },
53 .raw_bpp = 8,
54 .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
55 .fmt_type = CIF_FMT_TYPE_RAW,
56 }, {
57 .fourcc = V4L2_PIX_FMT_SBGGR8,
58 .cplanes = 1,
59 .mplanes = 1,
60 .bpp = { 8 },
61 .raw_bpp = 8,
62 .csi_fmt_val = CSI_WRDDR_TYPE_RAW8,
63 .fmt_type = CIF_FMT_TYPE_RAW,
64 }, {
65 .fourcc = V4L2_PIX_FMT_SRGGB10,
66 .cplanes = 1,
67 .mplanes = 1,
68 .bpp = { 16 },
69 .raw_bpp = 10,
70 .csi_fmt_val = CSI_WRDDR_TYPE_RAW10,
71 .fmt_type = CIF_FMT_TYPE_RAW,
72 }, {
73 .fourcc = V4L2_PIX_FMT_SGRBG10,
74 .cplanes = 1,
75 .mplanes = 1,
76 .bpp = { 16 },
77 .raw_bpp = 10,
78 .csi_fmt_val = CSI_WRDDR_TYPE_RAW10,
79 .fmt_type = CIF_FMT_TYPE_RAW,
80 }, {
81 .fourcc = V4L2_PIX_FMT_SGBRG10,
82 .cplanes = 1,
83 .mplanes = 1,
84 .bpp = { 16 },
85 .raw_bpp = 10,
86 .csi_fmt_val = CSI_WRDDR_TYPE_RAW10,
87 .fmt_type = CIF_FMT_TYPE_RAW,
88 }, {
89 .fourcc = V4L2_PIX_FMT_SBGGR10,
90 .cplanes = 1,
91 .mplanes = 1,
92 .bpp = { 16 },
93 .raw_bpp = 10,
94 .csi_fmt_val = CSI_WRDDR_TYPE_RAW10,
95 .fmt_type = CIF_FMT_TYPE_RAW,
96 }, {
97 .fourcc = V4L2_PIX_FMT_SRGGB12,
98 .cplanes = 1,
99 .mplanes = 1,
100 .bpp = { 16 },
101 .raw_bpp = 12,
102 .csi_fmt_val = CSI_WRDDR_TYPE_RAW12,
103 .fmt_type = CIF_FMT_TYPE_RAW,
104 }, {
105 .fourcc = V4L2_PIX_FMT_SGRBG12,
106 .cplanes = 1,
107 .mplanes = 1,
108 .bpp = { 16 },
109 .raw_bpp = 12,
110 .csi_fmt_val = CSI_WRDDR_TYPE_RAW12,
111 .fmt_type = CIF_FMT_TYPE_RAW,
112 }, {
113 .fourcc = V4L2_PIX_FMT_SGBRG12,
114 .cplanes = 1,
115 .mplanes = 1,
116 .bpp = { 16 },
117 .raw_bpp = 12,
118 .csi_fmt_val = CSI_WRDDR_TYPE_RAW12,
119 .fmt_type = CIF_FMT_TYPE_RAW,
120 }, {
121 .fourcc = V4L2_PIX_FMT_SBGGR12,
122 .cplanes = 1,
123 .mplanes = 1,
124 .bpp = { 16 },
125 .raw_bpp = 12,
126 .csi_fmt_val = CSI_WRDDR_TYPE_RAW12,
127 .fmt_type = CIF_FMT_TYPE_RAW,
128 }
129 };
130
rkcif_tools_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)131 static int rkcif_tools_enum_fmt_vid_cap(struct file *file, void *priv,
132 struct v4l2_fmtdesc *f)
133 {
134 const struct cif_output_fmt *fmt = NULL;
135
136 if (f->index >= ARRAY_SIZE(tools_out_fmts))
137 return -EINVAL;
138 fmt = &tools_out_fmts[f->index];
139 f->pixelformat = fmt->fourcc;
140 return 0;
141 }
142
rkcif_tools_g_fmt_vid_cap_mplane(struct file * file,void * priv,struct v4l2_format * f)143 static int rkcif_tools_g_fmt_vid_cap_mplane(struct file *file, void *priv,
144 struct v4l2_format *f)
145 {
146 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
147
148 f->fmt.pix_mp = tools_vdev->pixm;
149 return 0;
150 }
151
152 static const struct
rkcif_tools_find_output_fmt(u32 pixelfmt)153 cif_output_fmt *rkcif_tools_find_output_fmt(u32 pixelfmt)
154 {
155 const struct cif_output_fmt *fmt;
156 u32 i;
157
158 for (i = 0; i < ARRAY_SIZE(tools_out_fmts); i++) {
159 fmt = &tools_out_fmts[i];
160 if (fmt->fourcc == pixelfmt)
161 return fmt;
162 }
163
164 return NULL;
165 }
166
rkcif_tools_set_fmt(struct rkcif_tools_vdev * tools_vdev,struct v4l2_pix_format_mplane * pixm,bool try)167 static int rkcif_tools_set_fmt(struct rkcif_tools_vdev *tools_vdev,
168 struct v4l2_pix_format_mplane *pixm,
169 bool try)
170 {
171 struct rkcif_stream *stream = tools_vdev->stream;
172
173 *pixm = stream->pixm;
174
175 if (!try) {
176 tools_vdev->tools_out_fmt = stream->cif_fmt_out;
177 tools_vdev->pixm = *pixm;
178
179 v4l2_dbg(1, rkcif_debug, &stream->cifdev->v4l2_dev,
180 "%s: req(%d, %d)\n", __func__,
181 pixm->width, pixm->height);
182 }
183 return 0;
184 }
185
rkcif_tools_s_fmt_vid_cap_mplane(struct file * file,void * priv,struct v4l2_format * f)186 static int rkcif_tools_s_fmt_vid_cap_mplane(struct file *file,
187 void *priv, struct v4l2_format *f)
188 {
189 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
190 int ret = 0;
191
192 if (vb2_is_busy(&tools_vdev->vnode.buf_queue)) {
193 v4l2_err(&tools_vdev->cifdev->v4l2_dev, "%s queue busy\n", __func__);
194 return -EBUSY;
195 }
196
197 ret = rkcif_tools_set_fmt(tools_vdev, &f->fmt.pix_mp, false);
198
199 return ret;
200 }
201
rkcif_tools_querycap(struct file * file,void * priv,struct v4l2_capability * cap)202 static int rkcif_tools_querycap(struct file *file,
203 void *priv, struct v4l2_capability *cap)
204 {
205 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
206 struct device *dev = tools_vdev->cifdev->dev;
207
208 strscpy(cap->driver, dev->driver->name, sizeof(cap->driver));
209 strscpy(cap->card, dev->driver->name, sizeof(cap->card));
210 snprintf(cap->bus_info, sizeof(cap->bus_info),
211 "platform:%s", dev_name(dev));
212 return 0;
213 }
214
rkcif_tools_ioctl_default(struct file * file,void * fh,bool valid_prio,unsigned int cmd,void * arg)215 static long rkcif_tools_ioctl_default(struct file *file, void *fh,
216 bool valid_prio, unsigned int cmd, void *arg)
217 {
218 return 0;
219 }
220
rkcif_tools_enum_input(struct file * file,void * priv,struct v4l2_input * input)221 static int rkcif_tools_enum_input(struct file *file, void *priv,
222 struct v4l2_input *input)
223 {
224
225 if (input->index > 0)
226 return -EINVAL;
227
228 input->type = V4L2_INPUT_TYPE_CAMERA;
229 strscpy(input->name, "Camera", sizeof(input->name));
230
231 return 0;
232 }
233
rkcif_tools_try_fmt_vid_cap_mplane(struct file * file,void * fh,struct v4l2_format * f)234 static int rkcif_tools_try_fmt_vid_cap_mplane(struct file *file, void *fh,
235 struct v4l2_format *f)
236 {
237 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
238 int ret = 0;
239
240 ret = rkcif_tools_set_fmt(tools_vdev, &f->fmt.pix_mp, true);
241
242 return ret;
243 }
244
rkcif_tools_enum_frameintervals(struct file * file,void * fh,struct v4l2_frmivalenum * fival)245 static int rkcif_tools_enum_frameintervals(struct file *file, void *fh,
246 struct v4l2_frmivalenum *fival)
247 {
248 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
249 struct rkcif_device *dev = tools_vdev->cifdev;
250 struct rkcif_sensor_info *sensor = &dev->terminal_sensor;
251 struct v4l2_subdev_frame_interval fi;
252 int ret;
253
254 if (fival->index != 0)
255 return -EINVAL;
256
257 if (!sensor || !sensor->sd) {
258 /* TODO: active_sensor is NULL if using DMARX path */
259 v4l2_err(&dev->v4l2_dev, "%s Not active sensor\n", __func__);
260 return -ENODEV;
261 }
262
263 ret = v4l2_subdev_call(sensor->sd, video, g_frame_interval, &fi);
264 if (ret && ret != -ENOIOCTLCMD) {
265 return ret;
266 } else if (ret == -ENOIOCTLCMD) {
267 /* Set a default value for sensors not implements ioctl */
268 fi.interval.numerator = 1;
269 fi.interval.denominator = 30;
270 }
271
272 fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
273 fival->stepwise.step.numerator = 1;
274 fival->stepwise.step.denominator = 1;
275 fival->stepwise.max.numerator = 1;
276 fival->stepwise.max.denominator = 1;
277 fival->stepwise.min.numerator = fi.interval.numerator;
278 fival->stepwise.min.denominator = fi.interval.denominator;
279
280 return 0;
281 }
282
rkcif_tools_enum_framesizes(struct file * file,void * prov,struct v4l2_frmsizeenum * fsize)283 static int rkcif_tools_enum_framesizes(struct file *file, void *prov,
284 struct v4l2_frmsizeenum *fsize)
285 {
286 struct v4l2_frmsize_discrete *s = &fsize->discrete;
287 struct rkcif_tools_vdev *tools_vdev = video_drvdata(file);
288 struct rkcif_device *dev = tools_vdev->cifdev;
289 struct v4l2_rect input_rect;
290 struct rkcif_sensor_info *terminal_sensor = &dev->terminal_sensor;
291 struct csi_channel_info csi_info;
292
293 if (fsize->index >= ARRAY_SIZE(tools_out_fmts))
294 return -EINVAL;
295
296 if (!rkcif_tools_find_output_fmt(fsize->pixel_format))
297 return -EINVAL;
298
299 input_rect.width = RKCIF_DEFAULT_WIDTH;
300 input_rect.height = RKCIF_DEFAULT_HEIGHT;
301
302 if (terminal_sensor && terminal_sensor->sd)
303 rkcif_get_input_fmt(dev,
304 &input_rect, 0, &csi_info);
305
306 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
307 s->width = input_rect.width;
308 s->height = input_rect.height;
309
310 return 0;
311 }
312
313 /* ISP video device IOCTLs */
314 static const struct v4l2_ioctl_ops rkcif_tools_ioctl = {
315 .vidioc_reqbufs = vb2_ioctl_reqbufs,
316 .vidioc_querybuf = vb2_ioctl_querybuf,
317 .vidioc_create_bufs = vb2_ioctl_create_bufs,
318 .vidioc_qbuf = vb2_ioctl_qbuf,
319 .vidioc_dqbuf = vb2_ioctl_dqbuf,
320 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
321 .vidioc_expbuf = vb2_ioctl_expbuf,
322 .vidioc_streamon = vb2_ioctl_streamon,
323 .vidioc_streamoff = vb2_ioctl_streamoff,
324 .vidioc_enum_input = rkcif_tools_enum_input,
325 .vidioc_enum_fmt_vid_cap = rkcif_tools_enum_fmt_vid_cap,
326 .vidioc_g_fmt_vid_cap_mplane = rkcif_tools_g_fmt_vid_cap_mplane,
327 .vidioc_s_fmt_vid_cap_mplane = rkcif_tools_s_fmt_vid_cap_mplane,
328 .vidioc_try_fmt_vid_cap_mplane = rkcif_tools_try_fmt_vid_cap_mplane,
329 .vidioc_querycap = rkcif_tools_querycap,
330 .vidioc_enum_frameintervals = rkcif_tools_enum_frameintervals,
331 .vidioc_enum_framesizes = rkcif_tools_enum_framesizes,
332 .vidioc_default = rkcif_tools_ioctl_default,
333 };
334
rkcif_tools_fh_open(struct file * file)335 static int rkcif_tools_fh_open(struct file *file)
336 {
337 struct video_device *vdev = video_devdata(file);
338 struct rkcif_vdev_node *vnode = vdev_to_node(vdev);
339 struct rkcif_tools_vdev *tools_vdev = to_rkcif_tools_vdev(vnode);
340 struct rkcif_device *cifdev = tools_vdev->cifdev;
341 int ret;
342
343 ret = rkcif_update_sensor_info(tools_vdev->stream);
344 if (ret < 0) {
345 v4l2_err(vdev,
346 "update sensor info failed %d\n",
347 ret);
348
349 return ret;
350 }
351
352 ret = pm_runtime_resume_and_get(cifdev->dev);
353 if (ret < 0)
354 v4l2_err(&cifdev->v4l2_dev, "Failed to get runtime pm, %d\n",
355 ret);
356 ret = v4l2_fh_open(file);
357 if (!ret) {
358 ret = v4l2_pipeline_pm_get(&vnode->vdev.entity);
359 if (ret < 0)
360 vb2_fop_release(file);
361 }
362 return ret;
363 }
364
rkcif_tools_fop_release(struct file * file)365 static int rkcif_tools_fop_release(struct file *file)
366 {
367 struct video_device *vdev = video_devdata(file);
368 struct rkcif_vdev_node *vnode = vdev_to_node(vdev);
369 struct rkcif_tools_vdev *tools_vdev = to_rkcif_tools_vdev(vnode);
370 struct rkcif_device *cifdev = tools_vdev->cifdev;
371 int ret = 0;
372
373 ret = vb2_fop_release(file);
374 if (!ret)
375 v4l2_pipeline_pm_put(&vnode->vdev.entity);
376 pm_runtime_put_sync(cifdev->dev);
377 return 0;
378 }
379
380 struct v4l2_file_operations rkcif_tools_fops = {
381 .mmap = vb2_fop_mmap,
382 .unlocked_ioctl = video_ioctl2,
383 .poll = vb2_fop_poll,
384 .open = rkcif_tools_fh_open,
385 .release = rkcif_tools_fop_release
386 };
387
rkcif_tools_vb2_queue_setup(struct vb2_queue * queue,unsigned int * num_buffers,unsigned int * num_planes,unsigned int sizes[],struct device * alloc_ctxs[])388 static int rkcif_tools_vb2_queue_setup(struct vb2_queue *queue,
389 unsigned int *num_buffers,
390 unsigned int *num_planes,
391 unsigned int sizes[],
392 struct device *alloc_ctxs[])
393 {
394 struct rkcif_tools_vdev *tools_vdev = queue->drv_priv;
395 struct rkcif_device *cif_dev = tools_vdev->cifdev;
396 const struct v4l2_pix_format_mplane *pixm = NULL;
397 const struct cif_output_fmt *cif_fmt;
398 u32 i;
399 const struct v4l2_plane_pix_format *plane_fmt;
400
401 pixm = &tools_vdev->pixm;
402 cif_fmt = tools_vdev->tools_out_fmt;
403 *num_planes = cif_fmt->mplanes;
404
405 for (i = 0; i < cif_fmt->mplanes; i++) {
406 plane_fmt = &pixm->plane_fmt[i];
407 sizes[i] = plane_fmt->sizeimage;
408 }
409
410 v4l2_dbg(1, rkcif_debug, &cif_dev->v4l2_dev, "%s count %d, size %d\n",
411 v4l2_type_names[queue->type], *num_buffers, sizes[0]);
412 return 0;
413
414 }
415
rkcif_tools_vb2_buf_queue(struct vb2_buffer * vb)416 static void rkcif_tools_vb2_buf_queue(struct vb2_buffer *vb)
417 {
418 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
419 struct rkcif_buffer *cifbuf = to_rkcif_buffer(vbuf);
420 struct vb2_queue *queue = vb->vb2_queue;
421 struct rkcif_tools_vdev *tools_vdev = queue->drv_priv;
422 struct v4l2_pix_format_mplane *pixm = &tools_vdev->pixm;
423 const struct cif_output_fmt *fmt = tools_vdev->tools_out_fmt;
424 struct rkcif_hw *hw_dev = tools_vdev->cifdev->hw_dev;
425 unsigned long lock_flags = 0;
426 int i;
427
428 memset(cifbuf->buff_addr, 0, sizeof(cifbuf->buff_addr));
429 /* If mplanes > 1, every c-plane has its own m-plane,
430 * otherwise, multiple c-planes are in the same m-plane
431 */
432 for (i = 0; i < fmt->mplanes; i++) {
433 void *addr = vb2_plane_vaddr(vb, i);
434
435 if (hw_dev->is_dma_sg_ops) {
436 struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, i);
437
438 cifbuf->buff_addr[i] = sg_dma_address(sgt->sgl);
439 } else {
440 cifbuf->buff_addr[i] = vb2_dma_contig_plane_dma_addr(vb, i);
441 }
442 if (rkcif_debug && addr && !hw_dev->iommu_en) {
443 memset(addr, 0, pixm->plane_fmt[i].sizeimage);
444 v4l2_dbg(3, rkcif_debug, &tools_vdev->cifdev->v4l2_dev,
445 "Clear buffer, size: 0x%08x\n",
446 pixm->plane_fmt[i].sizeimage);
447 }
448 }
449
450 if (fmt->mplanes == 1) {
451 for (i = 0; i < fmt->cplanes - 1; i++)
452 cifbuf->buff_addr[i + 1] = cifbuf->buff_addr[i] +
453 pixm->plane_fmt[i].bytesperline * pixm->height;
454 }
455 spin_lock_irqsave(&tools_vdev->vbq_lock, lock_flags);
456 list_add_tail(&cifbuf->queue, &tools_vdev->buf_head);
457 spin_unlock_irqrestore(&tools_vdev->vbq_lock, lock_flags);
458 }
459
rkcif_tools_stop(struct rkcif_tools_vdev * tools_vdev)460 static int rkcif_tools_stop(struct rkcif_tools_vdev *tools_vdev)
461 {
462 unsigned long flags;
463
464 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
465 tools_vdev->state = RKCIF_STATE_READY;
466 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
467 tools_vdev->frame_idx = 0;
468 return 0;
469 }
470
rkcif_tools_vb2_stop_streaming(struct vb2_queue * vq)471 static void rkcif_tools_vb2_stop_streaming(struct vb2_queue *vq)
472 {
473 struct rkcif_tools_vdev *tools_vdev = vq->drv_priv;
474 struct rkcif_device *dev = tools_vdev->cifdev;
475 struct rkcif_buffer *buf = NULL;
476 struct rkcif_tools_buffer *tools_buf;
477 int ret = 0;
478
479 mutex_lock(&dev->tools_lock);
480
481 tools_vdev->stopping = true;
482 ret = wait_event_timeout(tools_vdev->wq_stopped,
483 tools_vdev->state != RKCIF_STATE_STREAMING,
484 msecs_to_jiffies(1000));
485 if (!ret) {
486 rkcif_tools_stop(tools_vdev);
487 tools_vdev->stopping = false;
488 }
489 /* release buffers */
490 if (tools_vdev->curr_buf)
491 list_add_tail(&tools_vdev->curr_buf->queue, &tools_vdev->buf_head);
492
493 tools_vdev->curr_buf = NULL;
494 while (!list_empty(&tools_vdev->buf_head)) {
495 buf = list_first_entry(&tools_vdev->buf_head,
496 struct rkcif_buffer, queue);
497 list_del(&buf->queue);
498 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
499 }
500 while (!list_empty(&tools_vdev->src_buf_head)) {
501 tools_buf = list_first_entry(&tools_vdev->src_buf_head,
502 struct rkcif_tools_buffer, list);
503 list_del(&tools_buf->list);
504 kfree(tools_buf);
505 tools_buf = NULL;
506 }
507 mutex_unlock(&dev->tools_lock);
508 }
509
rkcif_tools_start(struct rkcif_tools_vdev * tools_vdev)510 static int rkcif_tools_start(struct rkcif_tools_vdev *tools_vdev)
511 {
512 int ret = 0;
513 struct rkcif_device *dev = tools_vdev->cifdev;
514 struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
515
516 mutex_lock(&dev->tools_lock);
517 if (tools_vdev->state == RKCIF_STATE_STREAMING) {
518 ret = -EBUSY;
519 v4l2_err(v4l2_dev, "stream in busy state\n");
520 goto destroy_buf;
521 }
522
523 tools_vdev->frame_idx = 0;
524 tools_vdev->state = RKCIF_STATE_STREAMING;
525 mutex_unlock(&dev->tools_lock);
526 return 0;
527
528 destroy_buf:
529 if (tools_vdev->curr_buf) {
530 vb2_buffer_done(&tools_vdev->curr_buf->vb.vb2_buf,
531 VB2_BUF_STATE_QUEUED);
532 tools_vdev->curr_buf = NULL;
533 }
534 while (!list_empty(&tools_vdev->buf_head)) {
535 struct rkcif_buffer *buf;
536
537 buf = list_first_entry(&tools_vdev->buf_head,
538 struct rkcif_buffer, queue);
539 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_QUEUED);
540 list_del(&buf->queue);
541 }
542 mutex_unlock(&dev->tools_lock);
543 return ret;
544 }
545
546 static int
rkcif_tools_vb2_start_streaming(struct vb2_queue * queue,unsigned int count)547 rkcif_tools_vb2_start_streaming(struct vb2_queue *queue,
548 unsigned int count)
549 {
550 struct rkcif_tools_vdev *tools_vdev = queue->drv_priv;
551 int ret = 0;
552
553 ret = rkcif_tools_start(tools_vdev);
554 if (ret)
555 return -EINVAL;
556 return 0;
557 }
558
559 static const struct vb2_ops rkcif_tools_vb2_ops = {
560 .queue_setup = rkcif_tools_vb2_queue_setup,
561 .buf_queue = rkcif_tools_vb2_buf_queue,
562 .wait_prepare = vb2_ops_wait_prepare,
563 .wait_finish = vb2_ops_wait_finish,
564 .stop_streaming = rkcif_tools_vb2_stop_streaming,
565 .start_streaming = rkcif_tools_vb2_start_streaming,
566 };
567
rkcif_tools_init_vb2_queue(struct vb2_queue * q,struct rkcif_tools_vdev * tools_vdev,enum v4l2_buf_type buf_type)568 static int rkcif_tools_init_vb2_queue(struct vb2_queue *q,
569 struct rkcif_tools_vdev *tools_vdev,
570 enum v4l2_buf_type buf_type)
571 {
572 struct rkcif_hw *hw_dev = tools_vdev->cifdev->hw_dev;
573
574 q->type = buf_type;
575 q->io_modes = VB2_MMAP | VB2_DMABUF;
576 q->drv_priv = tools_vdev;
577 q->ops = &rkcif_tools_vb2_ops;
578 q->mem_ops = hw_dev->mem_ops;
579 q->buf_struct_size = sizeof(struct rkcif_buffer);
580 q->min_buffers_needed = CIF_TOOLS_REQ_BUFS_MIN;
581 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
582 q->lock = &tools_vdev->vnode.vlock;
583 q->dev = hw_dev->dev;
584 q->allow_cache_hints = 1;
585 q->bidirectional = 1;
586 q->gfp_flags = GFP_DMA32;
587 if (hw_dev->is_dma_contig)
588 q->dma_attrs = DMA_ATTR_FORCE_CONTIGUOUS;
589 return vb2_queue_init(q);
590 }
591
rkcif_tools_buf_done(struct rkcif_tools_vdev * tools_vdev)592 static void rkcif_tools_buf_done(struct rkcif_tools_vdev *tools_vdev)
593 {
594 struct rkcif_stream *stream = tools_vdev->stream;
595 struct rkcif_tools_buffer *tools_buf;
596 const struct cif_output_fmt *fmt = tools_vdev->tools_out_fmt;
597 struct rkcif_buffer *buf = NULL;
598 int i = 0;
599 bool is_find_tools_buf = false;
600 unsigned long flags;
601
602 retry_done_buf:
603 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
604 if (!list_empty(&tools_vdev->buf_done_head)) {
605 buf = list_first_entry(&tools_vdev->buf_done_head,
606 struct rkcif_buffer, queue);
607 if (buf)
608 list_del(&buf->queue);
609 }
610 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
611 if (!buf) {
612 v4l2_err(&stream->cifdev->v4l2_dev, "stream[%d] tools fail to get buf form list\n",
613 stream->id);
614 return;
615 }
616 if (!list_empty(&tools_vdev->src_buf_head)) {
617 list_for_each_entry(tools_buf, &tools_vdev->src_buf_head, list) {
618 if (tools_buf->vb == &buf->vb) {
619 is_find_tools_buf = true;
620 break;
621 }
622 }
623 }
624 if (!is_find_tools_buf) {
625 tools_buf = kzalloc(sizeof(struct rkcif_tools_buffer), GFP_KERNEL);
626 tools_buf->vb = &buf->vb;
627 list_add_tail(&tools_buf->list, &tools_vdev->src_buf_head);
628 }
629 tools_buf->use_cnt = 2;
630 tools_buf->frame_idx = buf->vb.sequence;
631 tools_buf->timestamp = buf->vb.vb2_buf.timestamp;
632
633 if (tools_vdev->stopping) {
634 rkcif_tools_stop(tools_vdev);
635 tools_vdev->stopping = false;
636 rkcif_vb_done_oneframe(stream, &buf->vb);
637 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
638 while (!list_empty(&tools_vdev->buf_done_head)) {
639 buf = list_first_entry(&tools_vdev->buf_done_head,
640 struct rkcif_buffer, queue);
641 if (buf) {
642 list_del(&buf->queue);
643 rkcif_vb_done_oneframe(stream, &buf->vb);
644 }
645 }
646 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
647 wake_up(&tools_vdev->wq_stopped);
648 return;
649 }
650 rkcif_vb_done_oneframe(stream, &buf->vb);
651
652 if (!list_empty(&tools_vdev->buf_head)) {
653 tools_vdev->curr_buf = list_first_entry(&tools_vdev->buf_head,
654 struct rkcif_buffer, queue);
655 if (!tools_vdev->curr_buf || tools_vdev->state != RKCIF_STATE_STREAMING) {
656 rkcif_buf_queue(&tools_buf->vb->vb2_buf);
657 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
658 if (!list_empty(&tools_vdev->buf_done_head)) {
659 spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
660 goto retry_done_buf;
661 }
662 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
663 return;
664 }
665 list_del(&tools_vdev->curr_buf->queue);
666
667 /* Dequeue a filled buffer */
668 for (i = 0; i < fmt->mplanes; i++) {
669 u32 payload_size = tools_vdev->pixm.plane_fmt[i].sizeimage;
670 void *src = vb2_plane_vaddr(&buf->vb.vb2_buf, i);
671 void *dst = vb2_plane_vaddr(&tools_vdev->curr_buf->vb.vb2_buf, i);
672
673 if (!src || !dst)
674 break;
675
676 if (buf->vb.vb2_buf.vb2_queue->mem_ops->finish)
677 buf->vb.vb2_buf.vb2_queue->mem_ops->finish(buf->vb.vb2_buf.planes[i].mem_priv);
678
679 vb2_set_plane_payload(&tools_vdev->curr_buf->vb.vb2_buf, i,
680 payload_size);
681 memcpy(dst, src, payload_size);
682 }
683 rkcif_buf_queue(&tools_buf->vb->vb2_buf);
684 tools_vdev->curr_buf->vb.sequence = tools_buf->frame_idx;
685 tools_vdev->curr_buf->vb.vb2_buf.timestamp = tools_buf->frame_idx;
686 vb2_buffer_done(&tools_vdev->curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
687 tools_vdev->curr_buf = NULL;
688 } else {
689 rkcif_buf_queue(&tools_buf->vb->vb2_buf);
690 }
691
692 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
693 if (!list_empty(&tools_vdev->buf_done_head)) {
694 spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
695 goto retry_done_buf;
696 }
697 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
698 }
699
rkcif_tools_buf_done_rdbk(struct rkcif_tools_vdev * tools_vdev)700 static void rkcif_tools_buf_done_rdbk(struct rkcif_tools_vdev *tools_vdev)
701 {
702 struct rkcif_stream *stream = tools_vdev->stream;
703 struct rkcif_device *dev = stream->cifdev;
704 const struct cif_output_fmt *fmt = tools_vdev->tools_out_fmt;
705 struct rkcif_rx_buffer *buf = NULL;
706 int i = 0;
707 unsigned long flags;
708
709 retry_done_rdbk_buf:
710 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
711 if (!list_empty(&tools_vdev->buf_done_head)) {
712 buf = list_first_entry(&tools_vdev->buf_done_head,
713 struct rkcif_rx_buffer, list);
714 if (buf)
715 list_del(&buf->list);
716 }
717 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
718 if (!buf) {
719 v4l2_err(&dev->v4l2_dev, "stream[%d] tools fail to get buf form list\n",
720 stream->id);
721 return;
722 }
723
724 if (tools_vdev->stopping) {
725 rkcif_tools_stop(tools_vdev);
726 tools_vdev->stopping = false;
727 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
728 while (!list_empty(&tools_vdev->buf_done_head)) {
729 buf = list_first_entry(&tools_vdev->buf_done_head,
730 struct rkcif_rx_buffer, list);
731 if (buf)
732 list_del(&buf->list);
733 }
734 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
735 wake_up(&tools_vdev->wq_stopped);
736 return;
737 }
738
739 if (!list_empty(&tools_vdev->buf_head)) {
740 tools_vdev->curr_buf = list_first_entry(&tools_vdev->buf_head,
741 struct rkcif_buffer, queue);
742 if (!tools_vdev->curr_buf || tools_vdev->state != RKCIF_STATE_STREAMING) {
743 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
744 if (!list_empty(&tools_vdev->buf_done_head)) {
745 spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
746 goto retry_done_rdbk_buf;
747 }
748 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
749 return;
750 }
751 list_del(&tools_vdev->curr_buf->queue);
752 /* Dequeue a filled buffer */
753 for (i = 0; i < fmt->mplanes; i++) {
754 u32 payload_size = tools_vdev->pixm.plane_fmt[i].sizeimage;
755 void *src = buf->dummy.vaddr;
756 void *dst = vb2_plane_vaddr(&tools_vdev->curr_buf->vb.vb2_buf, i);
757
758 if (!src || !dst)
759 break;
760 dma_sync_single_for_device(dev->dev,
761 buf->dummy.dma_addr,
762 buf->dummy.size,
763 DMA_FROM_DEVICE);
764 vb2_set_plane_payload(&tools_vdev->curr_buf->vb.vb2_buf, i,
765 payload_size);
766 memcpy(dst, src, payload_size);
767 }
768 tools_vdev->curr_buf->vb.sequence = buf->dbufs.sequence;
769 tools_vdev->curr_buf->vb.vb2_buf.timestamp = buf->dbufs.timestamp;
770 vb2_buffer_done(&tools_vdev->curr_buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
771 tools_vdev->curr_buf = NULL;
772 }
773
774 spin_lock_irqsave(&tools_vdev->vbq_lock, flags);
775 if (!list_empty(&tools_vdev->buf_done_head)) {
776 spin_unlock_irqrestore(&stream->tools_vdev->vbq_lock, flags);
777 goto retry_done_rdbk_buf;
778 }
779 spin_unlock_irqrestore(&tools_vdev->vbq_lock, flags);
780 }
781
rkcif_tools_work(struct work_struct * work)782 static void rkcif_tools_work(struct work_struct *work)
783 {
784 struct rkcif_tools_vdev *tools_vdev = container_of(work,
785 struct rkcif_tools_vdev,
786 work);
787 if (tools_vdev->stream->dma_en & RKCIF_DMAEN_BY_VICAP)
788 rkcif_tools_buf_done(tools_vdev);
789 else if (tools_vdev->stream->dma_en & RKCIF_DMAEN_BY_ISP)
790 rkcif_tools_buf_done_rdbk(tools_vdev);
791 }
792
rkcif_init_tools_vdev(struct rkcif_device * cif_dev,u32 ch)793 void rkcif_init_tools_vdev(struct rkcif_device *cif_dev, u32 ch)
794 {
795 struct rkcif_tools_vdev *tools_vdev = &cif_dev->tools_vdev[ch];
796 struct rkcif_stream *stream = &cif_dev->stream[ch];
797 struct v4l2_pix_format_mplane pixm;
798
799 memset(tools_vdev, 0, sizeof(*tools_vdev));
800 memset(&pixm, 0, sizeof(pixm));
801 tools_vdev->cifdev = cif_dev;
802 tools_vdev->stream = stream;
803 stream->tools_vdev = tools_vdev;
804 tools_vdev->ch = ch;
805 tools_vdev->frame_idx = 0;
806 pixm.pixelformat = V4L2_PIX_FMT_SBGGR10;
807 pixm.width = RKCIF_DEFAULT_WIDTH;
808 pixm.height = RKCIF_DEFAULT_HEIGHT;
809 tools_vdev->state = RKCIF_STATE_READY;
810 INIT_LIST_HEAD(&tools_vdev->buf_head);
811 INIT_LIST_HEAD(&tools_vdev->buf_done_head);
812 INIT_LIST_HEAD(&tools_vdev->src_buf_head);
813 spin_lock_init(&tools_vdev->vbq_lock);
814 rkcif_tools_set_fmt(tools_vdev, &pixm, false);
815 init_waitqueue_head(&tools_vdev->wq_stopped);
816 INIT_WORK(&tools_vdev->work, rkcif_tools_work);
817 }
818
rkcif_register_tools_vdev(struct rkcif_tools_vdev * tools_vdev,bool is_multi_input)819 static int rkcif_register_tools_vdev(struct rkcif_tools_vdev *tools_vdev, bool is_multi_input)
820 {
821 int ret = 0;
822 struct video_device *vdev = &tools_vdev->vnode.vdev;
823 struct rkcif_vdev_node *node;
824 char *vdev_name;
825
826 switch (tools_vdev->ch) {
827 case RKCIF_TOOLS_CH0:
828 vdev_name = CIF_TOOLS_CH0_VDEV_NAME;
829 break;
830 case RKCIF_TOOLS_CH1:
831 vdev_name = CIF_TOOLS_CH1_VDEV_NAME;
832 break;
833 case RKCIF_TOOLS_CH2:
834 vdev_name = CIF_TOOLS_CH2_VDEV_NAME;
835 break;
836 default:
837 ret = -EINVAL;
838 v4l2_err(&tools_vdev->cifdev->v4l2_dev, "Invalid stream\n");
839 goto err_cleanup_media_entity;
840 }
841
842 strscpy(vdev->name, vdev_name, sizeof(vdev->name));
843 node = container_of(vdev, struct rkcif_vdev_node, vdev);
844 mutex_init(&node->vlock);
845
846 vdev->ioctl_ops = &rkcif_tools_ioctl;
847 vdev->fops = &rkcif_tools_fops;
848 vdev->release = video_device_release_empty;
849 vdev->lock = &node->vlock;
850 vdev->v4l2_dev = &tools_vdev->cifdev->v4l2_dev;
851 vdev->queue = &node->buf_queue;
852 vdev->device_caps = V4L2_CAP_VIDEO_CAPTURE_MPLANE |
853 V4L2_CAP_STREAMING;
854 vdev->vfl_dir = VFL_DIR_RX;
855 node->pad.flags = MEDIA_PAD_FL_SINK;
856 video_set_drvdata(vdev, tools_vdev);
857
858 rkcif_tools_init_vb2_queue(&node->buf_queue,
859 tools_vdev,
860 V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
861 vdev->queue = &node->buf_queue;
862
863 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad);
864 if (ret < 0)
865 goto err_release_queue;
866
867 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1);
868 if (ret < 0) {
869 dev_err(&vdev->dev,
870 "could not register Video for Linux device\n");
871 goto err_cleanup_media_entity;
872 }
873 return 0;
874
875 err_cleanup_media_entity:
876 media_entity_cleanup(&vdev->entity);
877 err_release_queue:
878 vb2_queue_release(vdev->queue);
879 return ret;
880 }
881
rkcif_unregister_tools_vdev(struct rkcif_tools_vdev * tools_vdev)882 static void rkcif_unregister_tools_vdev(struct rkcif_tools_vdev *tools_vdev)
883 {
884 struct rkcif_vdev_node *node = &tools_vdev->vnode;
885 struct video_device *vdev = &node->vdev;
886
887 video_unregister_device(vdev);
888 media_entity_cleanup(&vdev->entity);
889 vb2_queue_release(vdev->queue);
890 }
891
rkcif_register_tools_vdevs(struct rkcif_device * cif_dev,int stream_num,bool is_multi_input)892 int rkcif_register_tools_vdevs(struct rkcif_device *cif_dev,
893 int stream_num,
894 bool is_multi_input)
895 {
896 struct rkcif_tools_vdev *tools_vdev;
897 int i, j, ret;
898
899 for (i = 0; i < stream_num; i++) {
900 tools_vdev = &cif_dev->tools_vdev[i];
901 ret = rkcif_register_tools_vdev(tools_vdev, is_multi_input);
902 if (ret < 0)
903 goto err;
904 }
905
906 return 0;
907 err:
908 for (j = 0; j < i; j++) {
909 tools_vdev = &cif_dev->tools_vdev[j];
910 rkcif_unregister_tools_vdev(tools_vdev);
911 }
912
913 return ret;
914 }
915
rkcif_unregister_tools_vdevs(struct rkcif_device * cif_dev,int stream_num)916 void rkcif_unregister_tools_vdevs(struct rkcif_device *cif_dev,
917 int stream_num)
918 {
919 struct rkcif_tools_vdev *tools_vdev;
920 int i;
921
922 for (i = 0; i < stream_num; i++) {
923 tools_vdev = &cif_dev->tools_vdev[i];
924 rkcif_unregister_tools_vdev(tools_vdev);
925 }
926 }
927