1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * TI Camera Access Layer (CAL) - Video Device
4 *
5 * Copyright (c) 2015-2020 Texas Instruments Inc.
6 *
7 * Authors:
8 * Benoit Parrot <bparrot@ti.com>
9 * Laurent Pinchart <laurent.pinchart@ideasonboard.com>
10 */
11
12 #include <linux/ioctl.h>
13 #include <linux/pm_runtime.h>
14 #include <linux/videodev2.h>
15
16 #include <media/media-device.h>
17 #include <media/v4l2-common.h>
18 #include <media/v4l2-ctrls.h>
19 #include <media/v4l2-device.h>
20 #include <media/v4l2-event.h>
21 #include <media/v4l2-fh.h>
22 #include <media/v4l2-ioctl.h>
23 #include <media/videobuf2-core.h>
24 #include <media/videobuf2-dma-contig.h>
25
26 #include "cal.h"
27
28 /* ------------------------------------------------------------------
29 * V4L2 Common IOCTLs
30 * ------------------------------------------------------------------
31 */
32
cal_querycap(struct file * file,void * priv,struct v4l2_capability * cap)33 static int cal_querycap(struct file *file, void *priv,
34 struct v4l2_capability *cap)
35 {
36 strscpy(cap->driver, CAL_MODULE_NAME, sizeof(cap->driver));
37 strscpy(cap->card, CAL_MODULE_NAME, sizeof(cap->card));
38
39 return 0;
40 }
41
cal_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)42 static int cal_g_fmt_vid_cap(struct file *file, void *priv,
43 struct v4l2_format *f)
44 {
45 struct cal_ctx *ctx = video_drvdata(file);
46
47 *f = ctx->v_fmt;
48
49 return 0;
50 }
51
52 /* ------------------------------------------------------------------
53 * V4L2 Video Node Centric IOCTLs
54 * ------------------------------------------------------------------
55 */
56
find_format_by_pix(struct cal_ctx * ctx,u32 pixelformat)57 static const struct cal_format_info *find_format_by_pix(struct cal_ctx *ctx,
58 u32 pixelformat)
59 {
60 const struct cal_format_info *fmtinfo;
61 unsigned int k;
62
63 for (k = 0; k < ctx->num_active_fmt; k++) {
64 fmtinfo = ctx->active_fmt[k];
65 if (fmtinfo->fourcc == pixelformat)
66 return fmtinfo;
67 }
68
69 return NULL;
70 }
71
find_format_by_code(struct cal_ctx * ctx,u32 code)72 static const struct cal_format_info *find_format_by_code(struct cal_ctx *ctx,
73 u32 code)
74 {
75 const struct cal_format_info *fmtinfo;
76 unsigned int k;
77
78 for (k = 0; k < ctx->num_active_fmt; k++) {
79 fmtinfo = ctx->active_fmt[k];
80 if (fmtinfo->code == code)
81 return fmtinfo;
82 }
83
84 return NULL;
85 }
86
cal_legacy_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)87 static int cal_legacy_enum_fmt_vid_cap(struct file *file, void *priv,
88 struct v4l2_fmtdesc *f)
89 {
90 struct cal_ctx *ctx = video_drvdata(file);
91 const struct cal_format_info *fmtinfo;
92
93 if (f->index >= ctx->num_active_fmt)
94 return -EINVAL;
95
96 fmtinfo = ctx->active_fmt[f->index];
97
98 f->pixelformat = fmtinfo->fourcc;
99 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
100 return 0;
101 }
102
__subdev_get_format(struct cal_ctx * ctx,struct v4l2_mbus_framefmt * fmt)103 static int __subdev_get_format(struct cal_ctx *ctx,
104 struct v4l2_mbus_framefmt *fmt)
105 {
106 struct v4l2_subdev_format sd_fmt = {
107 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
108 .pad = 0,
109 };
110 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
111 struct v4l2_subdev *sd = ctx->phy->source;
112 int ret;
113
114 ret = v4l2_subdev_call_state_active(sd, pad, get_fmt, &sd_fmt);
115 if (ret)
116 return ret;
117
118 *fmt = *mbus_fmt;
119
120 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
121 fmt->width, fmt->height, fmt->code);
122
123 return 0;
124 }
125
__subdev_set_format(struct cal_ctx * ctx,struct v4l2_mbus_framefmt * fmt)126 static int __subdev_set_format(struct cal_ctx *ctx,
127 struct v4l2_mbus_framefmt *fmt)
128 {
129 struct v4l2_subdev_format sd_fmt = {
130 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
131 .pad = 0,
132 };
133 struct v4l2_mbus_framefmt *mbus_fmt = &sd_fmt.format;
134 struct v4l2_subdev *sd = ctx->phy->source;
135 int ret;
136
137 *mbus_fmt = *fmt;
138
139 ret = v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
140 if (ret)
141 return ret;
142
143 ctx_dbg(1, ctx, "%s %dx%d code:%04X\n", __func__,
144 fmt->width, fmt->height, fmt->code);
145
146 return 0;
147 }
148
cal_calc_format_size(struct cal_ctx * ctx,const struct cal_format_info * fmtinfo,struct v4l2_format * f)149 static void cal_calc_format_size(struct cal_ctx *ctx,
150 const struct cal_format_info *fmtinfo,
151 struct v4l2_format *f)
152 {
153 u32 bpl, max_width;
154
155 /*
156 * Maximum width is bound by the DMA max width in bytes.
157 * We need to recalculate the actual maxi width depending on the
158 * number of bytes per pixels required.
159 */
160 max_width = CAL_MAX_WIDTH_BYTES / (ALIGN(fmtinfo->bpp, 8) >> 3);
161 v4l_bound_align_image(&f->fmt.pix.width, 48, max_width, 2,
162 &f->fmt.pix.height, 32, CAL_MAX_HEIGHT_LINES,
163 0, 0);
164
165 bpl = (f->fmt.pix.width * ALIGN(fmtinfo->bpp, 8)) >> 3;
166 f->fmt.pix.bytesperline = ALIGN(bpl, 16);
167
168 f->fmt.pix.sizeimage = f->fmt.pix.height *
169 f->fmt.pix.bytesperline;
170
171 ctx_dbg(3, ctx, "%s: fourcc: %p4cc size: %dx%d bpl:%d img_size:%d\n",
172 __func__, &f->fmt.pix.pixelformat,
173 f->fmt.pix.width, f->fmt.pix.height,
174 f->fmt.pix.bytesperline, f->fmt.pix.sizeimage);
175 }
176
cal_legacy_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)177 static int cal_legacy_try_fmt_vid_cap(struct file *file, void *priv,
178 struct v4l2_format *f)
179 {
180 struct cal_ctx *ctx = video_drvdata(file);
181 struct v4l2_subdev *sd = ctx->phy->source;
182 const struct cal_format_info *fmtinfo;
183 struct v4l2_subdev_frame_size_enum fse = {
184 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
185 };
186 int found;
187
188 fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
189 if (!fmtinfo) {
190 ctx_dbg(3, ctx, "Fourcc format (0x%08x) not found.\n",
191 f->fmt.pix.pixelformat);
192
193 /* Just get the first one enumerated */
194 fmtinfo = ctx->active_fmt[0];
195 f->fmt.pix.pixelformat = fmtinfo->fourcc;
196 }
197
198 f->fmt.pix.field = ctx->v_fmt.fmt.pix.field;
199
200 /* check for/find a valid width/height */
201 found = false;
202 fse.pad = 0;
203 fse.code = fmtinfo->code;
204 for (fse.index = 0; ; fse.index++) {
205 int ret;
206
207 ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size,
208 &fse);
209 if (ret)
210 break;
211
212 if ((f->fmt.pix.width == fse.max_width) &&
213 (f->fmt.pix.height == fse.max_height)) {
214 found = true;
215 break;
216 } else if ((f->fmt.pix.width >= fse.min_width) &&
217 (f->fmt.pix.width <= fse.max_width) &&
218 (f->fmt.pix.height >= fse.min_height) &&
219 (f->fmt.pix.height <= fse.max_height)) {
220 found = true;
221 break;
222 }
223 }
224
225 if (!found) {
226 /* use existing values as default */
227 f->fmt.pix.width = ctx->v_fmt.fmt.pix.width;
228 f->fmt.pix.height = ctx->v_fmt.fmt.pix.height;
229 }
230
231 /*
232 * Use current colorspace for now, it will get
233 * updated properly during s_fmt
234 */
235 f->fmt.pix.colorspace = ctx->v_fmt.fmt.pix.colorspace;
236 cal_calc_format_size(ctx, fmtinfo, f);
237 return 0;
238 }
239
cal_legacy_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)240 static int cal_legacy_s_fmt_vid_cap(struct file *file, void *priv,
241 struct v4l2_format *f)
242 {
243 struct cal_ctx *ctx = video_drvdata(file);
244 struct v4l2_subdev *sd = &ctx->phy->subdev;
245 struct vb2_queue *q = &ctx->vb_vidq;
246 struct v4l2_subdev_format sd_fmt = {
247 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
248 .pad = CAL_CAMERARX_PAD_SINK,
249 };
250 const struct cal_format_info *fmtinfo;
251 int ret;
252
253 if (vb2_is_busy(q)) {
254 ctx_dbg(3, ctx, "%s device busy\n", __func__);
255 return -EBUSY;
256 }
257
258 ret = cal_legacy_try_fmt_vid_cap(file, priv, f);
259 if (ret < 0)
260 return ret;
261
262 fmtinfo = find_format_by_pix(ctx, f->fmt.pix.pixelformat);
263
264 v4l2_fill_mbus_format(&sd_fmt.format, &f->fmt.pix, fmtinfo->code);
265
266 ret = __subdev_set_format(ctx, &sd_fmt.format);
267 if (ret)
268 return ret;
269
270 /* Just double check nothing has gone wrong */
271 if (sd_fmt.format.code != fmtinfo->code) {
272 ctx_dbg(3, ctx,
273 "%s subdev changed format on us, this should not happen\n",
274 __func__);
275 return -EINVAL;
276 }
277
278 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &sd_fmt.format);
279 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
280 ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc;
281 ctx->v_fmt.fmt.pix.field = sd_fmt.format.field;
282 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
283
284 v4l2_subdev_call_state_active(sd, pad, set_fmt, &sd_fmt);
285
286 ctx->fmtinfo = fmtinfo;
287 *f = ctx->v_fmt;
288
289 return 0;
290 }
291
cal_legacy_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)292 static int cal_legacy_enum_framesizes(struct file *file, void *fh,
293 struct v4l2_frmsizeenum *fsize)
294 {
295 struct cal_ctx *ctx = video_drvdata(file);
296 struct v4l2_subdev *sd = ctx->phy->source;
297 const struct cal_format_info *fmtinfo;
298 struct v4l2_subdev_frame_size_enum fse = {
299 .index = fsize->index,
300 .pad = 0,
301 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
302 };
303 int ret;
304
305 /* check for valid format */
306 fmtinfo = find_format_by_pix(ctx, fsize->pixel_format);
307 if (!fmtinfo) {
308 ctx_dbg(3, ctx, "Invalid pixel code: %x\n",
309 fsize->pixel_format);
310 return -EINVAL;
311 }
312
313 fse.code = fmtinfo->code;
314
315 ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_size, &fse);
316 if (ret)
317 return ret;
318
319 ctx_dbg(1, ctx, "%s: index: %d code: %x W:[%d,%d] H:[%d,%d]\n",
320 __func__, fse.index, fse.code, fse.min_width, fse.max_width,
321 fse.min_height, fse.max_height);
322
323 fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
324 fsize->discrete.width = fse.max_width;
325 fsize->discrete.height = fse.max_height;
326
327 return 0;
328 }
329
cal_legacy_enum_input(struct file * file,void * priv,struct v4l2_input * inp)330 static int cal_legacy_enum_input(struct file *file, void *priv,
331 struct v4l2_input *inp)
332 {
333 if (inp->index > 0)
334 return -EINVAL;
335
336 inp->type = V4L2_INPUT_TYPE_CAMERA;
337 sprintf(inp->name, "Camera %u", inp->index);
338 return 0;
339 }
340
cal_legacy_g_input(struct file * file,void * priv,unsigned int * i)341 static int cal_legacy_g_input(struct file *file, void *priv, unsigned int *i)
342 {
343 *i = 0;
344 return 0;
345 }
346
cal_legacy_s_input(struct file * file,void * priv,unsigned int i)347 static int cal_legacy_s_input(struct file *file, void *priv, unsigned int i)
348 {
349 return i > 0 ? -EINVAL : 0;
350 }
351
352 /* timeperframe is arbitrary and continuous */
cal_legacy_enum_frameintervals(struct file * file,void * priv,struct v4l2_frmivalenum * fival)353 static int cal_legacy_enum_frameintervals(struct file *file, void *priv,
354 struct v4l2_frmivalenum *fival)
355 {
356 struct cal_ctx *ctx = video_drvdata(file);
357 struct v4l2_subdev *sd = ctx->phy->source;
358 const struct cal_format_info *fmtinfo;
359 struct v4l2_subdev_frame_interval_enum fie = {
360 .index = fival->index,
361 .width = fival->width,
362 .height = fival->height,
363 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
364 };
365 int ret;
366
367 fmtinfo = find_format_by_pix(ctx, fival->pixel_format);
368 if (!fmtinfo)
369 return -EINVAL;
370
371 fie.code = fmtinfo->code;
372
373 ret = v4l2_subdev_call_state_active(sd, pad, enum_frame_interval, &fie);
374 if (ret)
375 return ret;
376 fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
377 fival->discrete = fie.interval;
378
379 return 0;
380 }
381
cal_legacy_g_parm(struct file * file,void * fh,struct v4l2_streamparm * a)382 static int cal_legacy_g_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
383 {
384 struct cal_ctx *ctx = video_drvdata(file);
385
386 return v4l2_g_parm_cap(video_devdata(file), ctx->phy->source, a);
387 }
388
cal_legacy_s_parm(struct file * file,void * fh,struct v4l2_streamparm * a)389 static int cal_legacy_s_parm(struct file *file, void *fh, struct v4l2_streamparm *a)
390 {
391 struct cal_ctx *ctx = video_drvdata(file);
392
393 return v4l2_s_parm_cap(video_devdata(file), ctx->phy->source, a);
394 }
395
396 static const struct v4l2_ioctl_ops cal_ioctl_legacy_ops = {
397 .vidioc_querycap = cal_querycap,
398 .vidioc_enum_fmt_vid_cap = cal_legacy_enum_fmt_vid_cap,
399 .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap,
400 .vidioc_try_fmt_vid_cap = cal_legacy_try_fmt_vid_cap,
401 .vidioc_s_fmt_vid_cap = cal_legacy_s_fmt_vid_cap,
402 .vidioc_enum_framesizes = cal_legacy_enum_framesizes,
403 .vidioc_reqbufs = vb2_ioctl_reqbufs,
404 .vidioc_create_bufs = vb2_ioctl_create_bufs,
405 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
406 .vidioc_querybuf = vb2_ioctl_querybuf,
407 .vidioc_qbuf = vb2_ioctl_qbuf,
408 .vidioc_dqbuf = vb2_ioctl_dqbuf,
409 .vidioc_expbuf = vb2_ioctl_expbuf,
410 .vidioc_enum_input = cal_legacy_enum_input,
411 .vidioc_g_input = cal_legacy_g_input,
412 .vidioc_s_input = cal_legacy_s_input,
413 .vidioc_enum_frameintervals = cal_legacy_enum_frameintervals,
414 .vidioc_streamon = vb2_ioctl_streamon,
415 .vidioc_streamoff = vb2_ioctl_streamoff,
416 .vidioc_log_status = v4l2_ctrl_log_status,
417 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
418 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
419 .vidioc_g_parm = cal_legacy_g_parm,
420 .vidioc_s_parm = cal_legacy_s_parm,
421 };
422
423 /* ------------------------------------------------------------------
424 * V4L2 Media Controller Centric IOCTLs
425 * ------------------------------------------------------------------
426 */
427
cal_mc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)428 static int cal_mc_enum_fmt_vid_cap(struct file *file, void *priv,
429 struct v4l2_fmtdesc *f)
430 {
431 unsigned int i;
432 unsigned int idx;
433
434 if (f->index >= cal_num_formats)
435 return -EINVAL;
436
437 idx = 0;
438
439 for (i = 0; i < cal_num_formats; ++i) {
440 if (cal_formats[i].meta)
441 continue;
442
443 if (f->mbus_code && cal_formats[i].code != f->mbus_code)
444 continue;
445
446 if (idx == f->index) {
447 f->pixelformat = cal_formats[i].fourcc;
448 f->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
449 return 0;
450 }
451
452 idx++;
453 }
454
455 return -EINVAL;
456 }
457
cal_mc_try_fmt(struct cal_ctx * ctx,struct v4l2_format * f,const struct cal_format_info ** info)458 static void cal_mc_try_fmt(struct cal_ctx *ctx, struct v4l2_format *f,
459 const struct cal_format_info **info)
460 {
461 struct v4l2_pix_format *format = &f->fmt.pix;
462 const struct cal_format_info *fmtinfo;
463 unsigned int bpp;
464
465 /*
466 * Default to the first format if the requested pixel format code isn't
467 * supported.
468 */
469 fmtinfo = cal_format_by_fourcc(f->fmt.pix.pixelformat);
470 if (!fmtinfo || fmtinfo->meta)
471 fmtinfo = &cal_formats[0];
472
473 /*
474 * Clamp the size, update the pixel format. The field and colorspace are
475 * accepted as-is, except for V4L2_FIELD_ANY that is turned into
476 * V4L2_FIELD_NONE.
477 */
478 bpp = ALIGN(fmtinfo->bpp, 8);
479
480 format->width = clamp_t(unsigned int, format->width,
481 CAL_MIN_WIDTH_BYTES * 8 / bpp,
482 CAL_MAX_WIDTH_BYTES * 8 / bpp);
483 format->height = clamp_t(unsigned int, format->height,
484 CAL_MIN_HEIGHT_LINES, CAL_MAX_HEIGHT_LINES);
485 format->pixelformat = fmtinfo->fourcc;
486
487 if (format->field == V4L2_FIELD_ANY)
488 format->field = V4L2_FIELD_NONE;
489
490 /*
491 * Calculate the number of bytes per line and the image size. The
492 * hardware stores the stride as a number of 16 bytes words, in a
493 * signed 15-bit value. Only 14 bits are thus usable.
494 */
495 format->bytesperline = ALIGN(clamp(format->bytesperline,
496 format->width * bpp / 8,
497 ((1U << 14) - 1) * 16), 16);
498
499 format->sizeimage = format->height * format->bytesperline;
500
501 format->colorspace = ctx->v_fmt.fmt.pix.colorspace;
502
503 if (info)
504 *info = fmtinfo;
505
506 ctx_dbg(3, ctx, "%s: %p4cc %ux%u (bytesperline %u sizeimage %u)\n",
507 __func__, &format->pixelformat,
508 format->width, format->height,
509 format->bytesperline, format->sizeimage);
510 }
511
cal_mc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)512 static int cal_mc_try_fmt_vid_cap(struct file *file, void *priv,
513 struct v4l2_format *f)
514 {
515 struct cal_ctx *ctx = video_drvdata(file);
516
517 cal_mc_try_fmt(ctx, f, NULL);
518 return 0;
519 }
520
cal_mc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)521 static int cal_mc_s_fmt_vid_cap(struct file *file, void *priv,
522 struct v4l2_format *f)
523 {
524 struct cal_ctx *ctx = video_drvdata(file);
525 const struct cal_format_info *fmtinfo;
526
527 if (vb2_is_busy(&ctx->vb_vidq)) {
528 ctx_dbg(3, ctx, "%s device busy\n", __func__);
529 return -EBUSY;
530 }
531
532 cal_mc_try_fmt(ctx, f, &fmtinfo);
533
534 ctx->v_fmt = *f;
535 ctx->fmtinfo = fmtinfo;
536
537 return 0;
538 }
539
cal_mc_enum_framesizes(struct file * file,void * fh,struct v4l2_frmsizeenum * fsize)540 static int cal_mc_enum_framesizes(struct file *file, void *fh,
541 struct v4l2_frmsizeenum *fsize)
542 {
543 struct cal_ctx *ctx = video_drvdata(file);
544 const struct cal_format_info *fmtinfo;
545 unsigned int bpp;
546
547 if (fsize->index > 0)
548 return -EINVAL;
549
550 fmtinfo = cal_format_by_fourcc(fsize->pixel_format);
551 if (!fmtinfo) {
552 ctx_dbg(3, ctx, "Invalid pixel format 0x%08x\n",
553 fsize->pixel_format);
554 return -EINVAL;
555 }
556
557 bpp = ALIGN(fmtinfo->bpp, 8);
558
559 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
560 fsize->stepwise.min_width = CAL_MIN_WIDTH_BYTES * 8 / bpp;
561 fsize->stepwise.max_width = CAL_MAX_WIDTH_BYTES * 8 / bpp;
562 fsize->stepwise.step_width = 64 / bpp;
563 fsize->stepwise.min_height = CAL_MIN_HEIGHT_LINES;
564 fsize->stepwise.max_height = CAL_MAX_HEIGHT_LINES;
565 fsize->stepwise.step_height = 1;
566
567 return 0;
568 }
569
570 static const struct v4l2_ioctl_ops cal_ioctl_mc_ops = {
571 .vidioc_querycap = cal_querycap,
572 .vidioc_enum_fmt_vid_cap = cal_mc_enum_fmt_vid_cap,
573 .vidioc_g_fmt_vid_cap = cal_g_fmt_vid_cap,
574 .vidioc_try_fmt_vid_cap = cal_mc_try_fmt_vid_cap,
575 .vidioc_s_fmt_vid_cap = cal_mc_s_fmt_vid_cap,
576 .vidioc_enum_framesizes = cal_mc_enum_framesizes,
577 .vidioc_reqbufs = vb2_ioctl_reqbufs,
578 .vidioc_create_bufs = vb2_ioctl_create_bufs,
579 .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
580 .vidioc_querybuf = vb2_ioctl_querybuf,
581 .vidioc_qbuf = vb2_ioctl_qbuf,
582 .vidioc_dqbuf = vb2_ioctl_dqbuf,
583 .vidioc_expbuf = vb2_ioctl_expbuf,
584 .vidioc_streamon = vb2_ioctl_streamon,
585 .vidioc_streamoff = vb2_ioctl_streamoff,
586 .vidioc_log_status = v4l2_ctrl_log_status,
587 };
588
589 /* ------------------------------------------------------------------
590 * videobuf2 Common Operations
591 * ------------------------------------------------------------------
592 */
593
cal_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])594 static int cal_queue_setup(struct vb2_queue *vq,
595 unsigned int *nbuffers, unsigned int *nplanes,
596 unsigned int sizes[], struct device *alloc_devs[])
597 {
598 struct cal_ctx *ctx = vb2_get_drv_priv(vq);
599 unsigned int size = ctx->v_fmt.fmt.pix.sizeimage;
600 unsigned int q_num_bufs = vb2_get_num_buffers(vq);
601
602 if (q_num_bufs + *nbuffers < 3)
603 *nbuffers = 3 - q_num_bufs;
604
605 if (*nplanes) {
606 if (sizes[0] < size)
607 return -EINVAL;
608 size = sizes[0];
609 }
610
611 *nplanes = 1;
612 sizes[0] = size;
613
614 ctx_dbg(3, ctx, "nbuffers=%d, size=%d\n", *nbuffers, sizes[0]);
615
616 return 0;
617 }
618
cal_buffer_prepare(struct vb2_buffer * vb)619 static int cal_buffer_prepare(struct vb2_buffer *vb)
620 {
621 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
622 struct cal_buffer *buf = container_of(vb, struct cal_buffer,
623 vb.vb2_buf);
624 unsigned long size;
625
626 size = ctx->v_fmt.fmt.pix.sizeimage;
627 if (vb2_plane_size(vb, 0) < size) {
628 ctx_err(ctx,
629 "data will not fit into plane (%lu < %lu)\n",
630 vb2_plane_size(vb, 0), size);
631 return -EINVAL;
632 }
633
634 vb2_set_plane_payload(&buf->vb.vb2_buf, 0, size);
635 return 0;
636 }
637
cal_buffer_queue(struct vb2_buffer * vb)638 static void cal_buffer_queue(struct vb2_buffer *vb)
639 {
640 struct cal_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
641 struct cal_buffer *buf = container_of(vb, struct cal_buffer,
642 vb.vb2_buf);
643 unsigned long flags;
644
645 /* recheck locking */
646 spin_lock_irqsave(&ctx->dma.lock, flags);
647 list_add_tail(&buf->list, &ctx->dma.queue);
648 spin_unlock_irqrestore(&ctx->dma.lock, flags);
649 }
650
cal_release_buffers(struct cal_ctx * ctx,enum vb2_buffer_state state)651 static void cal_release_buffers(struct cal_ctx *ctx,
652 enum vb2_buffer_state state)
653 {
654 struct cal_buffer *buf, *tmp;
655
656 /* Release all queued buffers. */
657 spin_lock_irq(&ctx->dma.lock);
658
659 list_for_each_entry_safe(buf, tmp, &ctx->dma.queue, list) {
660 list_del(&buf->list);
661 vb2_buffer_done(&buf->vb.vb2_buf, state);
662 }
663
664 if (ctx->dma.pending) {
665 vb2_buffer_done(&ctx->dma.pending->vb.vb2_buf, state);
666 ctx->dma.pending = NULL;
667 }
668
669 if (ctx->dma.active) {
670 vb2_buffer_done(&ctx->dma.active->vb.vb2_buf, state);
671 ctx->dma.active = NULL;
672 }
673
674 spin_unlock_irq(&ctx->dma.lock);
675 }
676
677 /* ------------------------------------------------------------------
678 * videobuf2 Operations
679 * ------------------------------------------------------------------
680 */
681
cal_video_check_format(struct cal_ctx * ctx)682 static int cal_video_check_format(struct cal_ctx *ctx)
683 {
684 const struct v4l2_mbus_framefmt *format;
685 struct v4l2_subdev_state *state;
686 struct media_pad *phy_source_pad;
687 int ret = 0;
688
689 phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
690 if (!phy_source_pad)
691 return -ENODEV;
692
693 state = v4l2_subdev_lock_and_get_active_state(&ctx->phy->subdev);
694
695 format = v4l2_subdev_state_get_format(state, phy_source_pad->index, 0);
696 if (!format) {
697 ret = -EINVAL;
698 goto out;
699 }
700
701 if (ctx->fmtinfo->code != format->code ||
702 ctx->v_fmt.fmt.pix.height != format->height ||
703 ctx->v_fmt.fmt.pix.width != format->width ||
704 ctx->v_fmt.fmt.pix.field != format->field) {
705 ret = -EPIPE;
706 goto out;
707 }
708
709 out:
710 v4l2_subdev_unlock_state(state);
711
712 return ret;
713 }
714
cal_start_streaming(struct vb2_queue * vq,unsigned int count)715 static int cal_start_streaming(struct vb2_queue *vq, unsigned int count)
716 {
717 struct cal_ctx *ctx = vb2_get_drv_priv(vq);
718 struct media_pad *phy_source_pad;
719 struct cal_buffer *buf;
720 dma_addr_t addr;
721 int ret;
722
723 phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
724 if (!phy_source_pad) {
725 ctx_err(ctx, "Context not connected\n");
726 ret = -ENODEV;
727 goto error_release_buffers;
728 }
729
730 ret = video_device_pipeline_alloc_start(&ctx->vdev);
731 if (ret < 0) {
732 ctx_err(ctx, "Failed to start media pipeline: %d\n", ret);
733 goto error_release_buffers;
734 }
735
736 /* Find the PHY connected to this video device */
737 if (cal_mc_api)
738 ctx->phy = cal_camerarx_get_phy_from_entity(phy_source_pad->entity);
739
740 /*
741 * Verify that the currently configured format matches the output of
742 * the connected CAMERARX.
743 */
744 ret = cal_video_check_format(ctx);
745 if (ret < 0) {
746 ctx_dbg(3, ctx,
747 "Format mismatch between CAMERARX and video node\n");
748 goto error_pipeline;
749 }
750
751 ret = cal_ctx_prepare(ctx);
752 if (ret) {
753 ctx_err(ctx, "Failed to prepare context: %d\n", ret);
754 goto error_pipeline;
755 }
756
757 spin_lock_irq(&ctx->dma.lock);
758 buf = list_first_entry(&ctx->dma.queue, struct cal_buffer, list);
759 ctx->dma.active = buf;
760 list_del(&buf->list);
761 spin_unlock_irq(&ctx->dma.lock);
762
763 addr = vb2_dma_contig_plane_dma_addr(&buf->vb.vb2_buf, 0);
764
765 ret = pm_runtime_resume_and_get(ctx->cal->dev);
766 if (ret < 0)
767 goto error_unprepare;
768
769 cal_ctx_set_dma_addr(ctx, addr);
770 cal_ctx_start(ctx);
771
772 ret = v4l2_subdev_enable_streams(&ctx->phy->subdev,
773 phy_source_pad->index, BIT(0));
774 if (ret)
775 goto error_stop;
776
777 if (cal_debug >= 4)
778 cal_quickdump_regs(ctx->cal);
779
780 return 0;
781
782 error_stop:
783 cal_ctx_stop(ctx);
784 pm_runtime_put_sync(ctx->cal->dev);
785 error_unprepare:
786 cal_ctx_unprepare(ctx);
787 error_pipeline:
788 video_device_pipeline_stop(&ctx->vdev);
789 error_release_buffers:
790 cal_release_buffers(ctx, VB2_BUF_STATE_QUEUED);
791
792 return ret;
793 }
794
cal_stop_streaming(struct vb2_queue * vq)795 static void cal_stop_streaming(struct vb2_queue *vq)
796 {
797 struct cal_ctx *ctx = vb2_get_drv_priv(vq);
798 struct media_pad *phy_source_pad;
799
800 cal_ctx_stop(ctx);
801
802 phy_source_pad = media_pad_remote_pad_first(&ctx->pad);
803
804 v4l2_subdev_disable_streams(&ctx->phy->subdev, phy_source_pad->index,
805 BIT(0));
806
807 pm_runtime_put_sync(ctx->cal->dev);
808
809 cal_ctx_unprepare(ctx);
810
811 cal_release_buffers(ctx, VB2_BUF_STATE_ERROR);
812
813 video_device_pipeline_stop(&ctx->vdev);
814
815 if (cal_mc_api)
816 ctx->phy = NULL;
817 }
818
819 static const struct vb2_ops cal_video_qops = {
820 .queue_setup = cal_queue_setup,
821 .buf_prepare = cal_buffer_prepare,
822 .buf_queue = cal_buffer_queue,
823 .start_streaming = cal_start_streaming,
824 .stop_streaming = cal_stop_streaming,
825 };
826
827 /* ------------------------------------------------------------------
828 * V4L2 Initialization and Registration
829 * ------------------------------------------------------------------
830 */
831
832 static const struct v4l2_file_operations cal_fops = {
833 .owner = THIS_MODULE,
834 .open = v4l2_fh_open,
835 .release = vb2_fop_release,
836 .poll = vb2_fop_poll,
837 .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
838 .mmap = vb2_fop_mmap,
839 };
840
cal_ctx_v4l2_init_formats(struct cal_ctx * ctx)841 static int cal_ctx_v4l2_init_formats(struct cal_ctx *ctx)
842 {
843 struct v4l2_subdev *sd = ctx->phy->source;
844 struct v4l2_mbus_framefmt mbus_fmt;
845 const struct cal_format_info *fmtinfo;
846 unsigned int i, j, k;
847 int ret = 0;
848
849 /* Enumerate sub device formats and enable all matching local formats */
850 ctx->active_fmt = devm_kcalloc(ctx->cal->dev, cal_num_formats,
851 sizeof(*ctx->active_fmt), GFP_KERNEL);
852 if (!ctx->active_fmt)
853 return -ENOMEM;
854
855 ctx->num_active_fmt = 0;
856
857 for (j = 0, i = 0; ; ++j) {
858 struct v4l2_subdev_mbus_code_enum mbus_code = {
859 .index = j,
860 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
861 };
862
863 ret = v4l2_subdev_call_state_active(sd, pad, enum_mbus_code,
864 &mbus_code);
865 if (ret == -EINVAL)
866 break;
867
868 if (ret) {
869 ctx_err(ctx, "Error enumerating mbus codes in subdev %s: %d\n",
870 sd->name, ret);
871 return ret;
872 }
873
874 ctx_dbg(2, ctx,
875 "subdev %s: code: %04x idx: %u\n",
876 sd->name, mbus_code.code, j);
877
878 for (k = 0; k < cal_num_formats; k++) {
879 fmtinfo = &cal_formats[k];
880
881 if (mbus_code.code == fmtinfo->code) {
882 ctx->active_fmt[i] = fmtinfo;
883 ctx_dbg(2, ctx,
884 "matched fourcc: %p4cc: code: %04x idx: %u\n",
885 &fmtinfo->fourcc,
886 fmtinfo->code, i);
887 ctx->num_active_fmt = ++i;
888 }
889 }
890 }
891
892 if (i == 0) {
893 ctx_err(ctx, "No suitable format reported by subdev %s\n",
894 sd->name);
895 return -EINVAL;
896 }
897
898 ret = __subdev_get_format(ctx, &mbus_fmt);
899 if (ret)
900 return ret;
901
902 fmtinfo = find_format_by_code(ctx, mbus_fmt.code);
903 if (!fmtinfo) {
904 ctx_dbg(3, ctx, "mbus code format (0x%08x) not found.\n",
905 mbus_fmt.code);
906 return -EINVAL;
907 }
908
909 /* Save current format */
910 v4l2_fill_pix_format(&ctx->v_fmt.fmt.pix, &mbus_fmt);
911 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
912 ctx->v_fmt.fmt.pix.pixelformat = fmtinfo->fourcc;
913 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
914 ctx->fmtinfo = fmtinfo;
915
916 return 0;
917 }
918
cal_ctx_v4l2_init_mc_format(struct cal_ctx * ctx)919 static int cal_ctx_v4l2_init_mc_format(struct cal_ctx *ctx)
920 {
921 const struct cal_format_info *fmtinfo;
922 struct v4l2_pix_format *pix_fmt = &ctx->v_fmt.fmt.pix;
923
924 fmtinfo = cal_format_by_code(MEDIA_BUS_FMT_UYVY8_1X16);
925 if (!fmtinfo)
926 return -EINVAL;
927
928 pix_fmt->width = 640;
929 pix_fmt->height = 480;
930 pix_fmt->field = V4L2_FIELD_NONE;
931 pix_fmt->colorspace = V4L2_COLORSPACE_SRGB;
932 pix_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
933 pix_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
934 pix_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
935 pix_fmt->pixelformat = fmtinfo->fourcc;
936
937 ctx->v_fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
938
939 /* Save current format */
940 cal_calc_format_size(ctx, fmtinfo, &ctx->v_fmt);
941 ctx->fmtinfo = fmtinfo;
942
943 return 0;
944 }
945
cal_ctx_v4l2_register(struct cal_ctx * ctx)946 int cal_ctx_v4l2_register(struct cal_ctx *ctx)
947 {
948 struct video_device *vfd = &ctx->vdev;
949 int ret;
950
951 if (!cal_mc_api) {
952 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
953
954 ret = cal_ctx_v4l2_init_formats(ctx);
955 if (ret) {
956 ctx_err(ctx, "Failed to init formats: %d\n", ret);
957 return ret;
958 }
959
960 ret = v4l2_ctrl_add_handler(hdl, ctx->phy->source->ctrl_handler,
961 NULL, true);
962 if (ret < 0) {
963 ctx_err(ctx, "Failed to add source ctrl handler\n");
964 return ret;
965 }
966 } else {
967 ret = cal_ctx_v4l2_init_mc_format(ctx);
968 if (ret) {
969 ctx_err(ctx, "Failed to init format: %d\n", ret);
970 return ret;
971 }
972 }
973
974 ret = video_register_device(vfd, VFL_TYPE_VIDEO, cal_video_nr);
975 if (ret < 0) {
976 ctx_err(ctx, "Failed to register video device\n");
977 return ret;
978 }
979
980 if (cal_mc_api) {
981 u16 phy_idx;
982 u16 pad_idx;
983
984 /* Create links from all video nodes to all PHYs */
985
986 for (phy_idx = 0; phy_idx < ctx->cal->data->num_csi2_phy;
987 ++phy_idx) {
988 struct media_entity *phy_entity =
989 &ctx->cal->phy[phy_idx]->subdev.entity;
990
991 for (pad_idx = 1; pad_idx < CAL_CAMERARX_NUM_PADS;
992 ++pad_idx) {
993 /*
994 * Enable only links from video0 to PHY0 pad 1,
995 * and video1 to PHY1 pad 1.
996 */
997 bool enable = (ctx->dma_ctx == 0 &&
998 phy_idx == 0 && pad_idx == 1) ||
999 (ctx->dma_ctx == 1 &&
1000 phy_idx == 1 && pad_idx == 1);
1001
1002 ret = media_create_pad_link(phy_entity, pad_idx,
1003 &vfd->entity, 0,
1004 enable ? MEDIA_LNK_FL_ENABLED : 0);
1005 if (ret) {
1006 ctx_err(ctx,
1007 "Failed to create media link for context %u\n",
1008 ctx->dma_ctx);
1009 video_unregister_device(vfd);
1010 return ret;
1011 }
1012 }
1013 }
1014 } else {
1015 ret = media_create_pad_link(&ctx->phy->subdev.entity,
1016 CAL_CAMERARX_PAD_FIRST_SOURCE,
1017 &vfd->entity, 0,
1018 MEDIA_LNK_FL_IMMUTABLE | MEDIA_LNK_FL_ENABLED);
1019 if (ret) {
1020 ctx_err(ctx,
1021 "Failed to create media link for context %u\n",
1022 ctx->dma_ctx);
1023 video_unregister_device(vfd);
1024 return ret;
1025 }
1026 }
1027
1028 ctx_info(ctx, "V4L2 device registered as %s\n",
1029 video_device_node_name(vfd));
1030
1031 return 0;
1032 }
1033
cal_ctx_v4l2_unregister(struct cal_ctx * ctx)1034 void cal_ctx_v4l2_unregister(struct cal_ctx *ctx)
1035 {
1036 ctx_dbg(1, ctx, "unregistering %s\n",
1037 video_device_node_name(&ctx->vdev));
1038
1039 video_unregister_device(&ctx->vdev);
1040 }
1041
cal_ctx_v4l2_init(struct cal_ctx * ctx)1042 int cal_ctx_v4l2_init(struct cal_ctx *ctx)
1043 {
1044 struct video_device *vfd = &ctx->vdev;
1045 struct vb2_queue *q = &ctx->vb_vidq;
1046 int ret;
1047
1048 INIT_LIST_HEAD(&ctx->dma.queue);
1049 spin_lock_init(&ctx->dma.lock);
1050 mutex_init(&ctx->mutex);
1051 init_waitqueue_head(&ctx->dma.wait);
1052
1053 /* Initialize the vb2 queue. */
1054 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1055 q->io_modes = VB2_MMAP | VB2_DMABUF;
1056 q->drv_priv = ctx;
1057 q->buf_struct_size = sizeof(struct cal_buffer);
1058 q->ops = &cal_video_qops;
1059 q->mem_ops = &vb2_dma_contig_memops;
1060 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
1061 q->lock = &ctx->mutex;
1062 q->min_queued_buffers = 3;
1063 q->dev = ctx->cal->dev;
1064
1065 ret = vb2_queue_init(q);
1066 if (ret)
1067 return ret;
1068
1069 /* Initialize the video device and media entity. */
1070 vfd->fops = &cal_fops;
1071 vfd->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING
1072 | (cal_mc_api ? V4L2_CAP_IO_MC : 0);
1073 vfd->v4l2_dev = &ctx->cal->v4l2_dev;
1074 vfd->queue = q;
1075 snprintf(vfd->name, sizeof(vfd->name), "CAL output %u", ctx->dma_ctx);
1076 vfd->release = video_device_release_empty;
1077 vfd->ioctl_ops = cal_mc_api ? &cal_ioctl_mc_ops : &cal_ioctl_legacy_ops;
1078 vfd->lock = &ctx->mutex;
1079 video_set_drvdata(vfd, ctx);
1080
1081 ctx->pad.flags = MEDIA_PAD_FL_SINK;
1082 ret = media_entity_pads_init(&vfd->entity, 1, &ctx->pad);
1083 if (ret < 0)
1084 return ret;
1085
1086 if (!cal_mc_api) {
1087 /* Initialize the control handler. */
1088 struct v4l2_ctrl_handler *hdl = &ctx->ctrl_handler;
1089
1090 ret = v4l2_ctrl_handler_init(hdl, 11);
1091 if (ret < 0) {
1092 ctx_err(ctx, "Failed to init ctrl handler\n");
1093 goto error;
1094 }
1095
1096 vfd->ctrl_handler = hdl;
1097 }
1098
1099 return 0;
1100
1101 error:
1102 media_entity_cleanup(&vfd->entity);
1103 return ret;
1104 }
1105
cal_ctx_v4l2_cleanup(struct cal_ctx * ctx)1106 void cal_ctx_v4l2_cleanup(struct cal_ctx *ctx)
1107 {
1108 if (!cal_mc_api)
1109 v4l2_ctrl_handler_free(&ctx->ctrl_handler);
1110
1111 media_entity_cleanup(&ctx->vdev.entity);
1112 }
1113