1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * V4L2 deinterlacing support. |
4 | * |
5 | * Copyright (c) 2012 Vista Silicon S.L. |
6 | * Javier Martin <javier.martin@vista-silicon.com> |
7 | */ |
8 | |
9 | #include <linux/module.h> |
10 | #include <linux/slab.h> |
11 | #include <linux/interrupt.h> |
12 | #include <linux/dmaengine.h> |
13 | #include <linux/platform_device.h> |
14 | |
15 | #include <media/v4l2-mem2mem.h> |
16 | #include <media/v4l2-device.h> |
17 | #include <media/v4l2-ioctl.h> |
18 | #include <media/videobuf2-dma-contig.h> |
19 | |
20 | #define MEM2MEM_TEST_MODULE_NAME "mem2mem-deinterlace" |
21 | |
22 | MODULE_DESCRIPTION("mem2mem device which supports deinterlacing using dmaengine" ); |
23 | MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com" ); |
24 | MODULE_LICENSE("GPL" ); |
25 | MODULE_VERSION("0.0.1" ); |
26 | |
27 | static bool debug; |
28 | module_param(debug, bool, 0644); |
29 | |
30 | /* Flags that indicate a format can be used for capture/output */ |
31 | #define MEM2MEM_CAPTURE (1 << 0) |
32 | #define MEM2MEM_OUTPUT (1 << 1) |
33 | |
34 | #define MEM2MEM_NAME "m2m-deinterlace" |
35 | |
36 | #define dprintk(dev, fmt, arg...) \ |
37 | v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg) |
38 | |
39 | struct deinterlace_fmt { |
40 | u32 fourcc; |
41 | /* Types the format can be used for */ |
42 | u32 types; |
43 | }; |
44 | |
45 | static struct deinterlace_fmt formats[] = { |
46 | { |
47 | .fourcc = V4L2_PIX_FMT_YUV420, |
48 | .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, |
49 | }, |
50 | { |
51 | .fourcc = V4L2_PIX_FMT_YUYV, |
52 | .types = MEM2MEM_CAPTURE | MEM2MEM_OUTPUT, |
53 | }, |
54 | }; |
55 | |
56 | #define NUM_FORMATS ARRAY_SIZE(formats) |
57 | |
58 | /* Per-queue, driver-specific private data */ |
59 | struct deinterlace_q_data { |
60 | unsigned int width; |
61 | unsigned int height; |
62 | unsigned int sizeimage; |
63 | struct deinterlace_fmt *fmt; |
64 | enum v4l2_field field; |
65 | }; |
66 | |
67 | enum { |
68 | V4L2_M2M_SRC = 0, |
69 | V4L2_M2M_DST = 1, |
70 | }; |
71 | |
72 | enum { |
73 | YUV420_DMA_Y_ODD, |
74 | YUV420_DMA_Y_EVEN, |
75 | YUV420_DMA_U_ODD, |
76 | YUV420_DMA_U_EVEN, |
77 | YUV420_DMA_V_ODD, |
78 | YUV420_DMA_V_EVEN, |
79 | YUV420_DMA_Y_ODD_DOUBLING, |
80 | YUV420_DMA_U_ODD_DOUBLING, |
81 | YUV420_DMA_V_ODD_DOUBLING, |
82 | YUYV_DMA_ODD, |
83 | YUYV_DMA_EVEN, |
84 | YUYV_DMA_EVEN_DOUBLING, |
85 | }; |
86 | |
87 | /* Source and destination queue data */ |
88 | static struct deinterlace_q_data q_data[2]; |
89 | |
90 | static struct deinterlace_q_data *get_q_data(enum v4l2_buf_type type) |
91 | { |
92 | switch (type) { |
93 | case V4L2_BUF_TYPE_VIDEO_OUTPUT: |
94 | return &q_data[V4L2_M2M_SRC]; |
95 | case V4L2_BUF_TYPE_VIDEO_CAPTURE: |
96 | return &q_data[V4L2_M2M_DST]; |
97 | default: |
98 | BUG(); |
99 | } |
100 | return NULL; |
101 | } |
102 | |
103 | static struct deinterlace_fmt *find_format(struct v4l2_format *f) |
104 | { |
105 | struct deinterlace_fmt *fmt; |
106 | unsigned int k; |
107 | |
108 | for (k = 0; k < NUM_FORMATS; k++) { |
109 | fmt = &formats[k]; |
110 | if ((fmt->types & f->type) && |
111 | (fmt->fourcc == f->fmt.pix.pixelformat)) |
112 | break; |
113 | } |
114 | |
115 | if (k == NUM_FORMATS) |
116 | return NULL; |
117 | |
118 | return &formats[k]; |
119 | } |
120 | |
121 | struct deinterlace_dev { |
122 | struct v4l2_device v4l2_dev; |
123 | struct video_device vfd; |
124 | |
125 | atomic_t busy; |
126 | struct mutex dev_mutex; |
127 | spinlock_t irqlock; |
128 | |
129 | struct dma_chan *dma_chan; |
130 | |
131 | struct v4l2_m2m_dev *m2m_dev; |
132 | }; |
133 | |
134 | struct deinterlace_ctx { |
135 | struct v4l2_fh fh; |
136 | struct deinterlace_dev *dev; |
137 | |
138 | /* Abort requested by m2m */ |
139 | int aborting; |
140 | enum v4l2_colorspace colorspace; |
141 | dma_cookie_t cookie; |
142 | struct dma_interleaved_template *xt; |
143 | }; |
144 | |
145 | /* |
146 | * mem2mem callbacks |
147 | */ |
148 | static int deinterlace_job_ready(void *priv) |
149 | { |
150 | struct deinterlace_ctx *ctx = priv; |
151 | struct deinterlace_dev *pcdev = ctx->dev; |
152 | |
153 | if (v4l2_m2m_num_src_bufs_ready(m2m_ctx: ctx->fh.m2m_ctx) > 0 && |
154 | v4l2_m2m_num_dst_bufs_ready(m2m_ctx: ctx->fh.m2m_ctx) > 0 && |
155 | !atomic_read(v: &ctx->dev->busy)) { |
156 | dprintk(pcdev, "Task ready\n" ); |
157 | return 1; |
158 | } |
159 | |
160 | dprintk(pcdev, "Task not ready to run\n" ); |
161 | |
162 | return 0; |
163 | } |
164 | |
165 | static void deinterlace_job_abort(void *priv) |
166 | { |
167 | struct deinterlace_ctx *ctx = priv; |
168 | struct deinterlace_dev *pcdev = ctx->dev; |
169 | |
170 | ctx->aborting = 1; |
171 | |
172 | dprintk(pcdev, "Aborting task\n" ); |
173 | |
174 | v4l2_m2m_job_finish(m2m_dev: pcdev->m2m_dev, m2m_ctx: ctx->fh.m2m_ctx); |
175 | } |
176 | |
177 | static void dma_callback(void *data) |
178 | { |
179 | struct deinterlace_ctx *curr_ctx = data; |
180 | struct deinterlace_dev *pcdev = curr_ctx->dev; |
181 | struct vb2_v4l2_buffer *src_vb, *dst_vb; |
182 | |
183 | atomic_set(v: &pcdev->busy, i: 0); |
184 | |
185 | src_vb = v4l2_m2m_src_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
186 | dst_vb = v4l2_m2m_dst_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
187 | |
188 | dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp; |
189 | dst_vb->flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK; |
190 | dst_vb->flags |= |
191 | src_vb->flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK; |
192 | dst_vb->timecode = src_vb->timecode; |
193 | |
194 | v4l2_m2m_buf_done(buf: src_vb, state: VB2_BUF_STATE_DONE); |
195 | v4l2_m2m_buf_done(buf: dst_vb, state: VB2_BUF_STATE_DONE); |
196 | |
197 | v4l2_m2m_job_finish(m2m_dev: pcdev->m2m_dev, m2m_ctx: curr_ctx->fh.m2m_ctx); |
198 | |
199 | dprintk(pcdev, "dma transfers completed.\n" ); |
200 | } |
201 | |
202 | static void deinterlace_issue_dma(struct deinterlace_ctx *ctx, int op, |
203 | int do_callback) |
204 | { |
205 | struct deinterlace_q_data *s_q_data; |
206 | struct vb2_v4l2_buffer *src_buf, *dst_buf; |
207 | struct deinterlace_dev *pcdev = ctx->dev; |
208 | struct dma_chan *chan = pcdev->dma_chan; |
209 | struct dma_device *dmadev = chan->device; |
210 | struct dma_async_tx_descriptor *tx; |
211 | unsigned int s_width, s_height; |
212 | unsigned int s_size; |
213 | dma_addr_t p_in, p_out; |
214 | enum dma_ctrl_flags flags; |
215 | |
216 | src_buf = v4l2_m2m_next_src_buf(m2m_ctx: ctx->fh.m2m_ctx); |
217 | dst_buf = v4l2_m2m_next_dst_buf(m2m_ctx: ctx->fh.m2m_ctx); |
218 | |
219 | s_q_data = get_q_data(type: V4L2_BUF_TYPE_VIDEO_OUTPUT); |
220 | s_width = s_q_data->width; |
221 | s_height = s_q_data->height; |
222 | s_size = s_width * s_height; |
223 | |
224 | p_in = (dma_addr_t)vb2_dma_contig_plane_dma_addr(vb: &src_buf->vb2_buf, plane_no: 0); |
225 | p_out = (dma_addr_t)vb2_dma_contig_plane_dma_addr(vb: &dst_buf->vb2_buf, |
226 | plane_no: 0); |
227 | if (!p_in || !p_out) { |
228 | v4l2_err(&pcdev->v4l2_dev, |
229 | "Acquiring kernel pointers to buffers failed\n" ); |
230 | return; |
231 | } |
232 | |
233 | switch (op) { |
234 | case YUV420_DMA_Y_ODD: |
235 | ctx->xt->numf = s_height / 2; |
236 | ctx->xt->sgl[0].size = s_width; |
237 | ctx->xt->sgl[0].icg = s_width; |
238 | ctx->xt->src_start = p_in; |
239 | ctx->xt->dst_start = p_out; |
240 | break; |
241 | case YUV420_DMA_Y_EVEN: |
242 | ctx->xt->numf = s_height / 2; |
243 | ctx->xt->sgl[0].size = s_width; |
244 | ctx->xt->sgl[0].icg = s_width; |
245 | ctx->xt->src_start = p_in + s_size / 2; |
246 | ctx->xt->dst_start = p_out + s_width; |
247 | break; |
248 | case YUV420_DMA_U_ODD: |
249 | ctx->xt->numf = s_height / 4; |
250 | ctx->xt->sgl[0].size = s_width / 2; |
251 | ctx->xt->sgl[0].icg = s_width / 2; |
252 | ctx->xt->src_start = p_in + s_size; |
253 | ctx->xt->dst_start = p_out + s_size; |
254 | break; |
255 | case YUV420_DMA_U_EVEN: |
256 | ctx->xt->numf = s_height / 4; |
257 | ctx->xt->sgl[0].size = s_width / 2; |
258 | ctx->xt->sgl[0].icg = s_width / 2; |
259 | ctx->xt->src_start = p_in + (9 * s_size) / 8; |
260 | ctx->xt->dst_start = p_out + s_size + s_width / 2; |
261 | break; |
262 | case YUV420_DMA_V_ODD: |
263 | ctx->xt->numf = s_height / 4; |
264 | ctx->xt->sgl[0].size = s_width / 2; |
265 | ctx->xt->sgl[0].icg = s_width / 2; |
266 | ctx->xt->src_start = p_in + (5 * s_size) / 4; |
267 | ctx->xt->dst_start = p_out + (5 * s_size) / 4; |
268 | break; |
269 | case YUV420_DMA_V_EVEN: |
270 | ctx->xt->numf = s_height / 4; |
271 | ctx->xt->sgl[0].size = s_width / 2; |
272 | ctx->xt->sgl[0].icg = s_width / 2; |
273 | ctx->xt->src_start = p_in + (11 * s_size) / 8; |
274 | ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2; |
275 | break; |
276 | case YUV420_DMA_Y_ODD_DOUBLING: |
277 | ctx->xt->numf = s_height / 2; |
278 | ctx->xt->sgl[0].size = s_width; |
279 | ctx->xt->sgl[0].icg = s_width; |
280 | ctx->xt->src_start = p_in; |
281 | ctx->xt->dst_start = p_out + s_width; |
282 | break; |
283 | case YUV420_DMA_U_ODD_DOUBLING: |
284 | ctx->xt->numf = s_height / 4; |
285 | ctx->xt->sgl[0].size = s_width / 2; |
286 | ctx->xt->sgl[0].icg = s_width / 2; |
287 | ctx->xt->src_start = p_in + s_size; |
288 | ctx->xt->dst_start = p_out + s_size + s_width / 2; |
289 | break; |
290 | case YUV420_DMA_V_ODD_DOUBLING: |
291 | ctx->xt->numf = s_height / 4; |
292 | ctx->xt->sgl[0].size = s_width / 2; |
293 | ctx->xt->sgl[0].icg = s_width / 2; |
294 | ctx->xt->src_start = p_in + (5 * s_size) / 4; |
295 | ctx->xt->dst_start = p_out + (5 * s_size) / 4 + s_width / 2; |
296 | break; |
297 | case YUYV_DMA_ODD: |
298 | ctx->xt->numf = s_height / 2; |
299 | ctx->xt->sgl[0].size = s_width * 2; |
300 | ctx->xt->sgl[0].icg = s_width * 2; |
301 | ctx->xt->src_start = p_in; |
302 | ctx->xt->dst_start = p_out; |
303 | break; |
304 | case YUYV_DMA_EVEN: |
305 | ctx->xt->numf = s_height / 2; |
306 | ctx->xt->sgl[0].size = s_width * 2; |
307 | ctx->xt->sgl[0].icg = s_width * 2; |
308 | ctx->xt->src_start = p_in + s_size; |
309 | ctx->xt->dst_start = p_out + s_width * 2; |
310 | break; |
311 | case YUYV_DMA_EVEN_DOUBLING: |
312 | default: |
313 | ctx->xt->numf = s_height / 2; |
314 | ctx->xt->sgl[0].size = s_width * 2; |
315 | ctx->xt->sgl[0].icg = s_width * 2; |
316 | ctx->xt->src_start = p_in; |
317 | ctx->xt->dst_start = p_out + s_width * 2; |
318 | break; |
319 | } |
320 | |
321 | /* Common parameters for al transfers */ |
322 | ctx->xt->frame_size = 1; |
323 | ctx->xt->dir = DMA_MEM_TO_MEM; |
324 | ctx->xt->src_sgl = false; |
325 | ctx->xt->dst_sgl = true; |
326 | flags = DMA_CTRL_ACK | DMA_PREP_INTERRUPT; |
327 | |
328 | tx = dmadev->device_prep_interleaved_dma(chan, ctx->xt, flags); |
329 | if (tx == NULL) { |
330 | v4l2_warn(&pcdev->v4l2_dev, "DMA interleaved prep error\n" ); |
331 | return; |
332 | } |
333 | |
334 | if (do_callback) { |
335 | tx->callback = dma_callback; |
336 | tx->callback_param = ctx; |
337 | } |
338 | |
339 | ctx->cookie = dmaengine_submit(desc: tx); |
340 | if (dma_submit_error(cookie: ctx->cookie)) { |
341 | v4l2_warn(&pcdev->v4l2_dev, |
342 | "DMA submit error %d with src=0x%x dst=0x%x len=0x%x\n" , |
343 | ctx->cookie, (unsigned)p_in, (unsigned)p_out, |
344 | s_size * 3/2); |
345 | return; |
346 | } |
347 | |
348 | dma_async_issue_pending(chan); |
349 | } |
350 | |
351 | static void deinterlace_device_run(void *priv) |
352 | { |
353 | struct deinterlace_ctx *ctx = priv; |
354 | struct deinterlace_q_data *dst_q_data; |
355 | |
356 | atomic_set(v: &ctx->dev->busy, i: 1); |
357 | |
358 | dprintk(ctx->dev, "%s: DMA try issue.\n" , __func__); |
359 | |
360 | dst_q_data = get_q_data(type: V4L2_BUF_TYPE_VIDEO_CAPTURE); |
361 | |
362 | /* |
363 | * 4 possible field conversions are possible at the moment: |
364 | * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_INTERLACED_TB: |
365 | * two separate fields in the same input buffer are interlaced |
366 | * in the output buffer using weaving. Top field comes first. |
367 | * V4L2_FIELD_SEQ_TB --> V4L2_FIELD_NONE: |
368 | * top field from the input buffer is copied to the output buffer |
369 | * using line doubling. Bottom field from the input buffer is discarded. |
370 | * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_INTERLACED_BT: |
371 | * two separate fields in the same input buffer are interlaced |
372 | * in the output buffer using weaving. Bottom field comes first. |
373 | * V4L2_FIELD_SEQ_BT --> V4L2_FIELD_NONE: |
374 | * bottom field from the input buffer is copied to the output buffer |
375 | * using line doubling. Top field from the input buffer is discarded. |
376 | */ |
377 | switch (dst_q_data->fmt->fourcc) { |
378 | case V4L2_PIX_FMT_YUV420: |
379 | switch (dst_q_data->field) { |
380 | case V4L2_FIELD_INTERLACED_TB: |
381 | case V4L2_FIELD_INTERLACED_BT: |
382 | dprintk(ctx->dev, "%s: yuv420 interlaced tb.\n" , |
383 | __func__); |
384 | deinterlace_issue_dma(ctx, op: YUV420_DMA_Y_ODD, do_callback: 0); |
385 | deinterlace_issue_dma(ctx, op: YUV420_DMA_Y_EVEN, do_callback: 0); |
386 | deinterlace_issue_dma(ctx, op: YUV420_DMA_U_ODD, do_callback: 0); |
387 | deinterlace_issue_dma(ctx, op: YUV420_DMA_U_EVEN, do_callback: 0); |
388 | deinterlace_issue_dma(ctx, op: YUV420_DMA_V_ODD, do_callback: 0); |
389 | deinterlace_issue_dma(ctx, op: YUV420_DMA_V_EVEN, do_callback: 1); |
390 | break; |
391 | case V4L2_FIELD_NONE: |
392 | default: |
393 | dprintk(ctx->dev, "%s: yuv420 interlaced line doubling.\n" , |
394 | __func__); |
395 | deinterlace_issue_dma(ctx, op: YUV420_DMA_Y_ODD, do_callback: 0); |
396 | deinterlace_issue_dma(ctx, op: YUV420_DMA_Y_ODD_DOUBLING, do_callback: 0); |
397 | deinterlace_issue_dma(ctx, op: YUV420_DMA_U_ODD, do_callback: 0); |
398 | deinterlace_issue_dma(ctx, op: YUV420_DMA_U_ODD_DOUBLING, do_callback: 0); |
399 | deinterlace_issue_dma(ctx, op: YUV420_DMA_V_ODD, do_callback: 0); |
400 | deinterlace_issue_dma(ctx, op: YUV420_DMA_V_ODD_DOUBLING, do_callback: 1); |
401 | break; |
402 | } |
403 | break; |
404 | case V4L2_PIX_FMT_YUYV: |
405 | default: |
406 | switch (dst_q_data->field) { |
407 | case V4L2_FIELD_INTERLACED_TB: |
408 | case V4L2_FIELD_INTERLACED_BT: |
409 | dprintk(ctx->dev, "%s: yuyv interlaced_tb.\n" , |
410 | __func__); |
411 | deinterlace_issue_dma(ctx, op: YUYV_DMA_ODD, do_callback: 0); |
412 | deinterlace_issue_dma(ctx, op: YUYV_DMA_EVEN, do_callback: 1); |
413 | break; |
414 | case V4L2_FIELD_NONE: |
415 | default: |
416 | dprintk(ctx->dev, "%s: yuyv interlaced line doubling.\n" , |
417 | __func__); |
418 | deinterlace_issue_dma(ctx, op: YUYV_DMA_ODD, do_callback: 0); |
419 | deinterlace_issue_dma(ctx, op: YUYV_DMA_EVEN_DOUBLING, do_callback: 1); |
420 | break; |
421 | } |
422 | break; |
423 | } |
424 | |
425 | dprintk(ctx->dev, "%s: DMA issue done.\n" , __func__); |
426 | } |
427 | |
428 | /* |
429 | * video ioctls |
430 | */ |
431 | static int vidioc_querycap(struct file *file, void *priv, |
432 | struct v4l2_capability *cap) |
433 | { |
434 | strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver)); |
435 | strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card)); |
436 | strscpy(cap->bus_info, MEM2MEM_NAME, sizeof(cap->bus_info)); |
437 | return 0; |
438 | } |
439 | |
440 | static int enum_fmt(struct v4l2_fmtdesc *f, u32 type) |
441 | { |
442 | int i, num; |
443 | struct deinterlace_fmt *fmt; |
444 | |
445 | num = 0; |
446 | |
447 | for (i = 0; i < NUM_FORMATS; ++i) { |
448 | if (formats[i].types & type) { |
449 | /* index-th format of type type found ? */ |
450 | if (num == f->index) |
451 | break; |
452 | /* Correct type but haven't reached our index yet, |
453 | * just increment per-type index */ |
454 | ++num; |
455 | } |
456 | } |
457 | |
458 | if (i < NUM_FORMATS) { |
459 | /* Format found */ |
460 | fmt = &formats[i]; |
461 | f->pixelformat = fmt->fourcc; |
462 | return 0; |
463 | } |
464 | |
465 | /* Format not found */ |
466 | return -EINVAL; |
467 | } |
468 | |
469 | static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, |
470 | struct v4l2_fmtdesc *f) |
471 | { |
472 | return enum_fmt(f, MEM2MEM_CAPTURE); |
473 | } |
474 | |
475 | static int vidioc_enum_fmt_vid_out(struct file *file, void *priv, |
476 | struct v4l2_fmtdesc *f) |
477 | { |
478 | return enum_fmt(f, MEM2MEM_OUTPUT); |
479 | } |
480 | |
481 | static int vidioc_g_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f) |
482 | { |
483 | struct vb2_queue *vq; |
484 | struct deinterlace_q_data *q_data; |
485 | |
486 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
487 | if (!vq) |
488 | return -EINVAL; |
489 | |
490 | q_data = get_q_data(type: f->type); |
491 | |
492 | f->fmt.pix.width = q_data->width; |
493 | f->fmt.pix.height = q_data->height; |
494 | f->fmt.pix.field = q_data->field; |
495 | f->fmt.pix.pixelformat = q_data->fmt->fourcc; |
496 | |
497 | switch (q_data->fmt->fourcc) { |
498 | case V4L2_PIX_FMT_YUV420: |
499 | f->fmt.pix.bytesperline = q_data->width * 3 / 2; |
500 | break; |
501 | case V4L2_PIX_FMT_YUYV: |
502 | default: |
503 | f->fmt.pix.bytesperline = q_data->width * 2; |
504 | } |
505 | |
506 | f->fmt.pix.sizeimage = q_data->sizeimage; |
507 | f->fmt.pix.colorspace = ctx->colorspace; |
508 | |
509 | return 0; |
510 | } |
511 | |
512 | static int vidioc_g_fmt_vid_out(struct file *file, void *priv, |
513 | struct v4l2_format *f) |
514 | { |
515 | return vidioc_g_fmt(ctx: priv, f); |
516 | } |
517 | |
518 | static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, |
519 | struct v4l2_format *f) |
520 | { |
521 | return vidioc_g_fmt(ctx: priv, f); |
522 | } |
523 | |
524 | static int vidioc_try_fmt(struct v4l2_format *f, struct deinterlace_fmt *fmt) |
525 | { |
526 | switch (f->fmt.pix.pixelformat) { |
527 | case V4L2_PIX_FMT_YUV420: |
528 | f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2; |
529 | break; |
530 | case V4L2_PIX_FMT_YUYV: |
531 | default: |
532 | f->fmt.pix.bytesperline = f->fmt.pix.width * 2; |
533 | } |
534 | f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; |
535 | |
536 | return 0; |
537 | } |
538 | |
539 | static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, |
540 | struct v4l2_format *f) |
541 | { |
542 | struct deinterlace_fmt *fmt; |
543 | struct deinterlace_ctx *ctx = priv; |
544 | |
545 | fmt = find_format(f); |
546 | if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) |
547 | f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; |
548 | |
549 | f->fmt.pix.colorspace = ctx->colorspace; |
550 | |
551 | if (f->fmt.pix.field != V4L2_FIELD_INTERLACED_TB && |
552 | f->fmt.pix.field != V4L2_FIELD_INTERLACED_BT && |
553 | f->fmt.pix.field != V4L2_FIELD_NONE) |
554 | f->fmt.pix.field = V4L2_FIELD_INTERLACED_TB; |
555 | |
556 | return vidioc_try_fmt(f, fmt); |
557 | } |
558 | |
559 | static int vidioc_try_fmt_vid_out(struct file *file, void *priv, |
560 | struct v4l2_format *f) |
561 | { |
562 | struct deinterlace_fmt *fmt; |
563 | |
564 | fmt = find_format(f); |
565 | if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) |
566 | f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUV420; |
567 | |
568 | if (!f->fmt.pix.colorspace) |
569 | f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709; |
570 | |
571 | if (f->fmt.pix.field != V4L2_FIELD_SEQ_TB && |
572 | f->fmt.pix.field != V4L2_FIELD_SEQ_BT) |
573 | f->fmt.pix.field = V4L2_FIELD_SEQ_TB; |
574 | |
575 | return vidioc_try_fmt(f, fmt); |
576 | } |
577 | |
578 | static int vidioc_s_fmt(struct deinterlace_ctx *ctx, struct v4l2_format *f) |
579 | { |
580 | struct deinterlace_q_data *q_data; |
581 | struct vb2_queue *vq; |
582 | |
583 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
584 | if (!vq) |
585 | return -EINVAL; |
586 | |
587 | q_data = get_q_data(type: f->type); |
588 | if (!q_data) |
589 | return -EINVAL; |
590 | |
591 | if (vb2_is_busy(q: vq)) { |
592 | v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n" , __func__); |
593 | return -EBUSY; |
594 | } |
595 | |
596 | q_data->fmt = find_format(f); |
597 | if (!q_data->fmt) { |
598 | v4l2_err(&ctx->dev->v4l2_dev, |
599 | "Couldn't set format type %d, wxh: %dx%d. fmt: %d, field: %d\n" , |
600 | f->type, f->fmt.pix.width, f->fmt.pix.height, |
601 | f->fmt.pix.pixelformat, f->fmt.pix.field); |
602 | return -EINVAL; |
603 | } |
604 | |
605 | q_data->width = f->fmt.pix.width; |
606 | q_data->height = f->fmt.pix.height; |
607 | q_data->field = f->fmt.pix.field; |
608 | |
609 | switch (f->fmt.pix.pixelformat) { |
610 | case V4L2_PIX_FMT_YUV420: |
611 | f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2; |
612 | q_data->sizeimage = (q_data->width * q_data->height * 3) / 2; |
613 | break; |
614 | case V4L2_PIX_FMT_YUYV: |
615 | default: |
616 | f->fmt.pix.bytesperline = f->fmt.pix.width * 2; |
617 | q_data->sizeimage = q_data->width * q_data->height * 2; |
618 | } |
619 | |
620 | dprintk(ctx->dev, |
621 | "Setting format for type %d, wxh: %dx%d, fmt: %d, field: %d\n" , |
622 | f->type, q_data->width, q_data->height, q_data->fmt->fourcc, |
623 | q_data->field); |
624 | |
625 | return 0; |
626 | } |
627 | |
628 | static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, |
629 | struct v4l2_format *f) |
630 | { |
631 | int ret; |
632 | |
633 | ret = vidioc_try_fmt_vid_cap(file, priv, f); |
634 | if (ret) |
635 | return ret; |
636 | return vidioc_s_fmt(ctx: priv, f); |
637 | } |
638 | |
639 | static int vidioc_s_fmt_vid_out(struct file *file, void *priv, |
640 | struct v4l2_format *f) |
641 | { |
642 | struct deinterlace_ctx *ctx = priv; |
643 | int ret; |
644 | |
645 | ret = vidioc_try_fmt_vid_out(file, priv, f); |
646 | if (ret) |
647 | return ret; |
648 | |
649 | ret = vidioc_s_fmt(ctx: priv, f); |
650 | if (!ret) |
651 | ctx->colorspace = f->fmt.pix.colorspace; |
652 | |
653 | return ret; |
654 | } |
655 | |
656 | static int vidioc_streamon(struct file *file, void *priv, |
657 | enum v4l2_buf_type type) |
658 | { |
659 | struct deinterlace_q_data *s_q_data, *d_q_data; |
660 | struct deinterlace_ctx *ctx = priv; |
661 | |
662 | s_q_data = get_q_data(type: V4L2_BUF_TYPE_VIDEO_OUTPUT); |
663 | d_q_data = get_q_data(type: V4L2_BUF_TYPE_VIDEO_CAPTURE); |
664 | |
665 | /* Check that src and dst queues have the same pix format */ |
666 | if (s_q_data->fmt->fourcc != d_q_data->fmt->fourcc) { |
667 | v4l2_err(&ctx->dev->v4l2_dev, |
668 | "src and dst formats don't match.\n" ); |
669 | return -EINVAL; |
670 | } |
671 | |
672 | /* Check that input and output deinterlacing types are compatible */ |
673 | switch (s_q_data->field) { |
674 | case V4L2_FIELD_SEQ_BT: |
675 | if (d_q_data->field != V4L2_FIELD_NONE && |
676 | d_q_data->field != V4L2_FIELD_INTERLACED_BT) { |
677 | v4l2_err(&ctx->dev->v4l2_dev, |
678 | "src and dst field conversion [(%d)->(%d)] not supported.\n" , |
679 | s_q_data->field, d_q_data->field); |
680 | return -EINVAL; |
681 | } |
682 | break; |
683 | case V4L2_FIELD_SEQ_TB: |
684 | if (d_q_data->field != V4L2_FIELD_NONE && |
685 | d_q_data->field != V4L2_FIELD_INTERLACED_TB) { |
686 | v4l2_err(&ctx->dev->v4l2_dev, |
687 | "src and dst field conversion [(%d)->(%d)] not supported.\n" , |
688 | s_q_data->field, d_q_data->field); |
689 | return -EINVAL; |
690 | } |
691 | break; |
692 | default: |
693 | return -EINVAL; |
694 | } |
695 | |
696 | return v4l2_m2m_streamon(file, m2m_ctx: ctx->fh.m2m_ctx, type); |
697 | } |
698 | |
699 | static const struct v4l2_ioctl_ops deinterlace_ioctl_ops = { |
700 | .vidioc_querycap = vidioc_querycap, |
701 | |
702 | .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, |
703 | .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, |
704 | .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, |
705 | .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, |
706 | |
707 | .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out, |
708 | .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out, |
709 | .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out, |
710 | .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out, |
711 | |
712 | .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs, |
713 | .vidioc_querybuf = v4l2_m2m_ioctl_querybuf, |
714 | .vidioc_qbuf = v4l2_m2m_ioctl_qbuf, |
715 | .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf, |
716 | .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf, |
717 | .vidioc_expbuf = v4l2_m2m_ioctl_expbuf, |
718 | |
719 | .vidioc_streamon = vidioc_streamon, |
720 | .vidioc_streamoff = v4l2_m2m_ioctl_streamoff, |
721 | }; |
722 | |
723 | |
724 | /* |
725 | * Queue operations |
726 | */ |
727 | struct vb2_dc_conf { |
728 | struct device *dev; |
729 | }; |
730 | |
731 | static int deinterlace_queue_setup(struct vb2_queue *vq, |
732 | unsigned int *nbuffers, unsigned int *nplanes, |
733 | unsigned int sizes[], struct device *alloc_devs[]) |
734 | { |
735 | struct deinterlace_ctx *ctx = vb2_get_drv_priv(q: vq); |
736 | struct deinterlace_q_data *q_data; |
737 | unsigned int size, count = *nbuffers; |
738 | |
739 | q_data = get_q_data(type: vq->type); |
740 | |
741 | switch (q_data->fmt->fourcc) { |
742 | case V4L2_PIX_FMT_YUV420: |
743 | size = q_data->width * q_data->height * 3 / 2; |
744 | break; |
745 | case V4L2_PIX_FMT_YUYV: |
746 | default: |
747 | size = q_data->width * q_data->height * 2; |
748 | } |
749 | |
750 | *nplanes = 1; |
751 | *nbuffers = count; |
752 | sizes[0] = size; |
753 | |
754 | dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n" , count, size); |
755 | |
756 | return 0; |
757 | } |
758 | |
759 | static int deinterlace_buf_prepare(struct vb2_buffer *vb) |
760 | { |
761 | struct deinterlace_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
762 | struct deinterlace_q_data *q_data; |
763 | |
764 | dprintk(ctx->dev, "type: %d\n" , vb->vb2_queue->type); |
765 | |
766 | q_data = get_q_data(type: vb->vb2_queue->type); |
767 | |
768 | if (vb2_plane_size(vb, plane_no: 0) < q_data->sizeimage) { |
769 | dprintk(ctx->dev, "%s data will not fit into plane (%lu < %lu)\n" , |
770 | __func__, vb2_plane_size(vb, 0), (long)q_data->sizeimage); |
771 | return -EINVAL; |
772 | } |
773 | |
774 | vb2_set_plane_payload(vb, plane_no: 0, size: q_data->sizeimage); |
775 | |
776 | return 0; |
777 | } |
778 | |
779 | static void deinterlace_buf_queue(struct vb2_buffer *vb) |
780 | { |
781 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); |
782 | struct deinterlace_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
783 | |
784 | v4l2_m2m_buf_queue(m2m_ctx: ctx->fh.m2m_ctx, vbuf); |
785 | } |
786 | |
787 | static const struct vb2_ops deinterlace_qops = { |
788 | .queue_setup = deinterlace_queue_setup, |
789 | .buf_prepare = deinterlace_buf_prepare, |
790 | .buf_queue = deinterlace_buf_queue, |
791 | .wait_prepare = vb2_ops_wait_prepare, |
792 | .wait_finish = vb2_ops_wait_finish, |
793 | }; |
794 | |
795 | static int queue_init(void *priv, struct vb2_queue *src_vq, |
796 | struct vb2_queue *dst_vq) |
797 | { |
798 | struct deinterlace_ctx *ctx = priv; |
799 | int ret; |
800 | |
801 | src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; |
802 | src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
803 | src_vq->drv_priv = ctx; |
804 | src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
805 | src_vq->ops = &deinterlace_qops; |
806 | src_vq->mem_ops = &vb2_dma_contig_memops; |
807 | src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
808 | src_vq->dev = ctx->dev->v4l2_dev.dev; |
809 | src_vq->lock = &ctx->dev->dev_mutex; |
810 | q_data[V4L2_M2M_SRC].fmt = &formats[0]; |
811 | q_data[V4L2_M2M_SRC].width = 640; |
812 | q_data[V4L2_M2M_SRC].height = 480; |
813 | q_data[V4L2_M2M_SRC].sizeimage = (640 * 480 * 3) / 2; |
814 | q_data[V4L2_M2M_SRC].field = V4L2_FIELD_SEQ_TB; |
815 | |
816 | ret = vb2_queue_init(q: src_vq); |
817 | if (ret) |
818 | return ret; |
819 | |
820 | dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
821 | dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
822 | dst_vq->drv_priv = ctx; |
823 | dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
824 | dst_vq->ops = &deinterlace_qops; |
825 | dst_vq->mem_ops = &vb2_dma_contig_memops; |
826 | dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
827 | dst_vq->dev = ctx->dev->v4l2_dev.dev; |
828 | dst_vq->lock = &ctx->dev->dev_mutex; |
829 | q_data[V4L2_M2M_DST].fmt = &formats[0]; |
830 | q_data[V4L2_M2M_DST].width = 640; |
831 | q_data[V4L2_M2M_DST].height = 480; |
832 | q_data[V4L2_M2M_DST].sizeimage = (640 * 480 * 3) / 2; |
833 | q_data[V4L2_M2M_SRC].field = V4L2_FIELD_INTERLACED_TB; |
834 | |
835 | return vb2_queue_init(q: dst_vq); |
836 | } |
837 | |
838 | /* |
839 | * File operations |
840 | */ |
841 | static int deinterlace_open(struct file *file) |
842 | { |
843 | struct deinterlace_dev *pcdev = video_drvdata(file); |
844 | struct deinterlace_ctx *ctx = NULL; |
845 | |
846 | ctx = kzalloc(size: sizeof *ctx, GFP_KERNEL); |
847 | if (!ctx) |
848 | return -ENOMEM; |
849 | |
850 | v4l2_fh_init(fh: &ctx->fh, vdev: video_devdata(file)); |
851 | file->private_data = &ctx->fh; |
852 | ctx->dev = pcdev; |
853 | |
854 | ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(m2m_dev: pcdev->m2m_dev, drv_priv: ctx, queue_init: &queue_init); |
855 | if (IS_ERR(ptr: ctx->fh.m2m_ctx)) { |
856 | int ret = PTR_ERR(ptr: ctx->fh.m2m_ctx); |
857 | |
858 | kfree(objp: ctx); |
859 | return ret; |
860 | } |
861 | |
862 | ctx->xt = kzalloc(size: sizeof(struct dma_interleaved_template) + |
863 | sizeof(struct data_chunk), GFP_KERNEL); |
864 | if (!ctx->xt) { |
865 | kfree(objp: ctx); |
866 | return -ENOMEM; |
867 | } |
868 | |
869 | ctx->colorspace = V4L2_COLORSPACE_REC709; |
870 | v4l2_fh_add(fh: &ctx->fh); |
871 | |
872 | dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n" , |
873 | ctx, ctx->fh.m2m_ctx); |
874 | |
875 | return 0; |
876 | } |
877 | |
878 | static int deinterlace_release(struct file *file) |
879 | { |
880 | struct deinterlace_dev *pcdev = video_drvdata(file); |
881 | struct deinterlace_ctx *ctx = file->private_data; |
882 | |
883 | dprintk(pcdev, "Releasing instance %p\n" , ctx); |
884 | |
885 | v4l2_fh_del(fh: &ctx->fh); |
886 | v4l2_fh_exit(fh: &ctx->fh); |
887 | v4l2_m2m_ctx_release(m2m_ctx: ctx->fh.m2m_ctx); |
888 | kfree(objp: ctx->xt); |
889 | kfree(objp: ctx); |
890 | |
891 | return 0; |
892 | } |
893 | |
894 | static const struct v4l2_file_operations deinterlace_fops = { |
895 | .owner = THIS_MODULE, |
896 | .open = deinterlace_open, |
897 | .release = deinterlace_release, |
898 | .poll = v4l2_m2m_fop_poll, |
899 | .unlocked_ioctl = video_ioctl2, |
900 | .mmap = v4l2_m2m_fop_mmap, |
901 | }; |
902 | |
903 | static const struct video_device deinterlace_videodev = { |
904 | .name = MEM2MEM_NAME, |
905 | .fops = &deinterlace_fops, |
906 | .ioctl_ops = &deinterlace_ioctl_ops, |
907 | .minor = -1, |
908 | .release = video_device_release_empty, |
909 | .vfl_dir = VFL_DIR_M2M, |
910 | .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING, |
911 | }; |
912 | |
913 | static const struct v4l2_m2m_ops m2m_ops = { |
914 | .device_run = deinterlace_device_run, |
915 | .job_ready = deinterlace_job_ready, |
916 | .job_abort = deinterlace_job_abort, |
917 | }; |
918 | |
919 | static int deinterlace_probe(struct platform_device *pdev) |
920 | { |
921 | struct deinterlace_dev *pcdev; |
922 | struct video_device *vfd; |
923 | dma_cap_mask_t mask; |
924 | int ret = 0; |
925 | |
926 | pcdev = devm_kzalloc(dev: &pdev->dev, size: sizeof(*pcdev), GFP_KERNEL); |
927 | if (!pcdev) |
928 | return -ENOMEM; |
929 | |
930 | spin_lock_init(&pcdev->irqlock); |
931 | |
932 | dma_cap_zero(mask); |
933 | dma_cap_set(DMA_INTERLEAVE, mask); |
934 | pcdev->dma_chan = dma_request_channel(mask, NULL, pcdev); |
935 | if (!pcdev->dma_chan) |
936 | return -ENODEV; |
937 | |
938 | if (!dma_has_cap(DMA_INTERLEAVE, pcdev->dma_chan->device->cap_mask)) { |
939 | dev_err(&pdev->dev, "DMA does not support INTERLEAVE\n" ); |
940 | ret = -ENODEV; |
941 | goto rel_dma; |
942 | } |
943 | |
944 | ret = v4l2_device_register(dev: &pdev->dev, v4l2_dev: &pcdev->v4l2_dev); |
945 | if (ret) |
946 | goto rel_dma; |
947 | |
948 | atomic_set(v: &pcdev->busy, i: 0); |
949 | mutex_init(&pcdev->dev_mutex); |
950 | |
951 | vfd = &pcdev->vfd; |
952 | *vfd = deinterlace_videodev; |
953 | vfd->lock = &pcdev->dev_mutex; |
954 | vfd->v4l2_dev = &pcdev->v4l2_dev; |
955 | |
956 | ret = video_register_device(vdev: vfd, type: VFL_TYPE_VIDEO, nr: 0); |
957 | if (ret) { |
958 | v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n" ); |
959 | goto unreg_dev; |
960 | } |
961 | |
962 | video_set_drvdata(vdev: vfd, data: pcdev); |
963 | v4l2_info(&pcdev->v4l2_dev, MEM2MEM_TEST_MODULE_NAME |
964 | " Device registered as /dev/video%d\n" , vfd->num); |
965 | |
966 | platform_set_drvdata(pdev, data: pcdev); |
967 | |
968 | pcdev->m2m_dev = v4l2_m2m_init(m2m_ops: &m2m_ops); |
969 | if (IS_ERR(ptr: pcdev->m2m_dev)) { |
970 | v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n" ); |
971 | ret = PTR_ERR(ptr: pcdev->m2m_dev); |
972 | goto err_m2m; |
973 | } |
974 | |
975 | return 0; |
976 | |
977 | err_m2m: |
978 | video_unregister_device(vdev: &pcdev->vfd); |
979 | unreg_dev: |
980 | v4l2_device_unregister(v4l2_dev: &pcdev->v4l2_dev); |
981 | rel_dma: |
982 | dma_release_channel(chan: pcdev->dma_chan); |
983 | |
984 | return ret; |
985 | } |
986 | |
987 | static void deinterlace_remove(struct platform_device *pdev) |
988 | { |
989 | struct deinterlace_dev *pcdev = platform_get_drvdata(pdev); |
990 | |
991 | v4l2_info(&pcdev->v4l2_dev, "Removing " MEM2MEM_TEST_MODULE_NAME); |
992 | v4l2_m2m_release(m2m_dev: pcdev->m2m_dev); |
993 | video_unregister_device(vdev: &pcdev->vfd); |
994 | v4l2_device_unregister(v4l2_dev: &pcdev->v4l2_dev); |
995 | dma_release_channel(chan: pcdev->dma_chan); |
996 | } |
997 | |
998 | static struct platform_driver deinterlace_pdrv = { |
999 | .probe = deinterlace_probe, |
1000 | .remove_new = deinterlace_remove, |
1001 | .driver = { |
1002 | .name = MEM2MEM_NAME, |
1003 | }, |
1004 | }; |
1005 | module_platform_driver(deinterlace_pdrv); |
1006 | |
1007 | |