1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Support eMMa-PrP through mem2mem framework. |
4 | * |
5 | * eMMa-PrP is a piece of HW that allows fetching buffers |
6 | * from one memory location and do several operations on |
7 | * them such as scaling or format conversion giving, as a result |
8 | * a new processed buffer in another memory location. |
9 | * |
10 | * Based on mem2mem_testdev.c by Pawel Osciak. |
11 | * |
12 | * Copyright (c) 2011 Vista Silicon S.L. |
13 | * Javier Martin <javier.martin@vista-silicon.com> |
14 | */ |
15 | #include <linux/module.h> |
16 | #include <linux/clk.h> |
17 | #include <linux/slab.h> |
18 | #include <linux/interrupt.h> |
19 | #include <linux/io.h> |
20 | |
21 | #include <linux/platform_device.h> |
22 | #include <media/v4l2-mem2mem.h> |
23 | #include <media/v4l2-device.h> |
24 | #include <media/v4l2-ioctl.h> |
25 | #include <media/videobuf2-dma-contig.h> |
26 | #include <linux/sizes.h> |
27 | |
28 | #define EMMAPRP_MODULE_NAME "mem2mem-emmaprp" |
29 | |
30 | MODULE_DESCRIPTION("Mem-to-mem device which supports eMMa-PrP present in mx2 SoCs" ); |
31 | MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com" ); |
32 | MODULE_LICENSE("GPL" ); |
33 | MODULE_VERSION("0.0.1" ); |
34 | |
35 | static bool debug; |
36 | module_param(debug, bool, 0644); |
37 | |
38 | #define MIN_W 32 |
39 | #define MIN_H 32 |
40 | #define MAX_W 2040 |
41 | #define MAX_H 2046 |
42 | |
43 | #define S_ALIGN 1 /* multiple of 2 */ |
44 | #define W_ALIGN_YUV420 3 /* multiple of 8 */ |
45 | #define W_ALIGN_OTHERS 2 /* multiple of 4 */ |
46 | #define H_ALIGN 1 /* multiple of 2 */ |
47 | |
48 | /* Flags that indicate a format can be used for capture/output */ |
49 | #define MEM2MEM_CAPTURE (1 << 0) |
50 | #define MEM2MEM_OUTPUT (1 << 1) |
51 | |
52 | #define MEM2MEM_NAME "m2m-emmaprp" |
53 | |
54 | /* In bytes, per queue */ |
55 | #define MEM2MEM_VID_MEM_LIMIT SZ_16M |
56 | |
57 | #define dprintk(dev, fmt, arg...) \ |
58 | v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg) |
59 | |
60 | /* EMMA PrP */ |
61 | #define PRP_CNTL 0x00 |
62 | #define PRP_INTR_CNTL 0x04 |
63 | #define PRP_INTRSTATUS 0x08 |
64 | #define PRP_SOURCE_Y_PTR 0x0c |
65 | #define PRP_SOURCE_CB_PTR 0x10 |
66 | #define PRP_SOURCE_CR_PTR 0x14 |
67 | #define PRP_DEST_RGB1_PTR 0x18 |
68 | #define PRP_DEST_RGB2_PTR 0x1c |
69 | #define PRP_DEST_Y_PTR 0x20 |
70 | #define PRP_DEST_CB_PTR 0x24 |
71 | #define PRP_DEST_CR_PTR 0x28 |
72 | #define PRP_SRC_FRAME_SIZE 0x2c |
73 | #define PRP_DEST_CH1_LINE_STRIDE 0x30 |
74 | #define PRP_SRC_PIXEL_FORMAT_CNTL 0x34 |
75 | #define PRP_CH1_PIXEL_FORMAT_CNTL 0x38 |
76 | #define PRP_CH1_OUT_IMAGE_SIZE 0x3c |
77 | #define PRP_CH2_OUT_IMAGE_SIZE 0x40 |
78 | #define PRP_SRC_LINE_STRIDE 0x44 |
79 | #define PRP_CSC_COEF_012 0x48 |
80 | #define PRP_CSC_COEF_345 0x4c |
81 | #define PRP_CSC_COEF_678 0x50 |
82 | #define PRP_CH1_RZ_HORI_COEF1 0x54 |
83 | #define PRP_CH1_RZ_HORI_COEF2 0x58 |
84 | #define PRP_CH1_RZ_HORI_VALID 0x5c |
85 | #define PRP_CH1_RZ_VERT_COEF1 0x60 |
86 | #define PRP_CH1_RZ_VERT_COEF2 0x64 |
87 | #define PRP_CH1_RZ_VERT_VALID 0x68 |
88 | #define PRP_CH2_RZ_HORI_COEF1 0x6c |
89 | #define PRP_CH2_RZ_HORI_COEF2 0x70 |
90 | #define PRP_CH2_RZ_HORI_VALID 0x74 |
91 | #define PRP_CH2_RZ_VERT_COEF1 0x78 |
92 | #define PRP_CH2_RZ_VERT_COEF2 0x7c |
93 | #define PRP_CH2_RZ_VERT_VALID 0x80 |
94 | |
95 | #define PRP_CNTL_CH1EN (1 << 0) |
96 | #define PRP_CNTL_CH2EN (1 << 1) |
97 | #define PRP_CNTL_CSIEN (1 << 2) |
98 | #define PRP_CNTL_DATA_IN_YUV420 (0 << 3) |
99 | #define PRP_CNTL_DATA_IN_YUV422 (1 << 3) |
100 | #define PRP_CNTL_DATA_IN_RGB16 (2 << 3) |
101 | #define PRP_CNTL_DATA_IN_RGB32 (3 << 3) |
102 | #define PRP_CNTL_CH1_OUT_RGB8 (0 << 5) |
103 | #define PRP_CNTL_CH1_OUT_RGB16 (1 << 5) |
104 | #define PRP_CNTL_CH1_OUT_RGB32 (2 << 5) |
105 | #define PRP_CNTL_CH1_OUT_YUV422 (3 << 5) |
106 | #define PRP_CNTL_CH2_OUT_YUV420 (0 << 7) |
107 | #define PRP_CNTL_CH2_OUT_YUV422 (1 << 7) |
108 | #define PRP_CNTL_CH2_OUT_YUV444 (2 << 7) |
109 | #define PRP_CNTL_CH1_LEN (1 << 9) |
110 | #define PRP_CNTL_CH2_LEN (1 << 10) |
111 | #define PRP_CNTL_SKIP_FRAME (1 << 11) |
112 | #define PRP_CNTL_SWRST (1 << 12) |
113 | #define PRP_CNTL_CLKEN (1 << 13) |
114 | #define PRP_CNTL_WEN (1 << 14) |
115 | #define PRP_CNTL_CH1BYP (1 << 15) |
116 | #define PRP_CNTL_IN_TSKIP(x) ((x) << 16) |
117 | #define PRP_CNTL_CH1_TSKIP(x) ((x) << 19) |
118 | #define PRP_CNTL_CH2_TSKIP(x) ((x) << 22) |
119 | #define PRP_CNTL_INPUT_FIFO_LEVEL(x) ((x) << 25) |
120 | #define PRP_CNTL_RZ_FIFO_LEVEL(x) ((x) << 27) |
121 | #define PRP_CNTL_CH2B1EN (1 << 29) |
122 | #define PRP_CNTL_CH2B2EN (1 << 30) |
123 | #define PRP_CNTL_CH2FEN (1UL << 31) |
124 | |
125 | #define PRP_SIZE_HEIGHT(x) (x) |
126 | #define PRP_SIZE_WIDTH(x) ((x) << 16) |
127 | |
128 | /* IRQ Enable and status register */ |
129 | #define PRP_INTR_RDERR (1 << 0) |
130 | #define PRP_INTR_CH1WERR (1 << 1) |
131 | #define PRP_INTR_CH2WERR (1 << 2) |
132 | #define PRP_INTR_CH1FC (1 << 3) |
133 | #define PRP_INTR_CH2FC (1 << 5) |
134 | #define PRP_INTR_LBOVF (1 << 7) |
135 | #define PRP_INTR_CH2OVF (1 << 8) |
136 | |
137 | #define PRP_INTR_ST_RDERR (1 << 0) |
138 | #define PRP_INTR_ST_CH1WERR (1 << 1) |
139 | #define PRP_INTR_ST_CH2WERR (1 << 2) |
140 | #define PRP_INTR_ST_CH2B2CI (1 << 3) |
141 | #define PRP_INTR_ST_CH2B1CI (1 << 4) |
142 | #define PRP_INTR_ST_CH1B2CI (1 << 5) |
143 | #define PRP_INTR_ST_CH1B1CI (1 << 6) |
144 | #define PRP_INTR_ST_LBOVF (1 << 7) |
145 | #define PRP_INTR_ST_CH2OVF (1 << 8) |
146 | |
147 | struct emmaprp_fmt { |
148 | u32 fourcc; |
149 | /* Types the format can be used for */ |
150 | u32 types; |
151 | }; |
152 | |
153 | static struct emmaprp_fmt formats[] = { |
154 | { |
155 | .fourcc = V4L2_PIX_FMT_YUV420, |
156 | .types = MEM2MEM_CAPTURE, |
157 | }, |
158 | { |
159 | .fourcc = V4L2_PIX_FMT_YUYV, |
160 | .types = MEM2MEM_OUTPUT, |
161 | }, |
162 | }; |
163 | |
164 | /* Per-queue, driver-specific private data */ |
165 | struct emmaprp_q_data { |
166 | unsigned int width; |
167 | unsigned int height; |
168 | unsigned int sizeimage; |
169 | struct emmaprp_fmt *fmt; |
170 | }; |
171 | |
172 | enum { |
173 | V4L2_M2M_SRC = 0, |
174 | V4L2_M2M_DST = 1, |
175 | }; |
176 | |
177 | #define NUM_FORMATS ARRAY_SIZE(formats) |
178 | |
179 | static struct emmaprp_fmt *find_format(struct v4l2_format *f) |
180 | { |
181 | struct emmaprp_fmt *fmt; |
182 | unsigned int k; |
183 | |
184 | for (k = 0; k < NUM_FORMATS; k++) { |
185 | fmt = &formats[k]; |
186 | if (fmt->fourcc == f->fmt.pix.pixelformat) |
187 | break; |
188 | } |
189 | |
190 | if (k == NUM_FORMATS) |
191 | return NULL; |
192 | |
193 | return &formats[k]; |
194 | } |
195 | |
196 | struct emmaprp_dev { |
197 | struct v4l2_device v4l2_dev; |
198 | struct video_device *vfd; |
199 | |
200 | struct mutex dev_mutex; |
201 | spinlock_t irqlock; |
202 | |
203 | void __iomem *base_emma; |
204 | struct clk *clk_emma_ahb, *clk_emma_ipg; |
205 | |
206 | struct v4l2_m2m_dev *m2m_dev; |
207 | }; |
208 | |
209 | struct emmaprp_ctx { |
210 | struct v4l2_fh fh; |
211 | struct emmaprp_dev *dev; |
212 | /* Abort requested by m2m */ |
213 | int aborting; |
214 | struct emmaprp_q_data q_data[2]; |
215 | }; |
216 | |
217 | static struct emmaprp_q_data *get_q_data(struct emmaprp_ctx *ctx, |
218 | enum v4l2_buf_type type) |
219 | { |
220 | switch (type) { |
221 | case V4L2_BUF_TYPE_VIDEO_OUTPUT: |
222 | return &(ctx->q_data[V4L2_M2M_SRC]); |
223 | case V4L2_BUF_TYPE_VIDEO_CAPTURE: |
224 | return &(ctx->q_data[V4L2_M2M_DST]); |
225 | default: |
226 | BUG(); |
227 | } |
228 | return NULL; |
229 | } |
230 | |
231 | /* |
232 | * mem2mem callbacks |
233 | */ |
234 | static void emmaprp_job_abort(void *priv) |
235 | { |
236 | struct emmaprp_ctx *ctx = priv; |
237 | struct emmaprp_dev *pcdev = ctx->dev; |
238 | |
239 | ctx->aborting = 1; |
240 | |
241 | dprintk(pcdev, "Aborting task\n" ); |
242 | |
243 | v4l2_m2m_job_finish(m2m_dev: pcdev->m2m_dev, m2m_ctx: ctx->fh.m2m_ctx); |
244 | } |
245 | |
246 | static inline void emmaprp_dump_regs(struct emmaprp_dev *pcdev) |
247 | { |
248 | dprintk(pcdev, |
249 | "eMMa-PrP Registers:\n" |
250 | " SOURCE_Y_PTR = 0x%08X\n" |
251 | " SRC_FRAME_SIZE = 0x%08X\n" |
252 | " DEST_Y_PTR = 0x%08X\n" |
253 | " DEST_CR_PTR = 0x%08X\n" |
254 | " DEST_CB_PTR = 0x%08X\n" |
255 | " CH2_OUT_IMAGE_SIZE = 0x%08X\n" |
256 | " CNTL = 0x%08X\n" , |
257 | readl(pcdev->base_emma + PRP_SOURCE_Y_PTR), |
258 | readl(pcdev->base_emma + PRP_SRC_FRAME_SIZE), |
259 | readl(pcdev->base_emma + PRP_DEST_Y_PTR), |
260 | readl(pcdev->base_emma + PRP_DEST_CR_PTR), |
261 | readl(pcdev->base_emma + PRP_DEST_CB_PTR), |
262 | readl(pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE), |
263 | readl(pcdev->base_emma + PRP_CNTL)); |
264 | } |
265 | |
266 | static void emmaprp_device_run(void *priv) |
267 | { |
268 | struct emmaprp_ctx *ctx = priv; |
269 | struct emmaprp_q_data *s_q_data, *d_q_data; |
270 | struct vb2_v4l2_buffer *src_buf, *dst_buf; |
271 | struct emmaprp_dev *pcdev = ctx->dev; |
272 | unsigned int s_width, s_height; |
273 | unsigned int d_width, d_height; |
274 | unsigned int d_size; |
275 | dma_addr_t p_in, p_out; |
276 | u32 tmp; |
277 | |
278 | src_buf = v4l2_m2m_next_src_buf(m2m_ctx: ctx->fh.m2m_ctx); |
279 | dst_buf = v4l2_m2m_next_dst_buf(m2m_ctx: ctx->fh.m2m_ctx); |
280 | |
281 | s_q_data = get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_OUTPUT); |
282 | s_width = s_q_data->width; |
283 | s_height = s_q_data->height; |
284 | |
285 | d_q_data = get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_CAPTURE); |
286 | d_width = d_q_data->width; |
287 | d_height = d_q_data->height; |
288 | d_size = d_width * d_height; |
289 | |
290 | p_in = vb2_dma_contig_plane_dma_addr(vb: &src_buf->vb2_buf, plane_no: 0); |
291 | p_out = vb2_dma_contig_plane_dma_addr(vb: &dst_buf->vb2_buf, plane_no: 0); |
292 | if (!p_in || !p_out) { |
293 | v4l2_err(&pcdev->v4l2_dev, |
294 | "Acquiring kernel pointers to buffers failed\n" ); |
295 | return; |
296 | } |
297 | |
298 | /* Input frame parameters */ |
299 | writel(val: p_in, addr: pcdev->base_emma + PRP_SOURCE_Y_PTR); |
300 | writel(PRP_SIZE_WIDTH(s_width) | PRP_SIZE_HEIGHT(s_height), |
301 | addr: pcdev->base_emma + PRP_SRC_FRAME_SIZE); |
302 | |
303 | /* Output frame parameters */ |
304 | writel(val: p_out, addr: pcdev->base_emma + PRP_DEST_Y_PTR); |
305 | writel(val: p_out + d_size, addr: pcdev->base_emma + PRP_DEST_CB_PTR); |
306 | writel(val: p_out + d_size + (d_size >> 2), |
307 | addr: pcdev->base_emma + PRP_DEST_CR_PTR); |
308 | writel(PRP_SIZE_WIDTH(d_width) | PRP_SIZE_HEIGHT(d_height), |
309 | addr: pcdev->base_emma + PRP_CH2_OUT_IMAGE_SIZE); |
310 | |
311 | /* IRQ configuration */ |
312 | tmp = readl(addr: pcdev->base_emma + PRP_INTR_CNTL); |
313 | writel(val: tmp | PRP_INTR_RDERR | |
314 | PRP_INTR_CH2WERR | |
315 | PRP_INTR_CH2FC, |
316 | addr: pcdev->base_emma + PRP_INTR_CNTL); |
317 | |
318 | emmaprp_dump_regs(pcdev); |
319 | |
320 | /* Enable transfer */ |
321 | tmp = readl(addr: pcdev->base_emma + PRP_CNTL); |
322 | writel(val: tmp | PRP_CNTL_CH2_OUT_YUV420 | |
323 | PRP_CNTL_DATA_IN_YUV422 | |
324 | PRP_CNTL_CH2EN, |
325 | addr: pcdev->base_emma + PRP_CNTL); |
326 | } |
327 | |
328 | static irqreturn_t emmaprp_irq(int irq_emma, void *data) |
329 | { |
330 | struct emmaprp_dev *pcdev = data; |
331 | struct emmaprp_ctx *curr_ctx; |
332 | struct vb2_v4l2_buffer *src_vb, *dst_vb; |
333 | unsigned long flags; |
334 | u32 irqst; |
335 | |
336 | /* Check irq flags and clear irq */ |
337 | irqst = readl(addr: pcdev->base_emma + PRP_INTRSTATUS); |
338 | writel(val: irqst, addr: pcdev->base_emma + PRP_INTRSTATUS); |
339 | dprintk(pcdev, "irqst = 0x%08x\n" , irqst); |
340 | |
341 | curr_ctx = v4l2_m2m_get_curr_priv(m2m_dev: pcdev->m2m_dev); |
342 | if (curr_ctx == NULL) { |
343 | pr_err("Instance released before the end of transaction\n" ); |
344 | return IRQ_HANDLED; |
345 | } |
346 | |
347 | if (!curr_ctx->aborting) { |
348 | if ((irqst & PRP_INTR_ST_RDERR) || |
349 | (irqst & PRP_INTR_ST_CH2WERR)) { |
350 | pr_err("PrP bus error occurred, this transfer is probably corrupted\n" ); |
351 | writel(PRP_CNTL_SWRST, addr: pcdev->base_emma + PRP_CNTL); |
352 | } else if (irqst & PRP_INTR_ST_CH2B1CI) { /* buffer ready */ |
353 | src_vb = v4l2_m2m_src_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
354 | dst_vb = v4l2_m2m_dst_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
355 | |
356 | dst_vb->vb2_buf.timestamp = src_vb->vb2_buf.timestamp; |
357 | dst_vb->flags &= |
358 | ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK; |
359 | dst_vb->flags |= |
360 | src_vb->flags |
361 | & V4L2_BUF_FLAG_TSTAMP_SRC_MASK; |
362 | dst_vb->timecode = src_vb->timecode; |
363 | |
364 | spin_lock_irqsave(&pcdev->irqlock, flags); |
365 | v4l2_m2m_buf_done(buf: src_vb, state: VB2_BUF_STATE_DONE); |
366 | v4l2_m2m_buf_done(buf: dst_vb, state: VB2_BUF_STATE_DONE); |
367 | spin_unlock_irqrestore(lock: &pcdev->irqlock, flags); |
368 | } |
369 | } |
370 | |
371 | v4l2_m2m_job_finish(m2m_dev: pcdev->m2m_dev, m2m_ctx: curr_ctx->fh.m2m_ctx); |
372 | return IRQ_HANDLED; |
373 | } |
374 | |
375 | /* |
376 | * video ioctls |
377 | */ |
378 | static int vidioc_querycap(struct file *file, void *priv, |
379 | struct v4l2_capability *cap) |
380 | { |
381 | strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver)); |
382 | strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card)); |
383 | return 0; |
384 | } |
385 | |
386 | static int enum_fmt(struct v4l2_fmtdesc *f, u32 type) |
387 | { |
388 | int i, num; |
389 | struct emmaprp_fmt *fmt; |
390 | |
391 | num = 0; |
392 | |
393 | for (i = 0; i < NUM_FORMATS; ++i) { |
394 | if (formats[i].types & type) { |
395 | /* index-th format of type type found ? */ |
396 | if (num == f->index) |
397 | break; |
398 | /* Correct type but haven't reached our index yet, |
399 | * just increment per-type index */ |
400 | ++num; |
401 | } |
402 | } |
403 | |
404 | if (i < NUM_FORMATS) { |
405 | /* Format found */ |
406 | fmt = &formats[i]; |
407 | f->pixelformat = fmt->fourcc; |
408 | return 0; |
409 | } |
410 | |
411 | /* Format not found */ |
412 | return -EINVAL; |
413 | } |
414 | |
415 | static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv, |
416 | struct v4l2_fmtdesc *f) |
417 | { |
418 | return enum_fmt(f, MEM2MEM_CAPTURE); |
419 | } |
420 | |
421 | static int vidioc_enum_fmt_vid_out(struct file *file, void *priv, |
422 | struct v4l2_fmtdesc *f) |
423 | { |
424 | return enum_fmt(f, MEM2MEM_OUTPUT); |
425 | } |
426 | |
427 | static int vidioc_g_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f) |
428 | { |
429 | struct vb2_queue *vq; |
430 | struct emmaprp_q_data *q_data; |
431 | |
432 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
433 | if (!vq) |
434 | return -EINVAL; |
435 | |
436 | q_data = get_q_data(ctx, type: f->type); |
437 | |
438 | f->fmt.pix.width = q_data->width; |
439 | f->fmt.pix.height = q_data->height; |
440 | f->fmt.pix.field = V4L2_FIELD_NONE; |
441 | f->fmt.pix.pixelformat = q_data->fmt->fourcc; |
442 | if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) |
443 | f->fmt.pix.bytesperline = q_data->width * 3 / 2; |
444 | else /* YUYV */ |
445 | f->fmt.pix.bytesperline = q_data->width * 2; |
446 | f->fmt.pix.sizeimage = q_data->sizeimage; |
447 | |
448 | return 0; |
449 | } |
450 | |
451 | static int vidioc_g_fmt_vid_out(struct file *file, void *priv, |
452 | struct v4l2_format *f) |
453 | { |
454 | return vidioc_g_fmt(ctx: priv, f); |
455 | } |
456 | |
457 | static int vidioc_g_fmt_vid_cap(struct file *file, void *priv, |
458 | struct v4l2_format *f) |
459 | { |
460 | return vidioc_g_fmt(ctx: priv, f); |
461 | } |
462 | |
463 | static int vidioc_try_fmt(struct v4l2_format *f) |
464 | { |
465 | enum v4l2_field field; |
466 | |
467 | |
468 | if (!find_format(f)) |
469 | return -EINVAL; |
470 | |
471 | field = f->fmt.pix.field; |
472 | if (field == V4L2_FIELD_ANY) |
473 | field = V4L2_FIELD_NONE; |
474 | else if (V4L2_FIELD_NONE != field) |
475 | return -EINVAL; |
476 | |
477 | /* V4L2 specification suggests the driver corrects the format struct |
478 | * if any of the dimensions is unsupported */ |
479 | f->fmt.pix.field = field; |
480 | |
481 | if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_YUV420) { |
482 | v4l_bound_align_image(width: &f->fmt.pix.width, MIN_W, MAX_W, |
483 | W_ALIGN_YUV420, height: &f->fmt.pix.height, |
484 | MIN_H, MAX_H, H_ALIGN, S_ALIGN); |
485 | f->fmt.pix.bytesperline = f->fmt.pix.width * 3 / 2; |
486 | } else { |
487 | v4l_bound_align_image(width: &f->fmt.pix.width, MIN_W, MAX_W, |
488 | W_ALIGN_OTHERS, height: &f->fmt.pix.height, |
489 | MIN_H, MAX_H, H_ALIGN, S_ALIGN); |
490 | f->fmt.pix.bytesperline = f->fmt.pix.width * 2; |
491 | } |
492 | f->fmt.pix.sizeimage = f->fmt.pix.height * f->fmt.pix.bytesperline; |
493 | |
494 | return 0; |
495 | } |
496 | |
497 | static int vidioc_try_fmt_vid_cap(struct file *file, void *priv, |
498 | struct v4l2_format *f) |
499 | { |
500 | struct emmaprp_fmt *fmt; |
501 | struct emmaprp_ctx *ctx = priv; |
502 | |
503 | fmt = find_format(f); |
504 | if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) { |
505 | v4l2_err(&ctx->dev->v4l2_dev, |
506 | "Fourcc format (0x%08x) invalid.\n" , |
507 | f->fmt.pix.pixelformat); |
508 | return -EINVAL; |
509 | } |
510 | |
511 | return vidioc_try_fmt(f); |
512 | } |
513 | |
514 | static int vidioc_try_fmt_vid_out(struct file *file, void *priv, |
515 | struct v4l2_format *f) |
516 | { |
517 | struct emmaprp_fmt *fmt; |
518 | struct emmaprp_ctx *ctx = priv; |
519 | |
520 | fmt = find_format(f); |
521 | if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) { |
522 | v4l2_err(&ctx->dev->v4l2_dev, |
523 | "Fourcc format (0x%08x) invalid.\n" , |
524 | f->fmt.pix.pixelformat); |
525 | return -EINVAL; |
526 | } |
527 | |
528 | return vidioc_try_fmt(f); |
529 | } |
530 | |
531 | static int vidioc_s_fmt(struct emmaprp_ctx *ctx, struct v4l2_format *f) |
532 | { |
533 | struct emmaprp_q_data *q_data; |
534 | struct vb2_queue *vq; |
535 | int ret; |
536 | |
537 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
538 | if (!vq) |
539 | return -EINVAL; |
540 | |
541 | q_data = get_q_data(ctx, type: f->type); |
542 | if (!q_data) |
543 | return -EINVAL; |
544 | |
545 | if (vb2_is_busy(q: vq)) { |
546 | v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n" , __func__); |
547 | return -EBUSY; |
548 | } |
549 | |
550 | ret = vidioc_try_fmt(f); |
551 | if (ret) |
552 | return ret; |
553 | |
554 | q_data->fmt = find_format(f); |
555 | q_data->width = f->fmt.pix.width; |
556 | q_data->height = f->fmt.pix.height; |
557 | if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420) |
558 | q_data->sizeimage = q_data->width * q_data->height * 3 / 2; |
559 | else /* YUYV */ |
560 | q_data->sizeimage = q_data->width * q_data->height * 2; |
561 | |
562 | dprintk(ctx->dev, |
563 | "Setting format for type %d, wxh: %dx%d, fmt: %d\n" , |
564 | f->type, q_data->width, q_data->height, q_data->fmt->fourcc); |
565 | |
566 | return 0; |
567 | } |
568 | |
569 | static int vidioc_s_fmt_vid_cap(struct file *file, void *priv, |
570 | struct v4l2_format *f) |
571 | { |
572 | int ret; |
573 | |
574 | ret = vidioc_try_fmt_vid_cap(file, priv, f); |
575 | if (ret) |
576 | return ret; |
577 | |
578 | return vidioc_s_fmt(ctx: priv, f); |
579 | } |
580 | |
581 | static int vidioc_s_fmt_vid_out(struct file *file, void *priv, |
582 | struct v4l2_format *f) |
583 | { |
584 | int ret; |
585 | |
586 | ret = vidioc_try_fmt_vid_out(file, priv, f); |
587 | if (ret) |
588 | return ret; |
589 | |
590 | return vidioc_s_fmt(ctx: priv, f); |
591 | } |
592 | |
593 | static const struct v4l2_ioctl_ops emmaprp_ioctl_ops = { |
594 | .vidioc_querycap = vidioc_querycap, |
595 | |
596 | .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap, |
597 | .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap, |
598 | .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap, |
599 | .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap, |
600 | |
601 | .vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out, |
602 | .vidioc_g_fmt_vid_out = vidioc_g_fmt_vid_out, |
603 | .vidioc_try_fmt_vid_out = vidioc_try_fmt_vid_out, |
604 | .vidioc_s_fmt_vid_out = vidioc_s_fmt_vid_out, |
605 | |
606 | .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs, |
607 | .vidioc_querybuf = v4l2_m2m_ioctl_querybuf, |
608 | .vidioc_qbuf = v4l2_m2m_ioctl_qbuf, |
609 | .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf, |
610 | .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf, |
611 | .vidioc_expbuf = v4l2_m2m_ioctl_expbuf, |
612 | .vidioc_streamon = v4l2_m2m_ioctl_streamon, |
613 | .vidioc_streamoff = v4l2_m2m_ioctl_streamoff, |
614 | }; |
615 | |
616 | |
617 | /* |
618 | * Queue operations |
619 | */ |
620 | static int emmaprp_queue_setup(struct vb2_queue *vq, |
621 | unsigned int *nbuffers, unsigned int *nplanes, |
622 | unsigned int sizes[], struct device *alloc_devs[]) |
623 | { |
624 | struct emmaprp_ctx *ctx = vb2_get_drv_priv(q: vq); |
625 | struct emmaprp_q_data *q_data; |
626 | unsigned int size, count = *nbuffers; |
627 | |
628 | q_data = get_q_data(ctx, type: vq->type); |
629 | |
630 | if (q_data->fmt->fourcc == V4L2_PIX_FMT_YUV420) |
631 | size = q_data->width * q_data->height * 3 / 2; |
632 | else |
633 | size = q_data->width * q_data->height * 2; |
634 | |
635 | while (size * count > MEM2MEM_VID_MEM_LIMIT) |
636 | (count)--; |
637 | |
638 | *nplanes = 1; |
639 | *nbuffers = count; |
640 | sizes[0] = size; |
641 | |
642 | dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n" , count, size); |
643 | |
644 | return 0; |
645 | } |
646 | |
647 | static int emmaprp_buf_prepare(struct vb2_buffer *vb) |
648 | { |
649 | struct emmaprp_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
650 | struct emmaprp_q_data *q_data; |
651 | |
652 | dprintk(ctx->dev, "type: %d\n" , vb->vb2_queue->type); |
653 | |
654 | q_data = get_q_data(ctx, type: vb->vb2_queue->type); |
655 | |
656 | if (vb2_plane_size(vb, plane_no: 0) < q_data->sizeimage) { |
657 | dprintk(ctx->dev, |
658 | "%s data will not fit into plane(%lu < %lu)\n" , |
659 | __func__, vb2_plane_size(vb, 0), |
660 | (long)q_data->sizeimage); |
661 | return -EINVAL; |
662 | } |
663 | |
664 | vb2_set_plane_payload(vb, plane_no: 0, size: q_data->sizeimage); |
665 | |
666 | return 0; |
667 | } |
668 | |
669 | static void emmaprp_buf_queue(struct vb2_buffer *vb) |
670 | { |
671 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); |
672 | struct emmaprp_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
673 | v4l2_m2m_buf_queue(m2m_ctx: ctx->fh.m2m_ctx, vbuf); |
674 | } |
675 | |
676 | static const struct vb2_ops emmaprp_qops = { |
677 | .queue_setup = emmaprp_queue_setup, |
678 | .buf_prepare = emmaprp_buf_prepare, |
679 | .buf_queue = emmaprp_buf_queue, |
680 | .wait_prepare = vb2_ops_wait_prepare, |
681 | .wait_finish = vb2_ops_wait_finish, |
682 | }; |
683 | |
684 | static int queue_init(void *priv, struct vb2_queue *src_vq, |
685 | struct vb2_queue *dst_vq) |
686 | { |
687 | struct emmaprp_ctx *ctx = priv; |
688 | int ret; |
689 | |
690 | src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT; |
691 | src_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
692 | src_vq->drv_priv = ctx; |
693 | src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
694 | src_vq->ops = &emmaprp_qops; |
695 | src_vq->mem_ops = &vb2_dma_contig_memops; |
696 | src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
697 | src_vq->dev = ctx->dev->v4l2_dev.dev; |
698 | src_vq->lock = &ctx->dev->dev_mutex; |
699 | |
700 | ret = vb2_queue_init(q: src_vq); |
701 | if (ret) |
702 | return ret; |
703 | |
704 | dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; |
705 | dst_vq->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; |
706 | dst_vq->drv_priv = ctx; |
707 | dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
708 | dst_vq->ops = &emmaprp_qops; |
709 | dst_vq->mem_ops = &vb2_dma_contig_memops; |
710 | dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
711 | dst_vq->dev = ctx->dev->v4l2_dev.dev; |
712 | dst_vq->lock = &ctx->dev->dev_mutex; |
713 | |
714 | return vb2_queue_init(q: dst_vq); |
715 | } |
716 | |
717 | /* |
718 | * File operations |
719 | */ |
720 | static int emmaprp_open(struct file *file) |
721 | { |
722 | struct emmaprp_dev *pcdev = video_drvdata(file); |
723 | struct emmaprp_ctx *ctx; |
724 | |
725 | ctx = kzalloc(size: sizeof *ctx, GFP_KERNEL); |
726 | if (!ctx) |
727 | return -ENOMEM; |
728 | |
729 | v4l2_fh_init(fh: &ctx->fh, vdev: video_devdata(file)); |
730 | file->private_data = &ctx->fh; |
731 | ctx->dev = pcdev; |
732 | |
733 | if (mutex_lock_interruptible(&pcdev->dev_mutex)) { |
734 | kfree(objp: ctx); |
735 | return -ERESTARTSYS; |
736 | } |
737 | |
738 | ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(m2m_dev: pcdev->m2m_dev, drv_priv: ctx, queue_init: &queue_init); |
739 | |
740 | if (IS_ERR(ptr: ctx->fh.m2m_ctx)) { |
741 | int ret = PTR_ERR(ptr: ctx->fh.m2m_ctx); |
742 | |
743 | mutex_unlock(lock: &pcdev->dev_mutex); |
744 | kfree(objp: ctx); |
745 | return ret; |
746 | } |
747 | |
748 | clk_prepare_enable(clk: pcdev->clk_emma_ipg); |
749 | clk_prepare_enable(clk: pcdev->clk_emma_ahb); |
750 | ctx->q_data[V4L2_M2M_SRC].fmt = &formats[1]; |
751 | ctx->q_data[V4L2_M2M_DST].fmt = &formats[0]; |
752 | v4l2_fh_add(fh: &ctx->fh); |
753 | mutex_unlock(lock: &pcdev->dev_mutex); |
754 | |
755 | dprintk(pcdev, "Created instance %p, m2m_ctx: %p\n" , ctx, ctx->fh.m2m_ctx); |
756 | |
757 | return 0; |
758 | } |
759 | |
760 | static int emmaprp_release(struct file *file) |
761 | { |
762 | struct emmaprp_dev *pcdev = video_drvdata(file); |
763 | struct emmaprp_ctx *ctx = file->private_data; |
764 | |
765 | dprintk(pcdev, "Releasing instance %p\n" , ctx); |
766 | |
767 | mutex_lock(&pcdev->dev_mutex); |
768 | clk_disable_unprepare(clk: pcdev->clk_emma_ahb); |
769 | clk_disable_unprepare(clk: pcdev->clk_emma_ipg); |
770 | v4l2_fh_del(fh: &ctx->fh); |
771 | v4l2_fh_exit(fh: &ctx->fh); |
772 | v4l2_m2m_ctx_release(m2m_ctx: ctx->fh.m2m_ctx); |
773 | mutex_unlock(lock: &pcdev->dev_mutex); |
774 | kfree(objp: ctx); |
775 | |
776 | return 0; |
777 | } |
778 | |
779 | static const struct v4l2_file_operations emmaprp_fops = { |
780 | .owner = THIS_MODULE, |
781 | .open = emmaprp_open, |
782 | .release = emmaprp_release, |
783 | .poll = v4l2_m2m_fop_poll, |
784 | .unlocked_ioctl = video_ioctl2, |
785 | .mmap = v4l2_m2m_fop_mmap, |
786 | }; |
787 | |
788 | static const struct video_device emmaprp_videodev = { |
789 | .name = MEM2MEM_NAME, |
790 | .fops = &emmaprp_fops, |
791 | .ioctl_ops = &emmaprp_ioctl_ops, |
792 | .minor = -1, |
793 | .release = video_device_release, |
794 | .vfl_dir = VFL_DIR_M2M, |
795 | .device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING, |
796 | }; |
797 | |
798 | static const struct v4l2_m2m_ops m2m_ops = { |
799 | .device_run = emmaprp_device_run, |
800 | .job_abort = emmaprp_job_abort, |
801 | }; |
802 | |
803 | static int emmaprp_probe(struct platform_device *pdev) |
804 | { |
805 | struct emmaprp_dev *pcdev; |
806 | struct video_device *vfd; |
807 | int irq, ret; |
808 | |
809 | pcdev = devm_kzalloc(dev: &pdev->dev, size: sizeof(*pcdev), GFP_KERNEL); |
810 | if (!pcdev) |
811 | return -ENOMEM; |
812 | |
813 | spin_lock_init(&pcdev->irqlock); |
814 | |
815 | pcdev->clk_emma_ipg = devm_clk_get(dev: &pdev->dev, id: "ipg" ); |
816 | if (IS_ERR(ptr: pcdev->clk_emma_ipg)) { |
817 | return PTR_ERR(ptr: pcdev->clk_emma_ipg); |
818 | } |
819 | |
820 | pcdev->clk_emma_ahb = devm_clk_get(dev: &pdev->dev, id: "ahb" ); |
821 | if (IS_ERR(ptr: pcdev->clk_emma_ahb)) |
822 | return PTR_ERR(ptr: pcdev->clk_emma_ahb); |
823 | |
824 | pcdev->base_emma = devm_platform_ioremap_resource(pdev, index: 0); |
825 | if (IS_ERR(ptr: pcdev->base_emma)) |
826 | return PTR_ERR(ptr: pcdev->base_emma); |
827 | |
828 | ret = v4l2_device_register(dev: &pdev->dev, v4l2_dev: &pcdev->v4l2_dev); |
829 | if (ret) |
830 | return ret; |
831 | |
832 | mutex_init(&pcdev->dev_mutex); |
833 | |
834 | vfd = video_device_alloc(); |
835 | if (!vfd) { |
836 | v4l2_err(&pcdev->v4l2_dev, "Failed to allocate video device\n" ); |
837 | ret = -ENOMEM; |
838 | goto unreg_dev; |
839 | } |
840 | |
841 | *vfd = emmaprp_videodev; |
842 | vfd->lock = &pcdev->dev_mutex; |
843 | vfd->v4l2_dev = &pcdev->v4l2_dev; |
844 | |
845 | video_set_drvdata(vdev: vfd, data: pcdev); |
846 | pcdev->vfd = vfd; |
847 | v4l2_info(&pcdev->v4l2_dev, EMMAPRP_MODULE_NAME |
848 | " Device registered as /dev/video%d\n" , vfd->num); |
849 | |
850 | platform_set_drvdata(pdev, data: pcdev); |
851 | |
852 | irq = platform_get_irq(pdev, 0); |
853 | if (irq < 0) { |
854 | ret = irq; |
855 | goto rel_vdev; |
856 | } |
857 | |
858 | ret = devm_request_irq(dev: &pdev->dev, irq, handler: emmaprp_irq, irqflags: 0, |
859 | devname: dev_name(dev: &pdev->dev), dev_id: pcdev); |
860 | if (ret) |
861 | goto rel_vdev; |
862 | |
863 | pcdev->m2m_dev = v4l2_m2m_init(m2m_ops: &m2m_ops); |
864 | if (IS_ERR(ptr: pcdev->m2m_dev)) { |
865 | v4l2_err(&pcdev->v4l2_dev, "Failed to init mem2mem device\n" ); |
866 | ret = PTR_ERR(ptr: pcdev->m2m_dev); |
867 | goto rel_vdev; |
868 | } |
869 | |
870 | ret = video_register_device(vdev: vfd, type: VFL_TYPE_VIDEO, nr: 0); |
871 | if (ret) { |
872 | v4l2_err(&pcdev->v4l2_dev, "Failed to register video device\n" ); |
873 | goto rel_m2m; |
874 | } |
875 | |
876 | return 0; |
877 | |
878 | |
879 | rel_m2m: |
880 | v4l2_m2m_release(m2m_dev: pcdev->m2m_dev); |
881 | rel_vdev: |
882 | video_device_release(vdev: vfd); |
883 | unreg_dev: |
884 | v4l2_device_unregister(v4l2_dev: &pcdev->v4l2_dev); |
885 | |
886 | mutex_destroy(lock: &pcdev->dev_mutex); |
887 | |
888 | return ret; |
889 | } |
890 | |
891 | static void emmaprp_remove(struct platform_device *pdev) |
892 | { |
893 | struct emmaprp_dev *pcdev = platform_get_drvdata(pdev); |
894 | |
895 | v4l2_info(&pcdev->v4l2_dev, "Removing " EMMAPRP_MODULE_NAME); |
896 | |
897 | video_unregister_device(vdev: pcdev->vfd); |
898 | v4l2_m2m_release(m2m_dev: pcdev->m2m_dev); |
899 | v4l2_device_unregister(v4l2_dev: &pcdev->v4l2_dev); |
900 | mutex_destroy(lock: &pcdev->dev_mutex); |
901 | } |
902 | |
903 | static struct platform_driver emmaprp_pdrv = { |
904 | .probe = emmaprp_probe, |
905 | .remove_new = emmaprp_remove, |
906 | .driver = { |
907 | .name = MEM2MEM_NAME, |
908 | }, |
909 | }; |
910 | module_platform_driver(emmaprp_pdrv); |
911 | |