1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3 * A virtual v4l2-mem2mem example device.
4 *
5 * This is a virtual device driver for testing mem-to-mem vb2 framework.
6 * It simulates a device that uses memory buffers for both source and
7 * destination, processes the data and issues an "irq" (simulated by a delayed
8 * workqueue).
9 * The device is capable of multi-instance, multi-buffer-per-transaction
10 * operation (via the mem2mem framework).
11 *
12 * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
13 * Pawel Osciak, <pawel@osciak.com>
14 * Marek Szyprowski, <m.szyprowski@samsung.com>
15 */
16 #include <linux/module.h>
17 #include <linux/delay.h>
18 #include <linux/fs.h>
19 #include <linux/sched.h>
20 #include <linux/slab.h>
21
22 #include <linux/platform_device.h>
23 #include <media/v4l2-mem2mem.h>
24 #include <media/v4l2-device.h>
25 #include <media/v4l2-ioctl.h>
26 #include <media/v4l2-ctrls.h>
27 #include <media/v4l2-event.h>
28 #include <media/videobuf2-vmalloc.h>
29
30 MODULE_DESCRIPTION("Virtual device for mem2mem framework testing");
31 MODULE_AUTHOR("Pawel Osciak, <pawel@osciak.com>");
32 MODULE_LICENSE("GPL");
33 MODULE_VERSION("0.2");
34 MODULE_ALIAS("mem2mem_testdev");
35
36 static unsigned int debug;
37 module_param(debug, uint, 0644);
38 MODULE_PARM_DESC(debug, "debug level");
39
40 /* Default transaction time in msec */
41 static unsigned int default_transtime = 40; /* Max 25 fps */
42 module_param(default_transtime, uint, 0644);
43 MODULE_PARM_DESC(default_transtime, "default transaction time in ms");
44
45 #define MIN_W 32
46 #define MIN_H 32
47 #define MAX_W 640
48 #define MAX_H 480
49
50 /* Pixel alignment for non-bayer formats */
51 #define WIDTH_ALIGN 2
52 #define HEIGHT_ALIGN 1
53
54 /* Pixel alignment for bayer formats */
55 #define BAYER_WIDTH_ALIGN 2
56 #define BAYER_HEIGHT_ALIGN 2
57
58 /* Flags that indicate a format can be used for capture/output */
59 #define MEM2MEM_CAPTURE BIT(0)
60 #define MEM2MEM_OUTPUT BIT(1)
61
62 #define MEM2MEM_NAME "vim2m"
63
64 /* Per queue */
65 #define MEM2MEM_DEF_NUM_BUFS VIDEO_MAX_FRAME
66 /* In bytes, per queue */
67 #define MEM2MEM_VID_MEM_LIMIT (16 * 1024 * 1024)
68
69 /* Flags that indicate processing mode */
70 #define MEM2MEM_HFLIP BIT(0)
71 #define MEM2MEM_VFLIP BIT(1)
72
73 #define dprintk(dev, lvl, fmt, arg...) \
74 v4l2_dbg(lvl, debug, &(dev)->v4l2_dev, "%s: " fmt, __func__, ## arg)
75
vim2m_dev_release(struct device * dev)76 static void vim2m_dev_release(struct device *dev)
77 {}
78
79 static struct platform_device vim2m_pdev = {
80 .name = MEM2MEM_NAME,
81 .dev.release = vim2m_dev_release,
82 };
83
84 struct vim2m_fmt {
85 u32 fourcc;
86 int depth;
87 /* Types the format can be used for */
88 u32 types;
89 };
90
91 static struct vim2m_fmt formats[] = {
92 {
93 .fourcc = V4L2_PIX_FMT_RGB565, /* rrrrrggg gggbbbbb */
94 .depth = 16,
95 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
96 }, {
97 .fourcc = V4L2_PIX_FMT_RGB565X, /* gggbbbbb rrrrrggg */
98 .depth = 16,
99 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
100 }, {
101 .fourcc = V4L2_PIX_FMT_RGB24,
102 .depth = 24,
103 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
104 }, {
105 .fourcc = V4L2_PIX_FMT_BGR24,
106 .depth = 24,
107 .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
108 }, {
109 .fourcc = V4L2_PIX_FMT_YUYV,
110 .depth = 16,
111 .types = MEM2MEM_CAPTURE,
112 }, {
113 .fourcc = V4L2_PIX_FMT_SBGGR8,
114 .depth = 8,
115 .types = MEM2MEM_CAPTURE,
116 }, {
117 .fourcc = V4L2_PIX_FMT_SGBRG8,
118 .depth = 8,
119 .types = MEM2MEM_CAPTURE,
120 }, {
121 .fourcc = V4L2_PIX_FMT_SGRBG8,
122 .depth = 8,
123 .types = MEM2MEM_CAPTURE,
124 }, {
125 .fourcc = V4L2_PIX_FMT_SRGGB8,
126 .depth = 8,
127 .types = MEM2MEM_CAPTURE,
128 },
129 };
130
131 #define NUM_FORMATS ARRAY_SIZE(formats)
132
133 /* Per-queue, driver-specific private data */
134 struct vim2m_q_data {
135 unsigned int width;
136 unsigned int height;
137 unsigned int sizeimage;
138 unsigned int sequence;
139 struct vim2m_fmt *fmt;
140 };
141
142 enum {
143 V4L2_M2M_SRC = 0,
144 V4L2_M2M_DST = 1,
145 };
146
147 #define V4L2_CID_TRANS_TIME_MSEC (V4L2_CID_USER_BASE + 0x1000)
148 #define V4L2_CID_TRANS_NUM_BUFS (V4L2_CID_USER_BASE + 0x1001)
149
find_format(u32 fourcc)150 static struct vim2m_fmt *find_format(u32 fourcc)
151 {
152 struct vim2m_fmt *fmt;
153 unsigned int k;
154
155 for (k = 0; k < NUM_FORMATS; k++) {
156 fmt = &formats[k];
157 if (fmt->fourcc == fourcc)
158 break;
159 }
160
161 if (k == NUM_FORMATS)
162 return NULL;
163
164 return &formats[k];
165 }
166
get_alignment(u32 fourcc,unsigned int * walign,unsigned int * halign)167 static void get_alignment(u32 fourcc,
168 unsigned int *walign, unsigned int *halign)
169 {
170 switch (fourcc) {
171 case V4L2_PIX_FMT_SBGGR8:
172 case V4L2_PIX_FMT_SGBRG8:
173 case V4L2_PIX_FMT_SGRBG8:
174 case V4L2_PIX_FMT_SRGGB8:
175 *walign = BAYER_WIDTH_ALIGN;
176 *halign = BAYER_HEIGHT_ALIGN;
177 return;
178 default:
179 *walign = WIDTH_ALIGN;
180 *halign = HEIGHT_ALIGN;
181 return;
182 }
183 }
184
185 struct vim2m_dev {
186 struct v4l2_device v4l2_dev;
187 struct video_device vfd;
188 #ifdef CONFIG_MEDIA_CONTROLLER
189 struct media_device mdev;
190 #endif
191
192 atomic_t num_inst;
193 struct mutex dev_mutex;
194
195 struct v4l2_m2m_dev *m2m_dev;
196 };
197
198 struct vim2m_ctx {
199 struct v4l2_fh fh;
200 struct vim2m_dev *dev;
201
202 struct v4l2_ctrl_handler hdl;
203
204 /* Processed buffers in this transaction */
205 u8 num_processed;
206
207 /* Transaction length (i.e. how many buffers per transaction) */
208 u32 translen;
209 /* Transaction time (i.e. simulated processing time) in milliseconds */
210 u32 transtime;
211
212 struct mutex vb_mutex;
213 struct delayed_work work_run;
214
215 /* Abort requested by m2m */
216 int aborting;
217
218 /* Processing mode */
219 int mode;
220
221 enum v4l2_colorspace colorspace;
222 enum v4l2_ycbcr_encoding ycbcr_enc;
223 enum v4l2_xfer_func xfer_func;
224 enum v4l2_quantization quant;
225
226 /* Source and destination queue data */
227 struct vim2m_q_data q_data[2];
228 };
229
file2ctx(struct file * file)230 static inline struct vim2m_ctx *file2ctx(struct file *file)
231 {
232 return container_of(file->private_data, struct vim2m_ctx, fh);
233 }
234
get_q_data(struct vim2m_ctx * ctx,enum v4l2_buf_type type)235 static struct vim2m_q_data *get_q_data(struct vim2m_ctx *ctx,
236 enum v4l2_buf_type type)
237 {
238 switch (type) {
239 case V4L2_BUF_TYPE_VIDEO_OUTPUT:
240 return &ctx->q_data[V4L2_M2M_SRC];
241 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
242 return &ctx->q_data[V4L2_M2M_DST];
243 default:
244 return NULL;
245 }
246 }
247
type_name(enum v4l2_buf_type type)248 static const char *type_name(enum v4l2_buf_type type)
249 {
250 switch (type) {
251 case V4L2_BUF_TYPE_VIDEO_OUTPUT:
252 return "Output";
253 case V4L2_BUF_TYPE_VIDEO_CAPTURE:
254 return "Capture";
255 default:
256 return "Invalid";
257 }
258 }
259
260 #define CLIP(__color) \
261 (u8)(((__color) > 0xff) ? 0xff : (((__color) < 0) ? 0 : (__color)))
262
copy_line(struct vim2m_q_data * q_data_out,u8 * src,u8 * dst,bool reverse)263 static void copy_line(struct vim2m_q_data *q_data_out,
264 u8 *src, u8 *dst, bool reverse)
265 {
266 int x, depth = q_data_out->fmt->depth >> 3;
267
268 if (!reverse) {
269 memcpy(dst, src, q_data_out->width * depth);
270 } else {
271 for (x = 0; x < q_data_out->width >> 1; x++) {
272 memcpy(dst, src, depth);
273 memcpy(dst + depth, src - depth, depth);
274 src -= depth << 1;
275 dst += depth << 1;
276 }
277 return;
278 }
279 }
280
copy_two_pixels(struct vim2m_q_data * q_data_in,struct vim2m_q_data * q_data_out,u8 * src[2],u8 ** dst,int ypos,bool reverse)281 static void copy_two_pixels(struct vim2m_q_data *q_data_in,
282 struct vim2m_q_data *q_data_out,
283 u8 *src[2], u8 **dst, int ypos, bool reverse)
284 {
285 struct vim2m_fmt *out = q_data_out->fmt;
286 struct vim2m_fmt *in = q_data_in->fmt;
287 u8 _r[2], _g[2], _b[2], *r, *g, *b;
288 int i;
289
290 /* Step 1: read two consecutive pixels from src pointer */
291
292 r = _r;
293 g = _g;
294 b = _b;
295
296 switch (in->fourcc) {
297 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
298 for (i = 0; i < 2; i++) {
299 u16 pix = le16_to_cpu(*(__le16 *)(src[i]));
300
301 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
302 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
303 *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
304 }
305 break;
306 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
307 for (i = 0; i < 2; i++) {
308 u16 pix = be16_to_cpu(*(__be16 *)(src[i]));
309
310 *r++ = (u8)(((pix & 0xf800) >> 11) << 3) | 0x07;
311 *g++ = (u8)((((pix & 0x07e0) >> 5)) << 2) | 0x03;
312 *b++ = (u8)((pix & 0x1f) << 3) | 0x07;
313 }
314 break;
315 default:
316 case V4L2_PIX_FMT_RGB24:
317 for (i = 0; i < 2; i++) {
318 *r++ = src[i][0];
319 *g++ = src[i][1];
320 *b++ = src[i][2];
321 }
322 break;
323 case V4L2_PIX_FMT_BGR24:
324 for (i = 0; i < 2; i++) {
325 *b++ = src[i][0];
326 *g++ = src[i][1];
327 *r++ = src[i][2];
328 }
329 break;
330 }
331
332 /* Step 2: store two consecutive points, reversing them if needed */
333
334 r = _r;
335 g = _g;
336 b = _b;
337
338 switch (out->fourcc) {
339 case V4L2_PIX_FMT_RGB565: /* rrrrrggg gggbbbbb */
340 for (i = 0; i < 2; i++) {
341 u16 pix;
342 __le16 *dst_pix = (__le16 *)*dst;
343
344 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
345 (*b >> 3);
346
347 *dst_pix = cpu_to_le16(pix);
348
349 *dst += 2;
350 }
351 return;
352 case V4L2_PIX_FMT_RGB565X: /* gggbbbbb rrrrrggg */
353 for (i = 0; i < 2; i++) {
354 u16 pix;
355 __be16 *dst_pix = (__be16 *)*dst;
356
357 pix = ((*r << 8) & 0xf800) | ((*g << 3) & 0x07e0) |
358 (*b >> 3);
359
360 *dst_pix = cpu_to_be16(pix);
361
362 *dst += 2;
363 }
364 return;
365 case V4L2_PIX_FMT_RGB24:
366 for (i = 0; i < 2; i++) {
367 *(*dst)++ = *r++;
368 *(*dst)++ = *g++;
369 *(*dst)++ = *b++;
370 }
371 return;
372 case V4L2_PIX_FMT_BGR24:
373 for (i = 0; i < 2; i++) {
374 *(*dst)++ = *b++;
375 *(*dst)++ = *g++;
376 *(*dst)++ = *r++;
377 }
378 return;
379 case V4L2_PIX_FMT_YUYV:
380 default:
381 {
382 u8 y, y1, u, v;
383
384 y = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)
385 + 524288) >> 15);
386 u = ((-4878 * (*r) - 9578 * (*g) + 14456 * (*b)
387 + 4210688) >> 15);
388 v = ((14456 * (*r++) - 12105 * (*g++) - 2351 * (*b++)
389 + 4210688) >> 15);
390 y1 = ((8453 * (*r) + 16594 * (*g) + 3223 * (*b)
391 + 524288) >> 15);
392
393 *(*dst)++ = y;
394 *(*dst)++ = u;
395
396 *(*dst)++ = y1;
397 *(*dst)++ = v;
398 return;
399 }
400 case V4L2_PIX_FMT_SBGGR8:
401 if (!(ypos & 1)) {
402 *(*dst)++ = *b;
403 *(*dst)++ = *++g;
404 } else {
405 *(*dst)++ = *g;
406 *(*dst)++ = *++r;
407 }
408 return;
409 case V4L2_PIX_FMT_SGBRG8:
410 if (!(ypos & 1)) {
411 *(*dst)++ = *g;
412 *(*dst)++ = *++b;
413 } else {
414 *(*dst)++ = *r;
415 *(*dst)++ = *++g;
416 }
417 return;
418 case V4L2_PIX_FMT_SGRBG8:
419 if (!(ypos & 1)) {
420 *(*dst)++ = *g;
421 *(*dst)++ = *++r;
422 } else {
423 *(*dst)++ = *b;
424 *(*dst)++ = *++g;
425 }
426 return;
427 case V4L2_PIX_FMT_SRGGB8:
428 if (!(ypos & 1)) {
429 *(*dst)++ = *r;
430 *(*dst)++ = *++g;
431 } else {
432 *(*dst)++ = *g;
433 *(*dst)++ = *++b;
434 }
435 return;
436 }
437 }
438
device_process(struct vim2m_ctx * ctx,struct vb2_v4l2_buffer * in_vb,struct vb2_v4l2_buffer * out_vb)439 static int device_process(struct vim2m_ctx *ctx,
440 struct vb2_v4l2_buffer *in_vb,
441 struct vb2_v4l2_buffer *out_vb)
442 {
443 struct vim2m_dev *dev = ctx->dev;
444 struct vim2m_q_data *q_data_in, *q_data_out;
445 u8 *p_in, *p_line, *p_in_x[2], *p, *p_out;
446 unsigned int width, height, bytesperline, bytes_per_pixel;
447 unsigned int x, y, y_in, y_out, x_int, x_fract, x_err, x_offset;
448 int start, end, step;
449
450 q_data_in = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
451 if (!q_data_in)
452 return 0;
453 bytesperline = (q_data_in->width * q_data_in->fmt->depth) >> 3;
454 bytes_per_pixel = q_data_in->fmt->depth >> 3;
455
456 q_data_out = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
457 if (!q_data_out)
458 return 0;
459
460 /* As we're doing scaling, use the output dimensions here */
461 height = q_data_out->height;
462 width = q_data_out->width;
463
464 p_in = vb2_plane_vaddr(&in_vb->vb2_buf, 0);
465 p_out = vb2_plane_vaddr(&out_vb->vb2_buf, 0);
466 if (!p_in || !p_out) {
467 v4l2_err(&dev->v4l2_dev,
468 "Acquiring kernel pointers to buffers failed\n");
469 return -EFAULT;
470 }
471
472 out_vb->sequence = q_data_out->sequence++;
473 in_vb->sequence = q_data_in->sequence++;
474 v4l2_m2m_buf_copy_metadata(in_vb, out_vb, true);
475
476 if (ctx->mode & MEM2MEM_VFLIP) {
477 start = height - 1;
478 end = -1;
479 step = -1;
480 } else {
481 start = 0;
482 end = height;
483 step = 1;
484 }
485 y_out = 0;
486
487 /*
488 * When format and resolution are identical,
489 * we can use a faster copy logic
490 */
491 if (q_data_in->fmt->fourcc == q_data_out->fmt->fourcc &&
492 q_data_in->width == q_data_out->width &&
493 q_data_in->height == q_data_out->height) {
494 for (y = start; y != end; y += step, y_out++) {
495 p = p_in + (y * bytesperline);
496 if (ctx->mode & MEM2MEM_HFLIP)
497 p += bytesperline - (q_data_in->fmt->depth >> 3);
498
499 copy_line(q_data_out, p, p_out,
500 ctx->mode & MEM2MEM_HFLIP);
501
502 p_out += bytesperline;
503 }
504 return 0;
505 }
506
507 /* Slower algorithm with format conversion, hflip, vflip and scaler */
508
509 /* To speed scaler up, use Bresenham for X dimension */
510 x_int = q_data_in->width / q_data_out->width;
511 x_fract = q_data_in->width % q_data_out->width;
512
513 for (y = start; y != end; y += step, y_out++) {
514 y_in = (y * q_data_in->height) / q_data_out->height;
515 x_offset = 0;
516 x_err = 0;
517
518 p_line = p_in + (y_in * bytesperline);
519 if (ctx->mode & MEM2MEM_HFLIP)
520 p_line += bytesperline - (q_data_in->fmt->depth >> 3);
521 p_in_x[0] = p_line;
522
523 for (x = 0; x < width >> 1; x++) {
524 x_offset += x_int;
525 x_err += x_fract;
526 if (x_err > width) {
527 x_offset++;
528 x_err -= width;
529 }
530
531 if (ctx->mode & MEM2MEM_HFLIP)
532 p_in_x[1] = p_line - x_offset * bytes_per_pixel;
533 else
534 p_in_x[1] = p_line + x_offset * bytes_per_pixel;
535
536 copy_two_pixels(q_data_in, q_data_out,
537 p_in_x, &p_out, y_out,
538 ctx->mode & MEM2MEM_HFLIP);
539
540 /* Calculate the next p_in_x0 */
541 x_offset += x_int;
542 x_err += x_fract;
543 if (x_err > width) {
544 x_offset++;
545 x_err -= width;
546 }
547
548 if (ctx->mode & MEM2MEM_HFLIP)
549 p_in_x[0] = p_line - x_offset * bytes_per_pixel;
550 else
551 p_in_x[0] = p_line + x_offset * bytes_per_pixel;
552 }
553 }
554
555 return 0;
556 }
557
558 /*
559 * mem2mem callbacks
560 */
561
562 /*
563 * job_ready() - check whether an instance is ready to be scheduled to run
564 */
job_ready(void * priv)565 static int job_ready(void *priv)
566 {
567 struct vim2m_ctx *ctx = priv;
568
569 if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen
570 || v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < ctx->translen) {
571 dprintk(ctx->dev, 1, "Not enough buffers available\n");
572 return 0;
573 }
574
575 return 1;
576 }
577
job_abort(void * priv)578 static void job_abort(void *priv)
579 {
580 struct vim2m_ctx *ctx = priv;
581
582 /* Will cancel the transaction in the next interrupt handler */
583 ctx->aborting = 1;
584 }
585
586 /* device_run() - prepares and starts the device
587 *
588 * This simulates all the immediate preparations required before starting
589 * a device. This will be called by the framework when it decides to schedule
590 * a particular instance.
591 */
device_run(void * priv)592 static void device_run(void *priv)
593 {
594 struct vim2m_ctx *ctx = priv;
595 struct vb2_v4l2_buffer *src_buf, *dst_buf;
596
597 src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
598 dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
599
600 /* Apply request controls if any */
601 v4l2_ctrl_request_setup(src_buf->vb2_buf.req_obj.req,
602 &ctx->hdl);
603
604 device_process(ctx, src_buf, dst_buf);
605
606 /* Complete request controls if any */
607 v4l2_ctrl_request_complete(src_buf->vb2_buf.req_obj.req,
608 &ctx->hdl);
609
610 /* Run delayed work, which simulates a hardware irq */
611 schedule_delayed_work(&ctx->work_run, msecs_to_jiffies(ctx->transtime));
612 }
613
device_work(struct work_struct * w)614 static void device_work(struct work_struct *w)
615 {
616 struct vim2m_ctx *curr_ctx;
617 struct vim2m_dev *vim2m_dev;
618 struct vb2_v4l2_buffer *src_vb, *dst_vb;
619
620 curr_ctx = container_of(w, struct vim2m_ctx, work_run.work);
621
622 vim2m_dev = curr_ctx->dev;
623
624 src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
625 dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
626
627 curr_ctx->num_processed++;
628
629 v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
630 v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
631
632 if (curr_ctx->num_processed == curr_ctx->translen
633 || curr_ctx->aborting) {
634 dprintk(curr_ctx->dev, 2, "Finishing capture buffer fill\n");
635 curr_ctx->num_processed = 0;
636 v4l2_m2m_job_finish(vim2m_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
637 } else {
638 device_run(curr_ctx);
639 }
640 }
641
642 /*
643 * video ioctls
644 */
vidioc_querycap(struct file * file,void * priv,struct v4l2_capability * cap)645 static int vidioc_querycap(struct file *file, void *priv,
646 struct v4l2_capability *cap)
647 {
648 strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
649 strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
650 snprintf(cap->bus_info, sizeof(cap->bus_info),
651 "platform:%s", MEM2MEM_NAME);
652 return 0;
653 }
654
enum_fmt(struct v4l2_fmtdesc * f,u32 type)655 static int enum_fmt(struct v4l2_fmtdesc *f, u32 type)
656 {
657 int i, num;
658 struct vim2m_fmt *fmt;
659
660 num = 0;
661
662 for (i = 0; i < NUM_FORMATS; ++i) {
663 if (formats[i].types & type) {
664 /* index-th format of type type found ? */
665 if (num == f->index)
666 break;
667 /*
668 * Correct type but haven't reached our index yet,
669 * just increment per-type index
670 */
671 ++num;
672 }
673 }
674
675 if (i < NUM_FORMATS) {
676 /* Format found */
677 fmt = &formats[i];
678 f->pixelformat = fmt->fourcc;
679 return 0;
680 }
681
682 /* Format not found */
683 return -EINVAL;
684 }
685
vidioc_enum_fmt_vid_cap(struct file * file,void * priv,struct v4l2_fmtdesc * f)686 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
687 struct v4l2_fmtdesc *f)
688 {
689 return enum_fmt(f, MEM2MEM_CAPTURE);
690 }
691
vidioc_enum_fmt_vid_out(struct file * file,void * priv,struct v4l2_fmtdesc * f)692 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
693 struct v4l2_fmtdesc *f)
694 {
695 return enum_fmt(f, MEM2MEM_OUTPUT);
696 }
697
vidioc_enum_framesizes(struct file * file,void * priv,struct v4l2_frmsizeenum * fsize)698 static int vidioc_enum_framesizes(struct file *file, void *priv,
699 struct v4l2_frmsizeenum *fsize)
700 {
701 if (fsize->index != 0)
702 return -EINVAL;
703
704 if (!find_format(fsize->pixel_format))
705 return -EINVAL;
706
707 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
708 fsize->stepwise.min_width = MIN_W;
709 fsize->stepwise.min_height = MIN_H;
710 fsize->stepwise.max_width = MAX_W;
711 fsize->stepwise.max_height = MAX_H;
712
713 get_alignment(fsize->pixel_format,
714 &fsize->stepwise.step_width,
715 &fsize->stepwise.step_height);
716 return 0;
717 }
718
vidioc_g_fmt(struct vim2m_ctx * ctx,struct v4l2_format * f)719 static int vidioc_g_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
720 {
721 struct vb2_queue *vq;
722 struct vim2m_q_data *q_data;
723
724 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
725 if (!vq)
726 return -EINVAL;
727
728 q_data = get_q_data(ctx, f->type);
729 if (!q_data)
730 return -EINVAL;
731
732 f->fmt.pix.width = q_data->width;
733 f->fmt.pix.height = q_data->height;
734 f->fmt.pix.field = V4L2_FIELD_NONE;
735 f->fmt.pix.pixelformat = q_data->fmt->fourcc;
736 f->fmt.pix.bytesperline = (q_data->width * q_data->fmt->depth) >> 3;
737 f->fmt.pix.sizeimage = q_data->sizeimage;
738 f->fmt.pix.colorspace = ctx->colorspace;
739 f->fmt.pix.xfer_func = ctx->xfer_func;
740 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
741 f->fmt.pix.quantization = ctx->quant;
742
743 return 0;
744 }
745
vidioc_g_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)746 static int vidioc_g_fmt_vid_out(struct file *file, void *priv,
747 struct v4l2_format *f)
748 {
749 return vidioc_g_fmt(file2ctx(file), f);
750 }
751
vidioc_g_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)752 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
753 struct v4l2_format *f)
754 {
755 return vidioc_g_fmt(file2ctx(file), f);
756 }
757
vidioc_try_fmt(struct v4l2_format * f,struct vim2m_fmt * fmt)758 static int vidioc_try_fmt(struct v4l2_format *f, struct vim2m_fmt *fmt)
759 {
760 int walign, halign;
761 /*
762 * V4L2 specification specifies the driver corrects the
763 * format struct if any of the dimensions is unsupported
764 */
765 if (f->fmt.pix.height < MIN_H)
766 f->fmt.pix.height = MIN_H;
767 else if (f->fmt.pix.height > MAX_H)
768 f->fmt.pix.height = MAX_H;
769
770 if (f->fmt.pix.width < MIN_W)
771 f->fmt.pix.width = MIN_W;
772 else if (f->fmt.pix.width > MAX_W)
773 f->fmt.pix.width = MAX_W;
774
775 get_alignment(f->fmt.pix.pixelformat, &walign, &halign);
776 f->fmt.pix.width &= ~(walign - 1);
777 f->fmt.pix.height &= ~(halign - 1);
778 f->fmt.pix.bytesperline = (f->fmt.pix.width * fmt->depth) >> 3;
779 f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline;
780 f->fmt.pix.field = V4L2_FIELD_NONE;
781
782 return 0;
783 }
784
vidioc_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)785 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
786 struct v4l2_format *f)
787 {
788 struct vim2m_fmt *fmt;
789 struct vim2m_ctx *ctx = file2ctx(file);
790
791 fmt = find_format(f->fmt.pix.pixelformat);
792 if (!fmt) {
793 f->fmt.pix.pixelformat = formats[0].fourcc;
794 fmt = find_format(f->fmt.pix.pixelformat);
795 }
796 if (!(fmt->types & MEM2MEM_CAPTURE)) {
797 v4l2_err(&ctx->dev->v4l2_dev,
798 "Fourcc format (0x%08x) invalid.\n",
799 f->fmt.pix.pixelformat);
800 return -EINVAL;
801 }
802 f->fmt.pix.colorspace = ctx->colorspace;
803 f->fmt.pix.xfer_func = ctx->xfer_func;
804 f->fmt.pix.ycbcr_enc = ctx->ycbcr_enc;
805 f->fmt.pix.quantization = ctx->quant;
806
807 return vidioc_try_fmt(f, fmt);
808 }
809
vidioc_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)810 static int vidioc_try_fmt_vid_out(struct file *file, void *priv,
811 struct v4l2_format *f)
812 {
813 struct vim2m_fmt *fmt;
814 struct vim2m_ctx *ctx = file2ctx(file);
815
816 fmt = find_format(f->fmt.pix.pixelformat);
817 if (!fmt) {
818 f->fmt.pix.pixelformat = formats[0].fourcc;
819 fmt = find_format(f->fmt.pix.pixelformat);
820 }
821 if (!(fmt->types & MEM2MEM_OUTPUT)) {
822 v4l2_err(&ctx->dev->v4l2_dev,
823 "Fourcc format (0x%08x) invalid.\n",
824 f->fmt.pix.pixelformat);
825 return -EINVAL;
826 }
827 if (!f->fmt.pix.colorspace)
828 f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
829
830 return vidioc_try_fmt(f, fmt);
831 }
832
vidioc_s_fmt(struct vim2m_ctx * ctx,struct v4l2_format * f)833 static int vidioc_s_fmt(struct vim2m_ctx *ctx, struct v4l2_format *f)
834 {
835 struct vim2m_q_data *q_data;
836 struct vb2_queue *vq;
837
838 vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
839 if (!vq)
840 return -EINVAL;
841
842 q_data = get_q_data(ctx, f->type);
843 if (!q_data)
844 return -EINVAL;
845
846 if (vb2_is_busy(vq)) {
847 v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
848 return -EBUSY;
849 }
850
851 q_data->fmt = find_format(f->fmt.pix.pixelformat);
852 q_data->width = f->fmt.pix.width;
853 q_data->height = f->fmt.pix.height;
854 q_data->sizeimage = q_data->width * q_data->height
855 * q_data->fmt->depth >> 3;
856
857 dprintk(ctx->dev, 1,
858 "Format for type %s: %dx%d (%d bpp), fmt: %c%c%c%c\n",
859 type_name(f->type), q_data->width, q_data->height,
860 q_data->fmt->depth,
861 (q_data->fmt->fourcc & 0xff),
862 (q_data->fmt->fourcc >> 8) & 0xff,
863 (q_data->fmt->fourcc >> 16) & 0xff,
864 (q_data->fmt->fourcc >> 24) & 0xff);
865
866 return 0;
867 }
868
vidioc_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)869 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
870 struct v4l2_format *f)
871 {
872 int ret;
873
874 ret = vidioc_try_fmt_vid_cap(file, priv, f);
875 if (ret)
876 return ret;
877
878 return vidioc_s_fmt(file2ctx(file), f);
879 }
880
vidioc_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)881 static int vidioc_s_fmt_vid_out(struct file *file, void *priv,
882 struct v4l2_format *f)
883 {
884 struct vim2m_ctx *ctx = file2ctx(file);
885 int ret;
886
887 ret = vidioc_try_fmt_vid_out(file, priv, f);
888 if (ret)
889 return ret;
890
891 ret = vidioc_s_fmt(file2ctx(file), f);
892 if (!ret) {
893 ctx->colorspace = f->fmt.pix.colorspace;
894 ctx->xfer_func = f->fmt.pix.xfer_func;
895 ctx->ycbcr_enc = f->fmt.pix.ycbcr_enc;
896 ctx->quant = f->fmt.pix.quantization;
897 }
898 return ret;
899 }
900
vim2m_s_ctrl(struct v4l2_ctrl * ctrl)901 static int vim2m_s_ctrl(struct v4l2_ctrl *ctrl)
902 {
903 struct vim2m_ctx *ctx =
904 container_of(ctrl->handler, struct vim2m_ctx, hdl);
905
906 switch (ctrl->id) {
907 case V4L2_CID_HFLIP:
908 if (ctrl->val)
909 ctx->mode |= MEM2MEM_HFLIP;
910 else
911 ctx->mode &= ~MEM2MEM_HFLIP;
912 break;
913
914 case V4L2_CID_VFLIP:
915 if (ctrl->val)
916 ctx->mode |= MEM2MEM_VFLIP;
917 else
918 ctx->mode &= ~MEM2MEM_VFLIP;
919 break;
920
921 case V4L2_CID_TRANS_TIME_MSEC:
922 ctx->transtime = ctrl->val;
923 if (ctx->transtime < 1)
924 ctx->transtime = 1;
925 break;
926
927 case V4L2_CID_TRANS_NUM_BUFS:
928 ctx->translen = ctrl->val;
929 break;
930
931 default:
932 v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
933 return -EINVAL;
934 }
935
936 return 0;
937 }
938
939 static const struct v4l2_ctrl_ops vim2m_ctrl_ops = {
940 .s_ctrl = vim2m_s_ctrl,
941 };
942
943 static const struct v4l2_ioctl_ops vim2m_ioctl_ops = {
944 .vidioc_querycap = vidioc_querycap,
945
946 .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
947 .vidioc_enum_framesizes = vidioc_enum_framesizes,
948 .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
949 .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
950 .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
951
952 .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
953 .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out,
954 .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out,
955 .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out,
956
957 .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs,
958 .vidioc_querybuf = v4l2_m2m_ioctl_querybuf,
959 .vidioc_qbuf = v4l2_m2m_ioctl_qbuf,
960 .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf,
961 .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf,
962 .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs,
963 .vidioc_expbuf = v4l2_m2m_ioctl_expbuf,
964
965 .vidioc_streamon = v4l2_m2m_ioctl_streamon,
966 .vidioc_streamoff = v4l2_m2m_ioctl_streamoff,
967
968 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
969 .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
970 };
971
972 /*
973 * Queue operations
974 */
975
vim2m_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])976 static int vim2m_queue_setup(struct vb2_queue *vq,
977 unsigned int *nbuffers,
978 unsigned int *nplanes,
979 unsigned int sizes[],
980 struct device *alloc_devs[])
981 {
982 struct vim2m_ctx *ctx = vb2_get_drv_priv(vq);
983 struct vim2m_q_data *q_data;
984 unsigned int size, count = *nbuffers;
985
986 q_data = get_q_data(ctx, vq->type);
987 if (!q_data)
988 return -EINVAL;
989
990 size = q_data->width * q_data->height * q_data->fmt->depth >> 3;
991
992 while (size * count > MEM2MEM_VID_MEM_LIMIT)
993 (count)--;
994 *nbuffers = count;
995
996 if (*nplanes)
997 return sizes[0] < size ? -EINVAL : 0;
998
999 *nplanes = 1;
1000 sizes[0] = size;
1001
1002 dprintk(ctx->dev, 1, "%s: get %d buffer(s) of size %d each.\n",
1003 type_name(vq->type), count, size);
1004
1005 return 0;
1006 }
1007
vim2m_buf_out_validate(struct vb2_buffer * vb)1008 static int vim2m_buf_out_validate(struct vb2_buffer *vb)
1009 {
1010 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1011 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1012
1013 if (vbuf->field == V4L2_FIELD_ANY)
1014 vbuf->field = V4L2_FIELD_NONE;
1015 if (vbuf->field != V4L2_FIELD_NONE) {
1016 dprintk(ctx->dev, 1, "%s field isn't supported\n", __func__);
1017 return -EINVAL;
1018 }
1019
1020 return 0;
1021 }
1022
vim2m_buf_prepare(struct vb2_buffer * vb)1023 static int vim2m_buf_prepare(struct vb2_buffer *vb)
1024 {
1025 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1026 struct vim2m_q_data *q_data;
1027
1028 dprintk(ctx->dev, 2, "type: %s\n", type_name(vb->vb2_queue->type));
1029
1030 q_data = get_q_data(ctx, vb->vb2_queue->type);
1031 if (!q_data)
1032 return -EINVAL;
1033 if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1034 dprintk(ctx->dev, 1,
1035 "%s data will not fit into plane (%lu < %lu)\n",
1036 __func__, vb2_plane_size(vb, 0),
1037 (long)q_data->sizeimage);
1038 return -EINVAL;
1039 }
1040
1041 vb2_set_plane_payload(vb, 0, q_data->sizeimage);
1042
1043 return 0;
1044 }
1045
vim2m_buf_queue(struct vb2_buffer * vb)1046 static void vim2m_buf_queue(struct vb2_buffer *vb)
1047 {
1048 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1049 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1050
1051 v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1052 }
1053
vim2m_start_streaming(struct vb2_queue * q,unsigned int count)1054 static int vim2m_start_streaming(struct vb2_queue *q, unsigned int count)
1055 {
1056 struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1057 struct vim2m_q_data *q_data = get_q_data(ctx, q->type);
1058
1059 if (!q_data)
1060 return -EINVAL;
1061
1062 if (V4L2_TYPE_IS_OUTPUT(q->type))
1063 ctx->aborting = 0;
1064
1065 q_data->sequence = 0;
1066 return 0;
1067 }
1068
vim2m_stop_streaming(struct vb2_queue * q)1069 static void vim2m_stop_streaming(struct vb2_queue *q)
1070 {
1071 struct vim2m_ctx *ctx = vb2_get_drv_priv(q);
1072 struct vb2_v4l2_buffer *vbuf;
1073
1074 cancel_delayed_work_sync(&ctx->work_run);
1075
1076 for (;;) {
1077 if (V4L2_TYPE_IS_OUTPUT(q->type))
1078 vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1079 else
1080 vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1081 if (!vbuf)
1082 return;
1083 v4l2_ctrl_request_complete(vbuf->vb2_buf.req_obj.req,
1084 &ctx->hdl);
1085 v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
1086 }
1087 }
1088
vim2m_buf_request_complete(struct vb2_buffer * vb)1089 static void vim2m_buf_request_complete(struct vb2_buffer *vb)
1090 {
1091 struct vim2m_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1092
1093 v4l2_ctrl_request_complete(vb->req_obj.req, &ctx->hdl);
1094 }
1095
1096 static const struct vb2_ops vim2m_qops = {
1097 .queue_setup = vim2m_queue_setup,
1098 .buf_out_validate = vim2m_buf_out_validate,
1099 .buf_prepare = vim2m_buf_prepare,
1100 .buf_queue = vim2m_buf_queue,
1101 .start_streaming = vim2m_start_streaming,
1102 .stop_streaming = vim2m_stop_streaming,
1103 .wait_prepare = vb2_ops_wait_prepare,
1104 .wait_finish = vb2_ops_wait_finish,
1105 .buf_request_complete = vim2m_buf_request_complete,
1106 };
1107
queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)1108 static int queue_init(void *priv, struct vb2_queue *src_vq,
1109 struct vb2_queue *dst_vq)
1110 {
1111 struct vim2m_ctx *ctx = priv;
1112 int ret;
1113
1114 src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1115 src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1116 src_vq->drv_priv = ctx;
1117 src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1118 src_vq->ops = &vim2m_qops;
1119 src_vq->mem_ops = &vb2_vmalloc_memops;
1120 src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1121 src_vq->lock = &ctx->vb_mutex;
1122 src_vq->supports_requests = true;
1123
1124 ret = vb2_queue_init(src_vq);
1125 if (ret)
1126 return ret;
1127
1128 dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1129 dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
1130 dst_vq->drv_priv = ctx;
1131 dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1132 dst_vq->ops = &vim2m_qops;
1133 dst_vq->mem_ops = &vb2_vmalloc_memops;
1134 dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1135 dst_vq->lock = &ctx->vb_mutex;
1136
1137 return vb2_queue_init(dst_vq);
1138 }
1139
1140 static struct v4l2_ctrl_config vim2m_ctrl_trans_time_msec = {
1141 .ops = &vim2m_ctrl_ops,
1142 .id = V4L2_CID_TRANS_TIME_MSEC,
1143 .name = "Transaction Time (msec)",
1144 .type = V4L2_CTRL_TYPE_INTEGER,
1145 .min = 1,
1146 .max = 10001,
1147 .step = 1,
1148 };
1149
1150 static const struct v4l2_ctrl_config vim2m_ctrl_trans_num_bufs = {
1151 .ops = &vim2m_ctrl_ops,
1152 .id = V4L2_CID_TRANS_NUM_BUFS,
1153 .name = "Buffers Per Transaction",
1154 .type = V4L2_CTRL_TYPE_INTEGER,
1155 .def = 1,
1156 .min = 1,
1157 .max = MEM2MEM_DEF_NUM_BUFS,
1158 .step = 1,
1159 };
1160
1161 /*
1162 * File operations
1163 */
vim2m_open(struct file * file)1164 static int vim2m_open(struct file *file)
1165 {
1166 struct vim2m_dev *dev = video_drvdata(file);
1167 struct vim2m_ctx *ctx = NULL;
1168 struct v4l2_ctrl_handler *hdl;
1169 int rc = 0;
1170
1171 if (mutex_lock_interruptible(&dev->dev_mutex))
1172 return -ERESTARTSYS;
1173 ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1174 if (!ctx) {
1175 rc = -ENOMEM;
1176 goto open_unlock;
1177 }
1178
1179 v4l2_fh_init(&ctx->fh, video_devdata(file));
1180 file->private_data = &ctx->fh;
1181 ctx->dev = dev;
1182 hdl = &ctx->hdl;
1183 v4l2_ctrl_handler_init(hdl, 4);
1184 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1185 v4l2_ctrl_new_std(hdl, &vim2m_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1186
1187 vim2m_ctrl_trans_time_msec.def = default_transtime;
1188 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_time_msec, NULL);
1189 v4l2_ctrl_new_custom(hdl, &vim2m_ctrl_trans_num_bufs, NULL);
1190 if (hdl->error) {
1191 rc = hdl->error;
1192 v4l2_ctrl_handler_free(hdl);
1193 kfree(ctx);
1194 goto open_unlock;
1195 }
1196 ctx->fh.ctrl_handler = hdl;
1197 v4l2_ctrl_handler_setup(hdl);
1198
1199 ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
1200 ctx->q_data[V4L2_M2M_SRC].width = 640;
1201 ctx->q_data[V4L2_M2M_SRC].height = 480;
1202 ctx->q_data[V4L2_M2M_SRC].sizeimage =
1203 ctx->q_data[V4L2_M2M_SRC].width *
1204 ctx->q_data[V4L2_M2M_SRC].height *
1205 (ctx->q_data[V4L2_M2M_SRC].fmt->depth >> 3);
1206 ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1207 ctx->colorspace = V4L2_COLORSPACE_REC709;
1208
1209 ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
1210
1211 mutex_init(&ctx->vb_mutex);
1212 INIT_DELAYED_WORK(&ctx->work_run, device_work);
1213
1214 if (IS_ERR(ctx->fh.m2m_ctx)) {
1215 rc = PTR_ERR(ctx->fh.m2m_ctx);
1216
1217 v4l2_ctrl_handler_free(hdl);
1218 v4l2_fh_exit(&ctx->fh);
1219 kfree(ctx);
1220 goto open_unlock;
1221 }
1222
1223 v4l2_fh_add(&ctx->fh);
1224 atomic_inc(&dev->num_inst);
1225
1226 dprintk(dev, 1, "Created instance: %p, m2m_ctx: %p\n",
1227 ctx, ctx->fh.m2m_ctx);
1228
1229 open_unlock:
1230 mutex_unlock(&dev->dev_mutex);
1231 return rc;
1232 }
1233
vim2m_release(struct file * file)1234 static int vim2m_release(struct file *file)
1235 {
1236 struct vim2m_dev *dev = video_drvdata(file);
1237 struct vim2m_ctx *ctx = file2ctx(file);
1238
1239 dprintk(dev, 1, "Releasing instance %p\n", ctx);
1240
1241 v4l2_fh_del(&ctx->fh);
1242 v4l2_fh_exit(&ctx->fh);
1243 v4l2_ctrl_handler_free(&ctx->hdl);
1244 mutex_lock(&dev->dev_mutex);
1245 v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1246 mutex_unlock(&dev->dev_mutex);
1247 kfree(ctx);
1248
1249 atomic_dec(&dev->num_inst);
1250
1251 return 0;
1252 }
1253
vim2m_device_release(struct video_device * vdev)1254 static void vim2m_device_release(struct video_device *vdev)
1255 {
1256 struct vim2m_dev *dev = container_of(vdev, struct vim2m_dev, vfd);
1257
1258 v4l2_device_unregister(&dev->v4l2_dev);
1259 v4l2_m2m_release(dev->m2m_dev);
1260 #ifdef CONFIG_MEDIA_CONTROLLER
1261 media_device_cleanup(&dev->mdev);
1262 #endif
1263 kfree(dev);
1264 }
1265
1266 static const struct v4l2_file_operations vim2m_fops = {
1267 .owner = THIS_MODULE,
1268 .open = vim2m_open,
1269 .release = vim2m_release,
1270 .poll = v4l2_m2m_fop_poll,
1271 .unlocked_ioctl = video_ioctl2,
1272 .mmap = v4l2_m2m_fop_mmap,
1273 };
1274
1275 static const struct video_device vim2m_videodev = {
1276 .name = MEM2MEM_NAME,
1277 .vfl_dir = VFL_DIR_M2M,
1278 .fops = &vim2m_fops,
1279 .ioctl_ops = &vim2m_ioctl_ops,
1280 .minor = -1,
1281 .release = vim2m_device_release,
1282 .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
1283 };
1284
1285 static const struct v4l2_m2m_ops m2m_ops = {
1286 .device_run = device_run,
1287 .job_ready = job_ready,
1288 .job_abort = job_abort,
1289 };
1290
1291 static const struct media_device_ops m2m_media_ops = {
1292 .req_validate = vb2_request_validate,
1293 .req_queue = v4l2_m2m_request_queue,
1294 };
1295
vim2m_probe(struct platform_device * pdev)1296 static int vim2m_probe(struct platform_device *pdev)
1297 {
1298 struct vim2m_dev *dev;
1299 struct video_device *vfd;
1300 int ret;
1301
1302 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
1303 if (!dev)
1304 return -ENOMEM;
1305
1306 ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1307 if (ret)
1308 goto error_free;
1309
1310 atomic_set(&dev->num_inst, 0);
1311 mutex_init(&dev->dev_mutex);
1312
1313 dev->vfd = vim2m_videodev;
1314 vfd = &dev->vfd;
1315 vfd->lock = &dev->dev_mutex;
1316 vfd->v4l2_dev = &dev->v4l2_dev;
1317
1318 video_set_drvdata(vfd, dev);
1319 v4l2_info(&dev->v4l2_dev,
1320 "Device registered as /dev/video%d\n", vfd->num);
1321
1322 platform_set_drvdata(pdev, dev);
1323
1324 dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1325 if (IS_ERR(dev->m2m_dev)) {
1326 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1327 ret = PTR_ERR(dev->m2m_dev);
1328 dev->m2m_dev = NULL;
1329 goto error_dev;
1330 }
1331
1332 #ifdef CONFIG_MEDIA_CONTROLLER
1333 dev->mdev.dev = &pdev->dev;
1334 strscpy(dev->mdev.model, "vim2m", sizeof(dev->mdev.model));
1335 strscpy(dev->mdev.bus_info, "platform:vim2m",
1336 sizeof(dev->mdev.bus_info));
1337 media_device_init(&dev->mdev);
1338 dev->mdev.ops = &m2m_media_ops;
1339 dev->v4l2_dev.mdev = &dev->mdev;
1340 #endif
1341
1342 ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
1343 if (ret) {
1344 v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1345 goto error_m2m;
1346 }
1347
1348 #ifdef CONFIG_MEDIA_CONTROLLER
1349 ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
1350 MEDIA_ENT_F_PROC_VIDEO_SCALER);
1351 if (ret) {
1352 v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem media controller\n");
1353 goto error_v4l2;
1354 }
1355
1356 ret = media_device_register(&dev->mdev);
1357 if (ret) {
1358 v4l2_err(&dev->v4l2_dev, "Failed to register mem2mem media device\n");
1359 goto error_m2m_mc;
1360 }
1361 #endif
1362 return 0;
1363
1364 #ifdef CONFIG_MEDIA_CONTROLLER
1365 error_m2m_mc:
1366 v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1367 #endif
1368 error_v4l2:
1369 video_unregister_device(&dev->vfd);
1370 /* vim2m_device_release called by video_unregister_device to release various objects */
1371 return ret;
1372 error_m2m:
1373 v4l2_m2m_release(dev->m2m_dev);
1374 error_dev:
1375 v4l2_device_unregister(&dev->v4l2_dev);
1376 error_free:
1377 kfree(dev);
1378
1379 return ret;
1380 }
1381
vim2m_remove(struct platform_device * pdev)1382 static void vim2m_remove(struct platform_device *pdev)
1383 {
1384 struct vim2m_dev *dev = platform_get_drvdata(pdev);
1385
1386 v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
1387
1388 #ifdef CONFIG_MEDIA_CONTROLLER
1389 media_device_unregister(&dev->mdev);
1390 v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1391 #endif
1392 video_unregister_device(&dev->vfd);
1393 }
1394
1395 static struct platform_driver vim2m_pdrv = {
1396 .probe = vim2m_probe,
1397 .remove_new = vim2m_remove,
1398 .driver = {
1399 .name = MEM2MEM_NAME,
1400 },
1401 };
1402
vim2m_exit(void)1403 static void __exit vim2m_exit(void)
1404 {
1405 platform_driver_unregister(&vim2m_pdrv);
1406 platform_device_unregister(&vim2m_pdev);
1407 }
1408
vim2m_init(void)1409 static int __init vim2m_init(void)
1410 {
1411 int ret;
1412
1413 ret = platform_device_register(&vim2m_pdev);
1414 if (ret)
1415 return ret;
1416
1417 ret = platform_driver_register(&vim2m_pdrv);
1418 if (ret)
1419 platform_device_unregister(&vim2m_pdev);
1420
1421 return ret;
1422 }
1423
1424 module_init(vim2m_init);
1425 module_exit(vim2m_exit);
1426