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 | |
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 | |
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 | |
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 | |
230 | static inline struct vim2m_ctx *file2ctx(struct file *file) |
231 | { |
232 | return container_of(file->private_data, struct vim2m_ctx, fh); |
233 | } |
234 | |
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 | |
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 | |
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 | |
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 | |
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, type: 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, type: 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(vb: &in_vb->vb2_buf, plane_no: 0); |
465 | p_out = vb2_plane_vaddr(vb: &out_vb->vb2_buf, plane_no: 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(out_vb: in_vb, cap_vb: out_vb, copy_frame_flags: 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, src: p, dst: p_out, |
500 | reverse: 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 | src: p_in_x, dst: &p_out, ypos: y_out, |
538 | reverse: 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 | */ |
565 | static int job_ready(void *priv) |
566 | { |
567 | struct vim2m_ctx *ctx = priv; |
568 | |
569 | if (v4l2_m2m_num_src_bufs_ready(m2m_ctx: ctx->fh.m2m_ctx) < ctx->translen |
570 | || v4l2_m2m_num_dst_bufs_ready(m2m_ctx: 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 | |
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 | */ |
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(m2m_ctx: ctx->fh.m2m_ctx); |
598 | dst_buf = v4l2_m2m_next_dst_buf(m2m_ctx: ctx->fh.m2m_ctx); |
599 | |
600 | /* Apply request controls if any */ |
601 | v4l2_ctrl_request_setup(req: src_buf->vb2_buf.req_obj.req, |
602 | parent: &ctx->hdl); |
603 | |
604 | device_process(ctx, in_vb: src_buf, out_vb: dst_buf); |
605 | |
606 | /* Complete request controls if any */ |
607 | v4l2_ctrl_request_complete(req: src_buf->vb2_buf.req_obj.req, |
608 | parent: &ctx->hdl); |
609 | |
610 | /* Run delayed work, which simulates a hardware irq */ |
611 | schedule_delayed_work(dwork: &ctx->work_run, delay: msecs_to_jiffies(m: ctx->transtime)); |
612 | } |
613 | |
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(m2m_ctx: curr_ctx->fh.m2m_ctx); |
625 | dst_vb = v4l2_m2m_dst_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
626 | |
627 | curr_ctx->num_processed++; |
628 | |
629 | v4l2_m2m_buf_done(buf: src_vb, state: VB2_BUF_STATE_DONE); |
630 | v4l2_m2m_buf_done(buf: dst_vb, state: 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(m2m_dev: vim2m_dev->m2m_dev, m2m_ctx: curr_ctx->fh.m2m_ctx); |
637 | } else { |
638 | device_run(priv: curr_ctx); |
639 | } |
640 | } |
641 | |
642 | /* |
643 | * video ioctls |
644 | */ |
645 | static int vidioc_querycap(struct file *file, void *priv, |
646 | struct v4l2_capability *cap) |
647 | { |
648 | strscpy(p: cap->driver, MEM2MEM_NAME, size: sizeof(cap->driver)); |
649 | strscpy(p: cap->card, MEM2MEM_NAME, size: sizeof(cap->card)); |
650 | snprintf(buf: cap->bus_info, size: sizeof(cap->bus_info), |
651 | fmt: "platform:%s" , MEM2MEM_NAME); |
652 | return 0; |
653 | } |
654 | |
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 | |
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 | |
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 | |
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(fourcc: 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(fourcc: fsize->pixel_format, |
714 | walign: &fsize->stepwise.step_width, |
715 | halign: &fsize->stepwise.step_height); |
716 | return 0; |
717 | } |
718 | |
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(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
725 | if (!vq) |
726 | return -EINVAL; |
727 | |
728 | q_data = get_q_data(ctx, type: 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 | |
746 | static int vidioc_g_fmt_vid_out(struct file *file, void *priv, |
747 | struct v4l2_format *f) |
748 | { |
749 | return vidioc_g_fmt(ctx: file2ctx(file), f); |
750 | } |
751 | |
752 | static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, |
753 | struct v4l2_format *f) |
754 | { |
755 | return vidioc_g_fmt(ctx: file2ctx(file), f); |
756 | } |
757 | |
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(fourcc: f->fmt.pix.pixelformat, walign: &walign, halign: &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 | |
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(fourcc: f->fmt.pix.pixelformat); |
792 | if (!fmt) { |
793 | f->fmt.pix.pixelformat = formats[0].fourcc; |
794 | fmt = find_format(fourcc: 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 | |
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(fourcc: f->fmt.pix.pixelformat); |
817 | if (!fmt) { |
818 | f->fmt.pix.pixelformat = formats[0].fourcc; |
819 | fmt = find_format(fourcc: 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 | |
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(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
839 | if (!vq) |
840 | return -EINVAL; |
841 | |
842 | q_data = get_q_data(ctx, type: f->type); |
843 | if (!q_data) |
844 | return -EINVAL; |
845 | |
846 | if (vb2_is_busy(q: vq)) { |
847 | v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n" , __func__); |
848 | return -EBUSY; |
849 | } |
850 | |
851 | q_data->fmt = find_format(fourcc: 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 | |
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(ctx: file2ctx(file), f); |
879 | } |
880 | |
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(ctx: 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 | |
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 | |
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(q: vq); |
983 | struct vim2m_q_data *q_data; |
984 | unsigned int size, count = *nbuffers; |
985 | |
986 | q_data = get_q_data(ctx, type: 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 | |
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(q: 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 | |
1023 | static int vim2m_buf_prepare(struct vb2_buffer *vb) |
1024 | { |
1025 | struct vim2m_ctx *ctx = vb2_get_drv_priv(q: 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, type: vb->vb2_queue->type); |
1031 | if (!q_data) |
1032 | return -EINVAL; |
1033 | if (vb2_plane_size(vb, plane_no: 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, plane_no: 0, size: q_data->sizeimage); |
1042 | |
1043 | return 0; |
1044 | } |
1045 | |
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(q: vb->vb2_queue); |
1050 | |
1051 | v4l2_m2m_buf_queue(m2m_ctx: ctx->fh.m2m_ctx, vbuf); |
1052 | } |
1053 | |
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, type: 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 | |
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(dwork: &ctx->work_run); |
1075 | |
1076 | for (;;) { |
1077 | if (V4L2_TYPE_IS_OUTPUT(q->type)) |
1078 | vbuf = v4l2_m2m_src_buf_remove(m2m_ctx: ctx->fh.m2m_ctx); |
1079 | else |
1080 | vbuf = v4l2_m2m_dst_buf_remove(m2m_ctx: ctx->fh.m2m_ctx); |
1081 | if (!vbuf) |
1082 | return; |
1083 | v4l2_ctrl_request_complete(req: vbuf->vb2_buf.req_obj.req, |
1084 | parent: &ctx->hdl); |
1085 | v4l2_m2m_buf_done(buf: vbuf, state: VB2_BUF_STATE_ERROR); |
1086 | } |
1087 | } |
1088 | |
1089 | static void vim2m_buf_request_complete(struct vb2_buffer *vb) |
1090 | { |
1091 | struct vim2m_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
1092 | |
1093 | v4l2_ctrl_request_complete(req: vb->req_obj.req, parent: &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 | |
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(q: 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(q: 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 | */ |
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(size: sizeof(*ctx), GFP_KERNEL); |
1174 | if (!ctx) { |
1175 | rc = -ENOMEM; |
1176 | goto open_unlock; |
1177 | } |
1178 | |
1179 | v4l2_fh_init(fh: &ctx->fh, vdev: 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, ops: &vim2m_ctrl_ops, V4L2_CID_HFLIP, min: 0, max: 1, step: 1, def: 0); |
1185 | v4l2_ctrl_new_std(hdl, ops: &vim2m_ctrl_ops, V4L2_CID_VFLIP, min: 0, max: 1, step: 1, def: 0); |
1186 | |
1187 | vim2m_ctrl_trans_time_msec.def = default_transtime; |
1188 | v4l2_ctrl_new_custom(hdl, cfg: &vim2m_ctrl_trans_time_msec, NULL); |
1189 | v4l2_ctrl_new_custom(hdl, cfg: &vim2m_ctrl_trans_num_bufs, NULL); |
1190 | if (hdl->error) { |
1191 | rc = hdl->error; |
1192 | v4l2_ctrl_handler_free(hdl); |
1193 | kfree(objp: 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(m2m_dev: dev->m2m_dev, drv_priv: ctx, queue_init: &queue_init); |
1210 | |
1211 | mutex_init(&ctx->vb_mutex); |
1212 | INIT_DELAYED_WORK(&ctx->work_run, device_work); |
1213 | |
1214 | if (IS_ERR(ptr: ctx->fh.m2m_ctx)) { |
1215 | rc = PTR_ERR(ptr: ctx->fh.m2m_ctx); |
1216 | |
1217 | v4l2_ctrl_handler_free(hdl); |
1218 | v4l2_fh_exit(fh: &ctx->fh); |
1219 | kfree(objp: ctx); |
1220 | goto open_unlock; |
1221 | } |
1222 | |
1223 | v4l2_fh_add(fh: &ctx->fh); |
1224 | atomic_inc(v: &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(lock: &dev->dev_mutex); |
1231 | return rc; |
1232 | } |
1233 | |
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(fh: &ctx->fh); |
1242 | v4l2_fh_exit(fh: &ctx->fh); |
1243 | v4l2_ctrl_handler_free(hdl: &ctx->hdl); |
1244 | mutex_lock(&dev->dev_mutex); |
1245 | v4l2_m2m_ctx_release(m2m_ctx: ctx->fh.m2m_ctx); |
1246 | mutex_unlock(lock: &dev->dev_mutex); |
1247 | kfree(objp: ctx); |
1248 | |
1249 | atomic_dec(v: &dev->num_inst); |
1250 | |
1251 | return 0; |
1252 | } |
1253 | |
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(v4l2_dev: &dev->v4l2_dev); |
1259 | v4l2_m2m_release(m2m_dev: dev->m2m_dev); |
1260 | #ifdef CONFIG_MEDIA_CONTROLLER |
1261 | media_device_cleanup(mdev: &dev->mdev); |
1262 | #endif |
1263 | kfree(objp: 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 | |
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(size: sizeof(*dev), GFP_KERNEL); |
1303 | if (!dev) |
1304 | return -ENOMEM; |
1305 | |
1306 | ret = v4l2_device_register(dev: &pdev->dev, v4l2_dev: &dev->v4l2_dev); |
1307 | if (ret) |
1308 | goto error_free; |
1309 | |
1310 | atomic_set(v: &dev->num_inst, i: 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(vdev: vfd, data: dev); |
1319 | v4l2_info(&dev->v4l2_dev, |
1320 | "Device registered as /dev/video%d\n" , vfd->num); |
1321 | |
1322 | platform_set_drvdata(pdev, data: dev); |
1323 | |
1324 | dev->m2m_dev = v4l2_m2m_init(m2m_ops: &m2m_ops); |
1325 | if (IS_ERR(ptr: dev->m2m_dev)) { |
1326 | v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n" ); |
1327 | ret = PTR_ERR(ptr: 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(p: dev->mdev.model, q: "vim2m" , size: sizeof(dev->mdev.model)); |
1335 | strscpy(p: dev->mdev.bus_info, q: "platform:vim2m" , |
1336 | size: sizeof(dev->mdev.bus_info)); |
1337 | media_device_init(mdev: &dev->mdev); |
1338 | dev->mdev.ops = &m2m_media_ops; |
1339 | dev->v4l2_dev.mdev = &dev->mdev; |
1340 | #endif |
1341 | |
1342 | ret = video_register_device(vdev: vfd, type: VFL_TYPE_VIDEO, nr: 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(m2m_dev: dev->m2m_dev, vdev: 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(m2m_dev: dev->m2m_dev); |
1367 | #endif |
1368 | error_v4l2: |
1369 | video_unregister_device(vdev: &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(m2m_dev: dev->m2m_dev); |
1374 | error_dev: |
1375 | v4l2_device_unregister(v4l2_dev: &dev->v4l2_dev); |
1376 | error_free: |
1377 | kfree(objp: dev); |
1378 | |
1379 | return ret; |
1380 | } |
1381 | |
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(mdev: &dev->mdev); |
1390 | v4l2_m2m_unregister_media_controller(m2m_dev: dev->m2m_dev); |
1391 | #endif |
1392 | video_unregister_device(vdev: &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 | |
1403 | static void __exit vim2m_exit(void) |
1404 | { |
1405 | platform_driver_unregister(&vim2m_pdrv); |
1406 | platform_device_unregister(&vim2m_pdev); |
1407 | } |
1408 | |
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 | |