1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * DW100 Hardware dewarper |
4 | * |
5 | * Copyright 2022 NXP |
6 | * Author: Xavier Roumegue (xavier.roumegue@oss.nxp.com) |
7 | * |
8 | */ |
9 | |
10 | #include <linux/clk.h> |
11 | #include <linux/debugfs.h> |
12 | #include <linux/interrupt.h> |
13 | #include <linux/io.h> |
14 | #include <linux/minmax.h> |
15 | #include <linux/module.h> |
16 | #include <linux/of.h> |
17 | #include <linux/platform_device.h> |
18 | #include <linux/pm_runtime.h> |
19 | |
20 | #include <media/v4l2-ctrls.h> |
21 | #include <media/v4l2-device.h> |
22 | #include <media/v4l2-event.h> |
23 | #include <media/v4l2-ioctl.h> |
24 | #include <media/v4l2-mem2mem.h> |
25 | #include <media/videobuf2-dma-contig.h> |
26 | |
27 | #include <uapi/linux/dw100.h> |
28 | |
29 | #include "dw100_regs.h" |
30 | |
31 | #define DRV_NAME "dw100" |
32 | |
33 | #define DW100_MIN_W 176u |
34 | #define DW100_MIN_H 144u |
35 | #define DW100_MAX_W 4096u |
36 | #define DW100_MAX_H 3072u |
37 | #define DW100_ALIGN_W 3 |
38 | #define DW100_ALIGN_H 3 |
39 | |
40 | #define DW100_BLOCK_SIZE 16 |
41 | |
42 | #define DW100_DEF_W 640u |
43 | #define DW100_DEF_H 480u |
44 | #define DW100_DEF_LUT_W (DIV_ROUND_UP(DW100_DEF_W, DW100_BLOCK_SIZE) + 1) |
45 | #define DW100_DEF_LUT_H (DIV_ROUND_UP(DW100_DEF_H, DW100_BLOCK_SIZE) + 1) |
46 | |
47 | /* |
48 | * 16 controls have been reserved for this driver for future extension, but |
49 | * let's limit the related driver allocation to the effective number of controls |
50 | * in use. |
51 | */ |
52 | #define DW100_MAX_CTRLS 1 |
53 | #define DW100_CTRL_DEWARPING_MAP 0 |
54 | |
55 | enum { |
56 | DW100_QUEUE_SRC = 0, |
57 | DW100_QUEUE_DST = 1, |
58 | }; |
59 | |
60 | enum { |
61 | DW100_FMT_CAPTURE = BIT(0), |
62 | DW100_FMT_OUTPUT = BIT(1), |
63 | }; |
64 | |
65 | struct dw100_device { |
66 | struct platform_device *pdev; |
67 | struct v4l2_m2m_dev *m2m_dev; |
68 | struct v4l2_device v4l2_dev; |
69 | struct video_device vfd; |
70 | struct media_device mdev; |
71 | /* Video device lock */ |
72 | struct mutex vfd_mutex; |
73 | void __iomem *mmio; |
74 | struct clk_bulk_data *clks; |
75 | int num_clks; |
76 | struct dentry *debugfs_root; |
77 | }; |
78 | |
79 | struct dw100_q_data { |
80 | struct v4l2_pix_format_mplane pix_fmt; |
81 | unsigned int sequence; |
82 | const struct dw100_fmt *fmt; |
83 | struct v4l2_rect crop; |
84 | }; |
85 | |
86 | struct dw100_ctx { |
87 | struct v4l2_fh fh; |
88 | struct dw100_device *dw_dev; |
89 | struct v4l2_ctrl_handler hdl; |
90 | struct v4l2_ctrl *ctrls[DW100_MAX_CTRLS]; |
91 | /* per context m2m queue lock */ |
92 | struct mutex vq_mutex; |
93 | |
94 | /* Look Up Table for pixel remapping */ |
95 | unsigned int *map; |
96 | dma_addr_t map_dma; |
97 | size_t map_size; |
98 | unsigned int map_width; |
99 | unsigned int map_height; |
100 | bool user_map_is_set; |
101 | |
102 | /* Source and destination queue data */ |
103 | struct dw100_q_data q_data[2]; |
104 | }; |
105 | |
106 | static const struct v4l2_frmsize_stepwise dw100_frmsize_stepwise = { |
107 | .min_width = DW100_MIN_W, |
108 | .min_height = DW100_MIN_H, |
109 | .max_width = DW100_MAX_W, |
110 | .max_height = DW100_MAX_H, |
111 | .step_width = 1UL << DW100_ALIGN_W, |
112 | .step_height = 1UL << DW100_ALIGN_H, |
113 | }; |
114 | |
115 | static const struct dw100_fmt { |
116 | u32 fourcc; |
117 | u32 types; |
118 | u32 reg_format; |
119 | bool reg_swap_uv; |
120 | } formats[] = { |
121 | { |
122 | .fourcc = V4L2_PIX_FMT_NV16, |
123 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
124 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_SP, |
125 | .reg_swap_uv = false, |
126 | }, { |
127 | .fourcc = V4L2_PIX_FMT_NV16M, |
128 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
129 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_SP, |
130 | .reg_swap_uv = false, |
131 | }, { |
132 | .fourcc = V4L2_PIX_FMT_NV61, |
133 | .types = DW100_FMT_CAPTURE, |
134 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_SP, |
135 | .reg_swap_uv = true, |
136 | }, { |
137 | .fourcc = V4L2_PIX_FMT_NV61M, |
138 | .types = DW100_FMT_CAPTURE, |
139 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_SP, |
140 | .reg_swap_uv = true, |
141 | }, { |
142 | .fourcc = V4L2_PIX_FMT_YUYV, |
143 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
144 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_PACKED, |
145 | .reg_swap_uv = false, |
146 | }, { |
147 | .fourcc = V4L2_PIX_FMT_UYVY, |
148 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
149 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV422_PACKED, |
150 | .reg_swap_uv = true, |
151 | }, { |
152 | .fourcc = V4L2_PIX_FMT_NV12, |
153 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
154 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV420_SP, |
155 | .reg_swap_uv = false, |
156 | }, { |
157 | .fourcc = V4L2_PIX_FMT_NV12M, |
158 | .types = DW100_FMT_OUTPUT | DW100_FMT_CAPTURE, |
159 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV420_SP, |
160 | .reg_swap_uv = false, |
161 | }, { |
162 | .fourcc = V4L2_PIX_FMT_NV21, |
163 | .types = DW100_FMT_CAPTURE, |
164 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV420_SP, |
165 | .reg_swap_uv = true, |
166 | }, { |
167 | .fourcc = V4L2_PIX_FMT_NV21M, |
168 | .types = DW100_FMT_CAPTURE, |
169 | .reg_format = DW100_DEWARP_CTRL_FORMAT_YUV420_SP, |
170 | .reg_swap_uv = true, |
171 | }, |
172 | }; |
173 | |
174 | static inline int to_dw100_fmt_type(enum v4l2_buf_type type) |
175 | { |
176 | if (V4L2_TYPE_IS_OUTPUT(type)) |
177 | return DW100_FMT_OUTPUT; |
178 | else |
179 | return DW100_FMT_CAPTURE; |
180 | } |
181 | |
182 | static const struct dw100_fmt *dw100_find_pixel_format(u32 pixel_format, |
183 | int fmt_type) |
184 | { |
185 | unsigned int i; |
186 | |
187 | for (i = 0; i < ARRAY_SIZE(formats); i++) { |
188 | const struct dw100_fmt *fmt = &formats[i]; |
189 | |
190 | if (fmt->fourcc == pixel_format && fmt->types & fmt_type) |
191 | return fmt; |
192 | } |
193 | |
194 | return NULL; |
195 | } |
196 | |
197 | static const struct dw100_fmt *dw100_find_format(struct v4l2_format *f) |
198 | { |
199 | return dw100_find_pixel_format(pixel_format: f->fmt.pix_mp.pixelformat, |
200 | fmt_type: to_dw100_fmt_type(type: f->type)); |
201 | } |
202 | |
203 | static inline u32 dw100_read(struct dw100_device *dw_dev, u32 reg) |
204 | { |
205 | return readl(addr: dw_dev->mmio + reg); |
206 | } |
207 | |
208 | static inline void dw100_write(struct dw100_device *dw_dev, u32 reg, u32 val) |
209 | { |
210 | writel(val, addr: dw_dev->mmio + reg); |
211 | } |
212 | |
213 | static inline int dw100_dump_regs(struct seq_file *m) |
214 | { |
215 | struct dw100_device *dw_dev = m->private; |
216 | #define __DECLARE_REG(x) { #x, x } |
217 | unsigned int i; |
218 | static const struct reg_desc { |
219 | const char * const name; |
220 | unsigned int addr; |
221 | } dw100_regs[] = { |
222 | __DECLARE_REG(DW100_DEWARP_ID), |
223 | __DECLARE_REG(DW100_DEWARP_CTRL), |
224 | __DECLARE_REG(DW100_MAP_LUT_ADDR), |
225 | __DECLARE_REG(DW100_MAP_LUT_SIZE), |
226 | __DECLARE_REG(DW100_MAP_LUT_ADDR2), |
227 | __DECLARE_REG(DW100_MAP_LUT_SIZE2), |
228 | __DECLARE_REG(DW100_SRC_IMG_Y_BASE), |
229 | __DECLARE_REG(DW100_SRC_IMG_UV_BASE), |
230 | __DECLARE_REG(DW100_SRC_IMG_SIZE), |
231 | __DECLARE_REG(DW100_SRC_IMG_STRIDE), |
232 | __DECLARE_REG(DW100_DST_IMG_Y_BASE), |
233 | __DECLARE_REG(DW100_DST_IMG_UV_BASE), |
234 | __DECLARE_REG(DW100_DST_IMG_SIZE), |
235 | __DECLARE_REG(DW100_DST_IMG_STRIDE), |
236 | __DECLARE_REG(DW100_DST_IMG_Y_SIZE1), |
237 | __DECLARE_REG(DW100_DST_IMG_UV_SIZE1), |
238 | __DECLARE_REG(DW100_SRC_IMG_Y_BASE2), |
239 | __DECLARE_REG(DW100_SRC_IMG_UV_BASE2), |
240 | __DECLARE_REG(DW100_SRC_IMG_SIZE2), |
241 | __DECLARE_REG(DW100_SRC_IMG_STRIDE2), |
242 | __DECLARE_REG(DW100_DST_IMG_Y_BASE2), |
243 | __DECLARE_REG(DW100_DST_IMG_UV_BASE2), |
244 | __DECLARE_REG(DW100_DST_IMG_SIZE2), |
245 | __DECLARE_REG(DW100_DST_IMG_STRIDE2), |
246 | __DECLARE_REG(DW100_DST_IMG_Y_SIZE2), |
247 | __DECLARE_REG(DW100_DST_IMG_UV_SIZE2), |
248 | __DECLARE_REG(DW100_SWAP_CONTROL), |
249 | __DECLARE_REG(DW100_VERTICAL_SPLIT_LINE), |
250 | __DECLARE_REG(DW100_HORIZON_SPLIT_LINE), |
251 | __DECLARE_REG(DW100_SCALE_FACTOR), |
252 | __DECLARE_REG(DW100_ROI_START), |
253 | __DECLARE_REG(DW100_BOUNDARY_PIXEL), |
254 | __DECLARE_REG(DW100_INTERRUPT_STATUS), |
255 | __DECLARE_REG(DW100_BUS_CTRL), |
256 | __DECLARE_REG(DW100_BUS_CTRL1), |
257 | __DECLARE_REG(DW100_BUS_TIME_OUT_CYCLE), |
258 | }; |
259 | |
260 | for (i = 0; i < ARRAY_SIZE(dw100_regs); i++) |
261 | seq_printf(m, fmt: "%s: %#x\n" , dw100_regs[i].name, |
262 | dw100_read(dw_dev, reg: dw100_regs[i].addr)); |
263 | |
264 | return 0; |
265 | } |
266 | |
267 | static inline struct dw100_ctx *dw100_file2ctx(struct file *file) |
268 | { |
269 | return container_of(file->private_data, struct dw100_ctx, fh); |
270 | } |
271 | |
272 | static struct dw100_q_data *dw100_get_q_data(struct dw100_ctx *ctx, |
273 | enum v4l2_buf_type type) |
274 | { |
275 | if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) |
276 | return &ctx->q_data[DW100_QUEUE_SRC]; |
277 | else |
278 | return &ctx->q_data[DW100_QUEUE_DST]; |
279 | } |
280 | |
281 | static u32 dw100_get_n_vertices_from_length(u32 length) |
282 | { |
283 | return DIV_ROUND_UP(length, DW100_BLOCK_SIZE) + 1; |
284 | } |
285 | |
286 | static u16 dw100_map_convert_to_uq12_4(u32 a) |
287 | { |
288 | return (u16)((a & 0xfff) << 4); |
289 | } |
290 | |
291 | static u32 dw100_map_format_coordinates(u16 xq, u16 yq) |
292 | { |
293 | return (u32)((yq << 16) | xq); |
294 | } |
295 | |
296 | static u32 *dw100_get_user_map(struct dw100_ctx *ctx) |
297 | { |
298 | struct v4l2_ctrl *ctrl = ctx->ctrls[DW100_CTRL_DEWARPING_MAP]; |
299 | |
300 | return ctrl->p_cur.p_u32; |
301 | } |
302 | |
303 | /* |
304 | * Create the dewarp map used by the hardware from the V4L2 control values which |
305 | * have been initialized with an identity map or set by the application. |
306 | */ |
307 | static int dw100_create_mapping(struct dw100_ctx *ctx) |
308 | { |
309 | u32 *user_map; |
310 | |
311 | if (ctx->map) |
312 | dma_free_coherent(dev: &ctx->dw_dev->pdev->dev, size: ctx->map_size, |
313 | cpu_addr: ctx->map, dma_handle: ctx->map_dma); |
314 | |
315 | ctx->map = dma_alloc_coherent(dev: &ctx->dw_dev->pdev->dev, size: ctx->map_size, |
316 | dma_handle: &ctx->map_dma, GFP_KERNEL); |
317 | |
318 | if (!ctx->map) |
319 | return -ENOMEM; |
320 | |
321 | user_map = dw100_get_user_map(ctx); |
322 | memcpy(ctx->map, user_map, ctx->map_size); |
323 | |
324 | dev_dbg(&ctx->dw_dev->pdev->dev, |
325 | "%ux%u %s mapping created (d:%pad-c:%p) for stream %ux%u->%ux%u\n" , |
326 | ctx->map_width, ctx->map_height, |
327 | ctx->user_map_is_set ? "user" : "identity" , |
328 | &ctx->map_dma, ctx->map, |
329 | ctx->q_data[DW100_QUEUE_SRC].pix_fmt.width, |
330 | ctx->q_data[DW100_QUEUE_DST].pix_fmt.height, |
331 | ctx->q_data[DW100_QUEUE_SRC].pix_fmt.width, |
332 | ctx->q_data[DW100_QUEUE_DST].pix_fmt.height); |
333 | |
334 | return 0; |
335 | } |
336 | |
337 | static void dw100_destroy_mapping(struct dw100_ctx *ctx) |
338 | { |
339 | if (ctx->map) { |
340 | dma_free_coherent(dev: &ctx->dw_dev->pdev->dev, size: ctx->map_size, |
341 | cpu_addr: ctx->map, dma_handle: ctx->map_dma); |
342 | ctx->map = NULL; |
343 | } |
344 | } |
345 | |
346 | static int dw100_s_ctrl(struct v4l2_ctrl *ctrl) |
347 | { |
348 | struct dw100_ctx *ctx = |
349 | container_of(ctrl->handler, struct dw100_ctx, hdl); |
350 | |
351 | switch (ctrl->id) { |
352 | case V4L2_CID_DW100_DEWARPING_16x16_VERTEX_MAP: |
353 | ctx->user_map_is_set = true; |
354 | break; |
355 | } |
356 | |
357 | return 0; |
358 | } |
359 | |
360 | static const struct v4l2_ctrl_ops dw100_ctrl_ops = { |
361 | .s_ctrl = dw100_s_ctrl, |
362 | }; |
363 | |
364 | /* |
365 | * Initialize the dewarping map with an identity mapping. |
366 | * |
367 | * A 16 pixels cell size grid is mapped on the destination image. |
368 | * The last cells width/height might be lesser than 16 if the destination image |
369 | * width/height is not divisible by 16. This dewarping grid map specifies the |
370 | * source image pixel location (x, y) on each grid intersection point. |
371 | * Bilinear interpolation is used to compute inner cell points locations. |
372 | * |
373 | * The coordinates are saved in UQ12.4 fixed point format. |
374 | */ |
375 | static void dw100_ctrl_dewarping_map_init(const struct v4l2_ctrl *ctrl, |
376 | u32 from_idx, |
377 | union v4l2_ctrl_ptr ptr) |
378 | { |
379 | struct dw100_ctx *ctx = |
380 | container_of(ctrl->handler, struct dw100_ctx, hdl); |
381 | |
382 | u32 sw, sh, mw, mh, idx; |
383 | u16 qx, qy, qdx, qdy, qsh, qsw; |
384 | u32 *map = ctrl->p_cur.p_u32; |
385 | |
386 | sw = ctx->q_data[DW100_QUEUE_SRC].pix_fmt.width; |
387 | sh = ctx->q_data[DW100_QUEUE_SRC].pix_fmt.height; |
388 | |
389 | mw = ctrl->dims[0]; |
390 | mh = ctrl->dims[1]; |
391 | |
392 | qsw = dw100_map_convert_to_uq12_4(a: sw); |
393 | qsh = dw100_map_convert_to_uq12_4(a: sh); |
394 | qdx = qsw / (mw - 1); |
395 | qdy = qsh / (mh - 1); |
396 | |
397 | ctx->map_width = mw; |
398 | ctx->map_height = mh; |
399 | ctx->map_size = mh * mw * sizeof(u32); |
400 | |
401 | for (idx = from_idx; idx < ctrl->elems; idx++) { |
402 | qy = min_t(u32, (idx / mw) * qdy, qsh); |
403 | qx = min_t(u32, (idx % mw) * qdx, qsw); |
404 | map[idx] = dw100_map_format_coordinates(xq: qx, yq: qy); |
405 | } |
406 | |
407 | ctx->user_map_is_set = false; |
408 | } |
409 | |
410 | static const struct v4l2_ctrl_type_ops dw100_ctrl_type_ops = { |
411 | .init = dw100_ctrl_dewarping_map_init, |
412 | .validate = v4l2_ctrl_type_op_validate, |
413 | .log = v4l2_ctrl_type_op_log, |
414 | .equal = v4l2_ctrl_type_op_equal, |
415 | }; |
416 | |
417 | static const struct v4l2_ctrl_config controls[] = { |
418 | [DW100_CTRL_DEWARPING_MAP] = { |
419 | .ops = &dw100_ctrl_ops, |
420 | .type_ops = &dw100_ctrl_type_ops, |
421 | .id = V4L2_CID_DW100_DEWARPING_16x16_VERTEX_MAP, |
422 | .name = "Dewarping Vertex Map" , |
423 | .type = V4L2_CTRL_TYPE_U32, |
424 | .min = 0x00000000, |
425 | .max = 0xffffffff, |
426 | .step = 1, |
427 | .def = 0, |
428 | .dims = { DW100_DEF_LUT_W, DW100_DEF_LUT_H }, |
429 | }, |
430 | }; |
431 | |
432 | static int dw100_queue_setup(struct vb2_queue *vq, |
433 | unsigned int *nbuffers, unsigned int *nplanes, |
434 | unsigned int sizes[], struct device *alloc_devs[]) |
435 | { |
436 | struct dw100_ctx *ctx = vb2_get_drv_priv(q: vq); |
437 | const struct v4l2_pix_format_mplane *format; |
438 | unsigned int i; |
439 | |
440 | format = &dw100_get_q_data(ctx, type: vq->type)->pix_fmt; |
441 | |
442 | if (*nplanes) { |
443 | if (*nplanes != format->num_planes) |
444 | return -EINVAL; |
445 | |
446 | for (i = 0; i < *nplanes; ++i) { |
447 | if (sizes[i] < format->plane_fmt[i].sizeimage) |
448 | return -EINVAL; |
449 | } |
450 | |
451 | return 0; |
452 | } |
453 | |
454 | *nplanes = format->num_planes; |
455 | |
456 | for (i = 0; i < format->num_planes; ++i) |
457 | sizes[i] = format->plane_fmt[i].sizeimage; |
458 | |
459 | return 0; |
460 | } |
461 | |
462 | static int dw100_buf_prepare(struct vb2_buffer *vb) |
463 | { |
464 | unsigned int i; |
465 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); |
466 | struct dw100_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
467 | struct dw100_device *dw_dev = ctx->dw_dev; |
468 | const struct v4l2_pix_format_mplane *pix_fmt = |
469 | &dw100_get_q_data(ctx, type: vb->vb2_queue->type)->pix_fmt; |
470 | |
471 | if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) { |
472 | if (vbuf->field != V4L2_FIELD_NONE) { |
473 | dev_dbg(&dw_dev->pdev->dev, "%x field isn't supported\n" , |
474 | vbuf->field); |
475 | return -EINVAL; |
476 | } |
477 | } |
478 | |
479 | for (i = 0; i < pix_fmt->num_planes; i++) { |
480 | unsigned long size = pix_fmt->plane_fmt[i].sizeimage; |
481 | |
482 | if (vb2_plane_size(vb, plane_no: i) < size) { |
483 | dev_dbg(&dw_dev->pdev->dev, |
484 | "User buffer too small (%lu < %lu)\n" , |
485 | vb2_plane_size(vb, i), size); |
486 | return -EINVAL; |
487 | } |
488 | |
489 | vb2_set_plane_payload(vb, plane_no: i, size); |
490 | } |
491 | |
492 | return 0; |
493 | } |
494 | |
495 | static void dw100_buf_queue(struct vb2_buffer *vb) |
496 | { |
497 | struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); |
498 | struct dw100_ctx *ctx = vb2_get_drv_priv(q: vb->vb2_queue); |
499 | |
500 | v4l2_m2m_buf_queue(m2m_ctx: ctx->fh.m2m_ctx, vbuf); |
501 | } |
502 | |
503 | static void dw100_return_all_buffers(struct vb2_queue *q, |
504 | enum vb2_buffer_state state) |
505 | { |
506 | struct dw100_ctx *ctx = vb2_get_drv_priv(q); |
507 | struct vb2_v4l2_buffer *vbuf; |
508 | |
509 | for (;;) { |
510 | if (V4L2_TYPE_IS_OUTPUT(q->type)) |
511 | vbuf = v4l2_m2m_src_buf_remove(m2m_ctx: ctx->fh.m2m_ctx); |
512 | else |
513 | vbuf = v4l2_m2m_dst_buf_remove(m2m_ctx: ctx->fh.m2m_ctx); |
514 | if (!vbuf) |
515 | return; |
516 | v4l2_m2m_buf_done(buf: vbuf, state); |
517 | } |
518 | } |
519 | |
520 | static int dw100_start_streaming(struct vb2_queue *q, unsigned int count) |
521 | { |
522 | struct dw100_ctx *ctx = vb2_get_drv_priv(q); |
523 | struct dw100_q_data *q_data = dw100_get_q_data(ctx, type: q->type); |
524 | int ret; |
525 | |
526 | q_data->sequence = 0; |
527 | |
528 | ret = dw100_create_mapping(ctx); |
529 | if (ret) |
530 | goto err; |
531 | |
532 | ret = pm_runtime_resume_and_get(dev: &ctx->dw_dev->pdev->dev); |
533 | if (ret) { |
534 | dw100_destroy_mapping(ctx); |
535 | goto err; |
536 | } |
537 | |
538 | return 0; |
539 | err: |
540 | dw100_return_all_buffers(q, state: VB2_BUF_STATE_QUEUED); |
541 | return ret; |
542 | } |
543 | |
544 | static void dw100_stop_streaming(struct vb2_queue *q) |
545 | { |
546 | struct dw100_ctx *ctx = vb2_get_drv_priv(q); |
547 | |
548 | dw100_return_all_buffers(q, state: VB2_BUF_STATE_ERROR); |
549 | |
550 | pm_runtime_put_sync(dev: &ctx->dw_dev->pdev->dev); |
551 | |
552 | dw100_destroy_mapping(ctx); |
553 | } |
554 | |
555 | static const struct vb2_ops dw100_qops = { |
556 | .queue_setup = dw100_queue_setup, |
557 | .buf_prepare = dw100_buf_prepare, |
558 | .buf_queue = dw100_buf_queue, |
559 | .start_streaming = dw100_start_streaming, |
560 | .stop_streaming = dw100_stop_streaming, |
561 | .wait_prepare = vb2_ops_wait_prepare, |
562 | .wait_finish = vb2_ops_wait_finish, |
563 | }; |
564 | |
565 | static int dw100_m2m_queue_init(void *priv, struct vb2_queue *src_vq, |
566 | struct vb2_queue *dst_vq) |
567 | { |
568 | struct dw100_ctx *ctx = priv; |
569 | int ret; |
570 | |
571 | src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; |
572 | src_vq->io_modes = VB2_MMAP | VB2_DMABUF; |
573 | src_vq->drv_priv = ctx; |
574 | src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
575 | src_vq->ops = &dw100_qops; |
576 | src_vq->mem_ops = &vb2_dma_contig_memops; |
577 | src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
578 | src_vq->lock = &ctx->vq_mutex; |
579 | src_vq->dev = ctx->dw_dev->v4l2_dev.dev; |
580 | |
581 | ret = vb2_queue_init(q: src_vq); |
582 | if (ret) |
583 | return ret; |
584 | |
585 | dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE; |
586 | dst_vq->io_modes = VB2_MMAP | VB2_DMABUF; |
587 | dst_vq->drv_priv = ctx; |
588 | dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer); |
589 | dst_vq->ops = &dw100_qops; |
590 | dst_vq->mem_ops = &vb2_dma_contig_memops; |
591 | dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY; |
592 | dst_vq->lock = &ctx->vq_mutex; |
593 | dst_vq->dev = ctx->dw_dev->v4l2_dev.dev; |
594 | |
595 | return vb2_queue_init(q: dst_vq); |
596 | } |
597 | |
598 | static int dw100_open(struct file *file) |
599 | { |
600 | struct dw100_device *dw_dev = video_drvdata(file); |
601 | struct dw100_ctx *ctx; |
602 | struct v4l2_ctrl_handler *hdl; |
603 | struct v4l2_pix_format_mplane *pix_fmt; |
604 | int ret, i; |
605 | |
606 | ctx = kzalloc(size: sizeof(*ctx), GFP_KERNEL); |
607 | if (!ctx) |
608 | return -ENOMEM; |
609 | |
610 | mutex_init(&ctx->vq_mutex); |
611 | v4l2_fh_init(fh: &ctx->fh, vdev: video_devdata(file)); |
612 | file->private_data = &ctx->fh; |
613 | ctx->dw_dev = dw_dev; |
614 | |
615 | ctx->q_data[DW100_QUEUE_SRC].fmt = &formats[0]; |
616 | |
617 | pix_fmt = &ctx->q_data[DW100_QUEUE_SRC].pix_fmt; |
618 | pix_fmt->field = V4L2_FIELD_NONE; |
619 | pix_fmt->colorspace = V4L2_COLORSPACE_REC709; |
620 | pix_fmt->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix_fmt->colorspace); |
621 | pix_fmt->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix_fmt->colorspace); |
622 | pix_fmt->quantization = |
623 | V4L2_MAP_QUANTIZATION_DEFAULT(false, pix_fmt->colorspace, |
624 | pix_fmt->ycbcr_enc); |
625 | |
626 | v4l2_fill_pixfmt_mp(pixfmt: pix_fmt, pixelformat: formats[0].fourcc, DW100_DEF_W, DW100_DEF_H); |
627 | |
628 | ctx->q_data[DW100_QUEUE_SRC].crop.top = 0; |
629 | ctx->q_data[DW100_QUEUE_SRC].crop.left = 0; |
630 | ctx->q_data[DW100_QUEUE_SRC].crop.width = DW100_DEF_W; |
631 | ctx->q_data[DW100_QUEUE_SRC].crop.height = DW100_DEF_H; |
632 | |
633 | ctx->q_data[DW100_QUEUE_DST] = ctx->q_data[DW100_QUEUE_SRC]; |
634 | |
635 | hdl = &ctx->hdl; |
636 | v4l2_ctrl_handler_init(hdl, ARRAY_SIZE(controls)); |
637 | for (i = 0; i < ARRAY_SIZE(controls); i++) { |
638 | ctx->ctrls[i] = v4l2_ctrl_new_custom(hdl, cfg: &controls[i], NULL); |
639 | if (hdl->error) { |
640 | dev_err(&ctx->dw_dev->pdev->dev, |
641 | "Adding control (%d) failed\n" , i); |
642 | ret = hdl->error; |
643 | goto err; |
644 | } |
645 | } |
646 | ctx->fh.ctrl_handler = hdl; |
647 | |
648 | ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(m2m_dev: dw_dev->m2m_dev, |
649 | drv_priv: ctx, queue_init: &dw100_m2m_queue_init); |
650 | |
651 | if (IS_ERR(ptr: ctx->fh.m2m_ctx)) { |
652 | ret = PTR_ERR(ptr: ctx->fh.m2m_ctx); |
653 | goto err; |
654 | } |
655 | |
656 | v4l2_fh_add(fh: &ctx->fh); |
657 | |
658 | return 0; |
659 | |
660 | err: |
661 | v4l2_ctrl_handler_free(hdl); |
662 | v4l2_fh_exit(fh: &ctx->fh); |
663 | mutex_destroy(lock: &ctx->vq_mutex); |
664 | kfree(objp: ctx); |
665 | |
666 | return ret; |
667 | } |
668 | |
669 | static int dw100_release(struct file *file) |
670 | { |
671 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
672 | |
673 | v4l2_fh_del(fh: &ctx->fh); |
674 | v4l2_fh_exit(fh: &ctx->fh); |
675 | v4l2_ctrl_handler_free(hdl: &ctx->hdl); |
676 | v4l2_m2m_ctx_release(m2m_ctx: ctx->fh.m2m_ctx); |
677 | mutex_destroy(lock: &ctx->vq_mutex); |
678 | kfree(objp: ctx); |
679 | |
680 | return 0; |
681 | } |
682 | |
683 | static const struct v4l2_file_operations dw100_fops = { |
684 | .owner = THIS_MODULE, |
685 | .open = dw100_open, |
686 | .release = dw100_release, |
687 | .poll = v4l2_m2m_fop_poll, |
688 | .unlocked_ioctl = video_ioctl2, |
689 | .mmap = v4l2_m2m_fop_mmap, |
690 | }; |
691 | |
692 | static int dw100_querycap(struct file *file, void *priv, |
693 | struct v4l2_capability *cap) |
694 | { |
695 | strscpy(cap->driver, DRV_NAME, sizeof(cap->driver)); |
696 | strscpy(cap->card, "DW100 dewarper" , sizeof(cap->card)); |
697 | |
698 | return 0; |
699 | } |
700 | |
701 | static int dw100_enum_fmt_vid(struct file *file, void *priv, |
702 | struct v4l2_fmtdesc *f) |
703 | { |
704 | int i, num = 0; |
705 | |
706 | for (i = 0; i < ARRAY_SIZE(formats); i++) { |
707 | if (formats[i].types & to_dw100_fmt_type(type: f->type)) { |
708 | if (num == f->index) { |
709 | f->pixelformat = formats[i].fourcc; |
710 | return 0; |
711 | } |
712 | ++num; |
713 | } |
714 | } |
715 | |
716 | return -EINVAL; |
717 | } |
718 | |
719 | static int dw100_enum_framesizes(struct file *file, void *priv, |
720 | struct v4l2_frmsizeenum *fsize) |
721 | { |
722 | const struct dw100_fmt *fmt; |
723 | |
724 | if (fsize->index) |
725 | return -EINVAL; |
726 | |
727 | fmt = dw100_find_pixel_format(pixel_format: fsize->pixel_format, |
728 | fmt_type: DW100_FMT_OUTPUT | DW100_FMT_CAPTURE); |
729 | if (!fmt) |
730 | return -EINVAL; |
731 | |
732 | fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE; |
733 | fsize->stepwise = dw100_frmsize_stepwise; |
734 | |
735 | return 0; |
736 | } |
737 | |
738 | static int dw100_g_fmt_vid(struct file *file, void *priv, struct v4l2_format *f) |
739 | { |
740 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
741 | struct vb2_queue *vq; |
742 | struct dw100_q_data *q_data; |
743 | |
744 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
745 | if (!vq) |
746 | return -EINVAL; |
747 | |
748 | q_data = dw100_get_q_data(ctx, type: f->type); |
749 | |
750 | f->fmt.pix_mp = q_data->pix_fmt; |
751 | |
752 | return 0; |
753 | } |
754 | |
755 | static int dw100_try_fmt(struct file *file, struct v4l2_format *f) |
756 | { |
757 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
758 | struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp; |
759 | const struct dw100_fmt *fmt; |
760 | |
761 | fmt = dw100_find_format(f); |
762 | if (!fmt) { |
763 | fmt = &formats[0]; |
764 | pix->pixelformat = fmt->fourcc; |
765 | } |
766 | |
767 | v4l2_apply_frmsize_constraints(width: &pix->width, height: &pix->height, |
768 | frmsize: &dw100_frmsize_stepwise); |
769 | |
770 | v4l2_fill_pixfmt_mp(pixfmt: pix, pixelformat: fmt->fourcc, width: pix->width, height: pix->height); |
771 | |
772 | pix->field = V4L2_FIELD_NONE; |
773 | |
774 | if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) { |
775 | if (pix->colorspace == V4L2_COLORSPACE_DEFAULT) |
776 | pix->colorspace = V4L2_COLORSPACE_REC709; |
777 | if (pix->xfer_func == V4L2_XFER_FUNC_DEFAULT) |
778 | pix->xfer_func = V4L2_MAP_XFER_FUNC_DEFAULT(pix->colorspace); |
779 | if (pix->ycbcr_enc == V4L2_YCBCR_ENC_DEFAULT) |
780 | pix->ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(pix->colorspace); |
781 | if (pix->quantization == V4L2_QUANTIZATION_DEFAULT) |
782 | pix->quantization = |
783 | V4L2_MAP_QUANTIZATION_DEFAULT(false, |
784 | pix->colorspace, |
785 | pix->ycbcr_enc); |
786 | } else { |
787 | /* |
788 | * The DW100 can't perform colorspace conversion, the colorspace |
789 | * on the capture queue must be identical to the output queue. |
790 | */ |
791 | const struct dw100_q_data *q_data = |
792 | dw100_get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE); |
793 | |
794 | pix->colorspace = q_data->pix_fmt.colorspace; |
795 | pix->xfer_func = q_data->pix_fmt.xfer_func; |
796 | pix->ycbcr_enc = q_data->pix_fmt.ycbcr_enc; |
797 | pix->quantization = q_data->pix_fmt.quantization; |
798 | } |
799 | |
800 | return 0; |
801 | } |
802 | |
803 | static int dw100_s_fmt(struct dw100_ctx *ctx, struct v4l2_format *f) |
804 | { |
805 | struct dw100_q_data *q_data; |
806 | struct vb2_queue *vq; |
807 | |
808 | vq = v4l2_m2m_get_vq(m2m_ctx: ctx->fh.m2m_ctx, type: f->type); |
809 | if (!vq) |
810 | return -EINVAL; |
811 | |
812 | q_data = dw100_get_q_data(ctx, type: f->type); |
813 | if (!q_data) |
814 | return -EINVAL; |
815 | |
816 | if (vb2_is_busy(q: vq)) { |
817 | dev_dbg(&ctx->dw_dev->pdev->dev, "%s queue busy\n" , __func__); |
818 | return -EBUSY; |
819 | } |
820 | |
821 | q_data->fmt = dw100_find_format(f); |
822 | q_data->pix_fmt = f->fmt.pix_mp; |
823 | q_data->crop.top = 0; |
824 | q_data->crop.left = 0; |
825 | q_data->crop.width = f->fmt.pix_mp.width; |
826 | q_data->crop.height = f->fmt.pix_mp.height; |
827 | |
828 | /* Propagate buffers encoding */ |
829 | |
830 | if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) { |
831 | struct dw100_q_data *dst_q_data = |
832 | dw100_get_q_data(ctx, |
833 | type: V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE); |
834 | |
835 | dst_q_data->pix_fmt.colorspace = q_data->pix_fmt.colorspace; |
836 | dst_q_data->pix_fmt.ycbcr_enc = q_data->pix_fmt.ycbcr_enc; |
837 | dst_q_data->pix_fmt.quantization = q_data->pix_fmt.quantization; |
838 | dst_q_data->pix_fmt.xfer_func = q_data->pix_fmt.xfer_func; |
839 | } |
840 | |
841 | dev_dbg(&ctx->dw_dev->pdev->dev, |
842 | "Setting format for type %u, wxh: %ux%u, fmt: %p4cc\n" , |
843 | f->type, q_data->pix_fmt.width, q_data->pix_fmt.height, |
844 | &q_data->pix_fmt.pixelformat); |
845 | |
846 | if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) { |
847 | int ret; |
848 | u32 dims[V4L2_CTRL_MAX_DIMS] = {}; |
849 | struct v4l2_ctrl *ctrl = ctx->ctrls[DW100_CTRL_DEWARPING_MAP]; |
850 | |
851 | dims[0] = dw100_get_n_vertices_from_length(length: q_data->pix_fmt.width); |
852 | dims[1] = dw100_get_n_vertices_from_length(length: q_data->pix_fmt.height); |
853 | |
854 | ret = v4l2_ctrl_modify_dimensions(ctrl, dims); |
855 | |
856 | if (ret) { |
857 | dev_err(&ctx->dw_dev->pdev->dev, |
858 | "Modifying LUT dimensions failed with error %d\n" , |
859 | ret); |
860 | return ret; |
861 | } |
862 | } |
863 | |
864 | return 0; |
865 | } |
866 | |
867 | static int dw100_try_fmt_vid_cap(struct file *file, void *priv, |
868 | struct v4l2_format *f) |
869 | { |
870 | if (f->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) |
871 | return -EINVAL; |
872 | |
873 | return dw100_try_fmt(file, f); |
874 | } |
875 | |
876 | static int dw100_s_fmt_vid_cap(struct file *file, void *priv, |
877 | struct v4l2_format *f) |
878 | { |
879 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
880 | int ret; |
881 | |
882 | ret = dw100_try_fmt_vid_cap(file, priv, f); |
883 | if (ret) |
884 | return ret; |
885 | |
886 | ret = dw100_s_fmt(ctx, f); |
887 | if (ret) |
888 | return ret; |
889 | |
890 | return 0; |
891 | } |
892 | |
893 | static int dw100_try_fmt_vid_out(struct file *file, void *priv, |
894 | struct v4l2_format *f) |
895 | { |
896 | if (f->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) |
897 | return -EINVAL; |
898 | |
899 | return dw100_try_fmt(file, f); |
900 | } |
901 | |
902 | static int dw100_s_fmt_vid_out(struct file *file, void *priv, |
903 | struct v4l2_format *f) |
904 | { |
905 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
906 | int ret; |
907 | |
908 | ret = dw100_try_fmt_vid_out(file, priv, f); |
909 | if (ret) |
910 | return ret; |
911 | |
912 | ret = dw100_s_fmt(ctx, f); |
913 | if (ret) |
914 | return ret; |
915 | |
916 | return 0; |
917 | } |
918 | |
919 | static int dw100_g_selection(struct file *file, void *fh, |
920 | struct v4l2_selection *sel) |
921 | { |
922 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
923 | struct dw100_q_data *src_q_data; |
924 | |
925 | if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) |
926 | return -EINVAL; |
927 | |
928 | src_q_data = dw100_get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE); |
929 | |
930 | switch (sel->target) { |
931 | case V4L2_SEL_TGT_CROP_DEFAULT: |
932 | case V4L2_SEL_TGT_CROP_BOUNDS: |
933 | sel->r.top = 0; |
934 | sel->r.left = 0; |
935 | sel->r.width = src_q_data->pix_fmt.width; |
936 | sel->r.height = src_q_data->pix_fmt.height; |
937 | break; |
938 | case V4L2_SEL_TGT_CROP: |
939 | sel->r.top = src_q_data->crop.top; |
940 | sel->r.left = src_q_data->crop.left; |
941 | sel->r.width = src_q_data->crop.width; |
942 | sel->r.height = src_q_data->crop.height; |
943 | break; |
944 | default: |
945 | return -EINVAL; |
946 | } |
947 | |
948 | return 0; |
949 | } |
950 | |
951 | static int dw100_s_selection(struct file *file, void *fh, |
952 | struct v4l2_selection *sel) |
953 | { |
954 | struct dw100_ctx *ctx = dw100_file2ctx(file); |
955 | struct dw100_q_data *src_q_data; |
956 | u32 qscalex, qscaley, qscale; |
957 | int x, y, w, h; |
958 | unsigned int wframe, hframe; |
959 | |
960 | if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) |
961 | return -EINVAL; |
962 | |
963 | src_q_data = dw100_get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE); |
964 | |
965 | dev_dbg(&ctx->dw_dev->pdev->dev, |
966 | ">>> Buffer Type: %u Target: %u Rect: %ux%u@%d.%d\n" , |
967 | sel->type, sel->target, |
968 | sel->r.width, sel->r.height, sel->r.left, sel->r.top); |
969 | |
970 | switch (sel->target) { |
971 | case V4L2_SEL_TGT_CROP: |
972 | wframe = src_q_data->pix_fmt.width; |
973 | hframe = src_q_data->pix_fmt.height; |
974 | |
975 | sel->r.top = clamp_t(int, sel->r.top, 0, hframe - DW100_MIN_H); |
976 | sel->r.left = clamp_t(int, sel->r.left, 0, wframe - DW100_MIN_W); |
977 | sel->r.height = |
978 | clamp(sel->r.height, DW100_MIN_H, hframe - sel->r.top); |
979 | sel->r.width = |
980 | clamp(sel->r.width, DW100_MIN_W, wframe - sel->r.left); |
981 | |
982 | /* UQ16.16 for float operations */ |
983 | qscalex = (sel->r.width << 16) / wframe; |
984 | qscaley = (sel->r.height << 16) / hframe; |
985 | y = sel->r.top; |
986 | x = sel->r.left; |
987 | if (qscalex == qscaley) { |
988 | qscale = qscalex; |
989 | } else { |
990 | switch (sel->flags) { |
991 | case 0: |
992 | qscale = (qscalex + qscaley) / 2; |
993 | break; |
994 | case V4L2_SEL_FLAG_GE: |
995 | qscale = max(qscaley, qscalex); |
996 | break; |
997 | case V4L2_SEL_FLAG_LE: |
998 | qscale = min(qscaley, qscalex); |
999 | break; |
1000 | case V4L2_SEL_FLAG_LE | V4L2_SEL_FLAG_GE: |
1001 | return -ERANGE; |
1002 | default: |
1003 | return -EINVAL; |
1004 | } |
1005 | } |
1006 | |
1007 | w = (u32)((((u64)wframe << 16) * qscale) >> 32); |
1008 | h = (u32)((((u64)hframe << 16) * qscale) >> 32); |
1009 | x = x + (sel->r.width - w) / 2; |
1010 | y = y + (sel->r.height - h) / 2; |
1011 | x = min(wframe - w, (unsigned int)max(0, x)); |
1012 | y = min(hframe - h, (unsigned int)max(0, y)); |
1013 | |
1014 | sel->r.top = y; |
1015 | sel->r.left = x; |
1016 | sel->r.width = w; |
1017 | sel->r.height = h; |
1018 | |
1019 | src_q_data->crop.top = sel->r.top; |
1020 | src_q_data->crop.left = sel->r.left; |
1021 | src_q_data->crop.width = sel->r.width; |
1022 | src_q_data->crop.height = sel->r.height; |
1023 | break; |
1024 | |
1025 | default: |
1026 | return -EINVAL; |
1027 | } |
1028 | |
1029 | dev_dbg(&ctx->dw_dev->pdev->dev, |
1030 | "<<< Buffer Type: %u Target: %u Rect: %ux%u@%d.%d\n" , |
1031 | sel->type, sel->target, |
1032 | sel->r.width, sel->r.height, sel->r.left, sel->r.top); |
1033 | |
1034 | return 0; |
1035 | } |
1036 | |
1037 | static const struct v4l2_ioctl_ops dw100_ioctl_ops = { |
1038 | .vidioc_querycap = dw100_querycap, |
1039 | |
1040 | .vidioc_enum_fmt_vid_cap = dw100_enum_fmt_vid, |
1041 | .vidioc_enum_framesizes = dw100_enum_framesizes, |
1042 | .vidioc_g_fmt_vid_cap_mplane = dw100_g_fmt_vid, |
1043 | .vidioc_try_fmt_vid_cap_mplane = dw100_try_fmt_vid_cap, |
1044 | .vidioc_s_fmt_vid_cap_mplane = dw100_s_fmt_vid_cap, |
1045 | |
1046 | .vidioc_enum_fmt_vid_out = dw100_enum_fmt_vid, |
1047 | .vidioc_g_fmt_vid_out_mplane = dw100_g_fmt_vid, |
1048 | .vidioc_try_fmt_vid_out_mplane = dw100_try_fmt_vid_out, |
1049 | .vidioc_s_fmt_vid_out_mplane = dw100_s_fmt_vid_out, |
1050 | |
1051 | .vidioc_g_selection = dw100_g_selection, |
1052 | .vidioc_s_selection = dw100_s_selection, |
1053 | .vidioc_reqbufs = v4l2_m2m_ioctl_reqbufs, |
1054 | .vidioc_querybuf = v4l2_m2m_ioctl_querybuf, |
1055 | .vidioc_qbuf = v4l2_m2m_ioctl_qbuf, |
1056 | .vidioc_dqbuf = v4l2_m2m_ioctl_dqbuf, |
1057 | .vidioc_prepare_buf = v4l2_m2m_ioctl_prepare_buf, |
1058 | .vidioc_create_bufs = v4l2_m2m_ioctl_create_bufs, |
1059 | .vidioc_expbuf = v4l2_m2m_ioctl_expbuf, |
1060 | |
1061 | .vidioc_streamon = v4l2_m2m_ioctl_streamon, |
1062 | .vidioc_streamoff = v4l2_m2m_ioctl_streamoff, |
1063 | |
1064 | .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, |
1065 | .vidioc_unsubscribe_event = v4l2_event_unsubscribe, |
1066 | }; |
1067 | |
1068 | static void dw100_job_finish(struct dw100_device *dw_dev, bool with_error) |
1069 | { |
1070 | struct dw100_ctx *curr_ctx; |
1071 | struct vb2_v4l2_buffer *src_vb, *dst_vb; |
1072 | enum vb2_buffer_state buf_state; |
1073 | |
1074 | curr_ctx = v4l2_m2m_get_curr_priv(m2m_dev: dw_dev->m2m_dev); |
1075 | |
1076 | if (!curr_ctx) { |
1077 | dev_err(&dw_dev->pdev->dev, |
1078 | "Instance released before the end of transaction\n" ); |
1079 | return; |
1080 | } |
1081 | |
1082 | src_vb = v4l2_m2m_src_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
1083 | dst_vb = v4l2_m2m_dst_buf_remove(m2m_ctx: curr_ctx->fh.m2m_ctx); |
1084 | |
1085 | if (likely(!with_error)) |
1086 | buf_state = VB2_BUF_STATE_DONE; |
1087 | else |
1088 | buf_state = VB2_BUF_STATE_ERROR; |
1089 | |
1090 | v4l2_m2m_buf_done(buf: src_vb, state: buf_state); |
1091 | v4l2_m2m_buf_done(buf: dst_vb, state: buf_state); |
1092 | |
1093 | dev_dbg(&dw_dev->pdev->dev, "Finishing transaction with%s error(s)\n" , |
1094 | with_error ? "" : "out" ); |
1095 | |
1096 | v4l2_m2m_job_finish(m2m_dev: dw_dev->m2m_dev, m2m_ctx: curr_ctx->fh.m2m_ctx); |
1097 | } |
1098 | |
1099 | static void dw100_hw_reset(struct dw100_device *dw_dev) |
1100 | { |
1101 | u32 val; |
1102 | |
1103 | val = dw100_read(dw_dev, DW100_DEWARP_CTRL); |
1104 | val |= DW100_DEWARP_CTRL_ENABLE; |
1105 | val |= DW100_DEWARP_CTRL_SOFT_RESET; |
1106 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1107 | val &= ~DW100_DEWARP_CTRL_SOFT_RESET; |
1108 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1109 | } |
1110 | |
1111 | static void _dw100_hw_set_master_bus_enable(struct dw100_device *dw_dev, |
1112 | unsigned int enable) |
1113 | { |
1114 | u32 val; |
1115 | |
1116 | dev_dbg(&dw_dev->pdev->dev, "%sable master bus\n" , |
1117 | enable ? "En" : "Dis" ); |
1118 | |
1119 | val = dw100_read(dw_dev, DW100_BUS_CTRL); |
1120 | |
1121 | if (enable) |
1122 | val |= DW100_BUS_CTRL_AXI_MASTER_ENABLE; |
1123 | else |
1124 | val &= ~DW100_BUS_CTRL_AXI_MASTER_ENABLE; |
1125 | |
1126 | dw100_write(dw_dev, DW100_BUS_CTRL, val); |
1127 | } |
1128 | |
1129 | static void dw100_hw_master_bus_enable(struct dw100_device *dw_dev) |
1130 | { |
1131 | _dw100_hw_set_master_bus_enable(dw_dev, enable: 1); |
1132 | } |
1133 | |
1134 | static void dw100_hw_master_bus_disable(struct dw100_device *dw_dev) |
1135 | { |
1136 | _dw100_hw_set_master_bus_enable(dw_dev, enable: 0); |
1137 | } |
1138 | |
1139 | static void dw100_hw_dewarp_start(struct dw100_device *dw_dev) |
1140 | { |
1141 | u32 val; |
1142 | |
1143 | val = dw100_read(dw_dev, DW100_DEWARP_CTRL); |
1144 | |
1145 | dev_dbg(&dw_dev->pdev->dev, "Starting Hardware CTRL:0x%08x\n" , val); |
1146 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val: val | DW100_DEWARP_CTRL_START); |
1147 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1148 | } |
1149 | |
1150 | static void dw100_hw_init_ctrl(struct dw100_device *dw_dev) |
1151 | { |
1152 | u32 val; |
1153 | /* |
1154 | * Input format YUV422_SP |
1155 | * Output format YUV422_SP |
1156 | * No hardware handshake (SW) |
1157 | * No automatic double src buffering (Single) |
1158 | * No automatic double dst buffering (Single) |
1159 | * No Black Line |
1160 | * Prefetch image pixel traversal |
1161 | */ |
1162 | |
1163 | val = DW100_DEWARP_CTRL_ENABLE |
1164 | /* Valid only for auto prefetch mode*/ |
1165 | | DW100_DEWARP_CTRL_PREFETCH_THRESHOLD(32); |
1166 | |
1167 | /* |
1168 | * Calculation mode required to support any scaling factor, |
1169 | * but x4 slower than traversal mode. |
1170 | * |
1171 | * DW100_DEWARP_CTRL_PREFETCH_MODE_TRAVERSAL |
1172 | * DW100_DEWARP_CTRL_PREFETCH_MODE_CALCULATION |
1173 | * DW100_DEWARP_CTRL_PREFETCH_MODE_AUTO |
1174 | * |
1175 | * TODO: Find heuristics requiring calculation mode |
1176 | */ |
1177 | val |= DW100_DEWARP_CTRL_PREFETCH_MODE_CALCULATION; |
1178 | |
1179 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1180 | } |
1181 | |
1182 | static void dw100_hw_set_pixel_boundary(struct dw100_device *dw_dev) |
1183 | { |
1184 | u32 val; |
1185 | |
1186 | val = DW100_BOUNDARY_PIXEL_V(128) |
1187 | | DW100_BOUNDARY_PIXEL_U(128) |
1188 | | DW100_BOUNDARY_PIXEL_Y(0); |
1189 | |
1190 | dw100_write(dw_dev, DW100_BOUNDARY_PIXEL, val); |
1191 | } |
1192 | |
1193 | static void dw100_hw_set_scale(struct dw100_device *dw_dev, u8 scale) |
1194 | { |
1195 | dev_dbg(&dw_dev->pdev->dev, "Setting scale factor to %u\n" , scale); |
1196 | |
1197 | dw100_write(dw_dev, DW100_SCALE_FACTOR, val: scale); |
1198 | } |
1199 | |
1200 | static void dw100_hw_set_roi(struct dw100_device *dw_dev, u32 x, u32 y) |
1201 | { |
1202 | u32 val; |
1203 | |
1204 | dev_dbg(&dw_dev->pdev->dev, "Setting ROI region to %u.%u\n" , x, y); |
1205 | |
1206 | val = DW100_ROI_START_X(x) | DW100_ROI_START_Y(y); |
1207 | |
1208 | dw100_write(dw_dev, DW100_ROI_START, val); |
1209 | } |
1210 | |
1211 | static void dw100_hw_set_src_crop(struct dw100_device *dw_dev, |
1212 | const struct dw100_q_data *src_q_data, |
1213 | const struct dw100_q_data *dst_q_data) |
1214 | { |
1215 | const struct v4l2_rect *rect = &src_q_data->crop; |
1216 | u32 src_scale, qscale, left_scale, top_scale; |
1217 | |
1218 | /* HW Scale is UQ1.7 encoded */ |
1219 | src_scale = (rect->width << 7) / src_q_data->pix_fmt.width; |
1220 | dw100_hw_set_scale(dw_dev, scale: src_scale); |
1221 | |
1222 | qscale = (dst_q_data->pix_fmt.width << 7) / src_q_data->pix_fmt.width; |
1223 | |
1224 | left_scale = ((rect->left << 7) * qscale) >> 14; |
1225 | top_scale = ((rect->top << 7) * qscale) >> 14; |
1226 | |
1227 | dw100_hw_set_roi(dw_dev, x: left_scale, y: top_scale); |
1228 | } |
1229 | |
1230 | static void dw100_hw_set_source(struct dw100_device *dw_dev, |
1231 | const struct dw100_q_data *q_data, |
1232 | struct vb2_buffer *buffer) |
1233 | { |
1234 | u32 width, height, stride, fourcc, val; |
1235 | const struct dw100_fmt *fmt = q_data->fmt; |
1236 | dma_addr_t addr_y = vb2_dma_contig_plane_dma_addr(vb: buffer, plane_no: 0); |
1237 | dma_addr_t addr_uv; |
1238 | |
1239 | width = q_data->pix_fmt.width; |
1240 | height = q_data->pix_fmt.height; |
1241 | stride = q_data->pix_fmt.plane_fmt[0].bytesperline; |
1242 | fourcc = q_data->fmt->fourcc; |
1243 | |
1244 | if (q_data->pix_fmt.num_planes == 2) |
1245 | addr_uv = vb2_dma_contig_plane_dma_addr(vb: buffer, plane_no: 1); |
1246 | else |
1247 | addr_uv = addr_y + (stride * height); |
1248 | |
1249 | dev_dbg(&dw_dev->pdev->dev, |
1250 | "Set HW source registers for %ux%u - stride %u, pixfmt: %p4cc, dma:%pad\n" , |
1251 | width, height, stride, &fourcc, &addr_y); |
1252 | |
1253 | /* Pixel Format */ |
1254 | val = dw100_read(dw_dev, DW100_DEWARP_CTRL); |
1255 | |
1256 | val &= ~DW100_DEWARP_CTRL_INPUT_FORMAT_MASK; |
1257 | val |= DW100_DEWARP_CTRL_INPUT_FORMAT(fmt->reg_format); |
1258 | |
1259 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1260 | |
1261 | /* Swap */ |
1262 | val = dw100_read(dw_dev, DW100_SWAP_CONTROL); |
1263 | |
1264 | val &= ~DW100_SWAP_CONTROL_SRC_MASK; |
1265 | /* |
1266 | * Data swapping is performed only on Y plane for source image. |
1267 | */ |
1268 | if (fmt->reg_swap_uv && |
1269 | fmt->reg_format == DW100_DEWARP_CTRL_FORMAT_YUV422_PACKED) |
1270 | val |= DW100_SWAP_CONTROL_SRC(DW100_SWAP_CONTROL_Y |
1271 | (DW100_SWAP_CONTROL_BYTE)); |
1272 | |
1273 | dw100_write(dw_dev, DW100_SWAP_CONTROL, val); |
1274 | |
1275 | /* Image resolution */ |
1276 | dw100_write(dw_dev, DW100_SRC_IMG_SIZE, |
1277 | DW100_IMG_SIZE_WIDTH(width) | DW100_IMG_SIZE_HEIGHT(height)); |
1278 | |
1279 | dw100_write(dw_dev, DW100_SRC_IMG_STRIDE, val: stride); |
1280 | |
1281 | /* Buffers */ |
1282 | dw100_write(dw_dev, DW100_SRC_IMG_Y_BASE, DW100_IMG_Y_BASE(addr_y)); |
1283 | dw100_write(dw_dev, DW100_SRC_IMG_UV_BASE, DW100_IMG_UV_BASE(addr_uv)); |
1284 | } |
1285 | |
1286 | static void dw100_hw_set_destination(struct dw100_device *dw_dev, |
1287 | const struct dw100_q_data *q_data, |
1288 | const struct dw100_fmt *ifmt, |
1289 | struct vb2_buffer *buffer) |
1290 | { |
1291 | u32 width, height, stride, fourcc, val, size_y, size_uv; |
1292 | const struct dw100_fmt *fmt = q_data->fmt; |
1293 | dma_addr_t addr_y, addr_uv; |
1294 | |
1295 | width = q_data->pix_fmt.width; |
1296 | height = q_data->pix_fmt.height; |
1297 | stride = q_data->pix_fmt.plane_fmt[0].bytesperline; |
1298 | fourcc = fmt->fourcc; |
1299 | |
1300 | addr_y = vb2_dma_contig_plane_dma_addr(vb: buffer, plane_no: 0); |
1301 | size_y = q_data->pix_fmt.plane_fmt[0].sizeimage; |
1302 | |
1303 | if (q_data->pix_fmt.num_planes == 2) { |
1304 | addr_uv = vb2_dma_contig_plane_dma_addr(vb: buffer, plane_no: 1); |
1305 | size_uv = q_data->pix_fmt.plane_fmt[1].sizeimage; |
1306 | } else { |
1307 | addr_uv = addr_y + ALIGN(stride * height, 16); |
1308 | size_uv = size_y; |
1309 | if (fmt->reg_format == DW100_DEWARP_CTRL_FORMAT_YUV420_SP) |
1310 | size_uv /= 2; |
1311 | } |
1312 | |
1313 | dev_dbg(&dw_dev->pdev->dev, |
1314 | "Set HW source registers for %ux%u - stride %u, pixfmt: %p4cc, dma:%pad\n" , |
1315 | width, height, stride, &fourcc, &addr_y); |
1316 | |
1317 | /* Pixel Format */ |
1318 | val = dw100_read(dw_dev, DW100_DEWARP_CTRL); |
1319 | |
1320 | val &= ~DW100_DEWARP_CTRL_OUTPUT_FORMAT_MASK; |
1321 | val |= DW100_DEWARP_CTRL_OUTPUT_FORMAT(fmt->reg_format); |
1322 | |
1323 | dw100_write(dw_dev, DW100_DEWARP_CTRL, val); |
1324 | |
1325 | /* Swap */ |
1326 | val = dw100_read(dw_dev, DW100_SWAP_CONTROL); |
1327 | |
1328 | val &= ~DW100_SWAP_CONTROL_DST_MASK; |
1329 | |
1330 | /* |
1331 | * Avoid to swap twice |
1332 | */ |
1333 | if (fmt->reg_swap_uv ^ |
1334 | (ifmt->reg_swap_uv && ifmt->reg_format != |
1335 | DW100_DEWARP_CTRL_FORMAT_YUV422_PACKED)) { |
1336 | if (fmt->reg_format == DW100_DEWARP_CTRL_FORMAT_YUV422_PACKED) |
1337 | val |= DW100_SWAP_CONTROL_DST(DW100_SWAP_CONTROL_Y |
1338 | (DW100_SWAP_CONTROL_BYTE)); |
1339 | else |
1340 | val |= DW100_SWAP_CONTROL_DST(DW100_SWAP_CONTROL_UV |
1341 | (DW100_SWAP_CONTROL_BYTE)); |
1342 | } |
1343 | |
1344 | dw100_write(dw_dev, DW100_SWAP_CONTROL, val); |
1345 | |
1346 | /* Image resolution */ |
1347 | dw100_write(dw_dev, DW100_DST_IMG_SIZE, |
1348 | DW100_IMG_SIZE_WIDTH(width) | DW100_IMG_SIZE_HEIGHT(height)); |
1349 | dw100_write(dw_dev, DW100_DST_IMG_STRIDE, val: stride); |
1350 | dw100_write(dw_dev, DW100_DST_IMG_Y_BASE, DW100_IMG_Y_BASE(addr_y)); |
1351 | dw100_write(dw_dev, DW100_DST_IMG_UV_BASE, DW100_IMG_UV_BASE(addr_uv)); |
1352 | dw100_write(dw_dev, DW100_DST_IMG_Y_SIZE1, DW100_DST_IMG_Y_SIZE(size_y)); |
1353 | dw100_write(dw_dev, DW100_DST_IMG_UV_SIZE1, |
1354 | DW100_DST_IMG_UV_SIZE(size_uv)); |
1355 | } |
1356 | |
1357 | static void dw100_hw_set_mapping(struct dw100_device *dw_dev, dma_addr_t addr, |
1358 | u32 width, u32 height) |
1359 | { |
1360 | dev_dbg(&dw_dev->pdev->dev, |
1361 | "Set HW mapping registers for %ux%u addr:%pad" , |
1362 | width, height, &addr); |
1363 | |
1364 | dw100_write(dw_dev, DW100_MAP_LUT_ADDR, DW100_MAP_LUT_ADDR_ADDR(addr)); |
1365 | dw100_write(dw_dev, DW100_MAP_LUT_SIZE, DW100_MAP_LUT_SIZE_WIDTH(width) |
1366 | | DW100_MAP_LUT_SIZE_HEIGHT(height)); |
1367 | } |
1368 | |
1369 | static void dw100_hw_clear_irq(struct dw100_device *dw_dev, unsigned int irq) |
1370 | { |
1371 | dw100_write(dw_dev, DW100_INTERRUPT_STATUS, |
1372 | DW100_INTERRUPT_STATUS_INT_CLEAR(irq)); |
1373 | } |
1374 | |
1375 | static void dw100_hw_enable_irq(struct dw100_device *dw_dev) |
1376 | { |
1377 | dw100_write(dw_dev, DW100_INTERRUPT_STATUS, |
1378 | DW100_INTERRUPT_STATUS_INT_ENABLE_MASK); |
1379 | } |
1380 | |
1381 | static void dw100_hw_disable_irq(struct dw100_device *dw_dev) |
1382 | { |
1383 | dw100_write(dw_dev, DW100_INTERRUPT_STATUS, val: 0); |
1384 | } |
1385 | |
1386 | static u32 dw_hw_get_pending_irqs(struct dw100_device *dw_dev) |
1387 | { |
1388 | u32 val; |
1389 | |
1390 | val = dw100_read(dw_dev, DW100_INTERRUPT_STATUS); |
1391 | |
1392 | return DW100_INTERRUPT_STATUS_INT_STATUS(val); |
1393 | } |
1394 | |
1395 | static irqreturn_t dw100_irq_handler(int irq, void *dev_id) |
1396 | { |
1397 | struct dw100_device *dw_dev = dev_id; |
1398 | u32 pending_irqs, err_irqs, frame_done_irq; |
1399 | bool with_error = true; |
1400 | |
1401 | pending_irqs = dw_hw_get_pending_irqs(dw_dev); |
1402 | frame_done_irq = pending_irqs & DW100_INTERRUPT_STATUS_INT_FRAME_DONE; |
1403 | err_irqs = DW100_INTERRUPT_STATUS_INT_ERR_STATUS(pending_irqs); |
1404 | |
1405 | if (frame_done_irq) { |
1406 | dev_dbg(&dw_dev->pdev->dev, "Frame done interrupt\n" ); |
1407 | with_error = false; |
1408 | err_irqs &= ~DW100_INTERRUPT_STATUS_INT_ERR_STATUS |
1409 | (DW100_INTERRUPT_STATUS_INT_ERR_FRAME_DONE); |
1410 | } |
1411 | |
1412 | if (err_irqs) |
1413 | dev_err(&dw_dev->pdev->dev, "Interrupt error: %#x\n" , err_irqs); |
1414 | |
1415 | dw100_hw_disable_irq(dw_dev); |
1416 | dw100_hw_master_bus_disable(dw_dev); |
1417 | dw100_hw_clear_irq(dw_dev, irq: pending_irqs | |
1418 | DW100_INTERRUPT_STATUS_INT_ERR_TIME_OUT); |
1419 | |
1420 | dw100_job_finish(dw_dev, with_error); |
1421 | |
1422 | return IRQ_HANDLED; |
1423 | } |
1424 | |
1425 | static void dw100_start(struct dw100_ctx *ctx, struct vb2_v4l2_buffer *in_vb, |
1426 | struct vb2_v4l2_buffer *out_vb) |
1427 | { |
1428 | struct dw100_device *dw_dev = ctx->dw_dev; |
1429 | |
1430 | out_vb->sequence = |
1431 | dw100_get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)->sequence++; |
1432 | in_vb->sequence = |
1433 | dw100_get_q_data(ctx, type: V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)->sequence++; |
1434 | |
1435 | dev_dbg(&ctx->dw_dev->pdev->dev, |
1436 | "Starting queues %p->%p, sequence %u->%u\n" , |
1437 | v4l2_m2m_get_vq(ctx->fh.m2m_ctx, |
1438 | V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE), |
1439 | v4l2_m2m_get_vq(ctx->fh.m2m_ctx, |
1440 | V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE), |
1441 | in_vb->sequence, out_vb->sequence); |
1442 | |
1443 | v4l2_m2m_buf_copy_metadata(out_vb: in_vb, cap_vb: out_vb, copy_frame_flags: true); |
1444 | |
1445 | /* Now, let's deal with hardware ... */ |
1446 | dw100_hw_master_bus_disable(dw_dev); |
1447 | dw100_hw_init_ctrl(dw_dev); |
1448 | dw100_hw_set_pixel_boundary(dw_dev); |
1449 | dw100_hw_set_src_crop(dw_dev, src_q_data: &ctx->q_data[DW100_QUEUE_SRC], |
1450 | dst_q_data: &ctx->q_data[DW100_QUEUE_DST]); |
1451 | dw100_hw_set_source(dw_dev, q_data: &ctx->q_data[DW100_QUEUE_SRC], |
1452 | buffer: &in_vb->vb2_buf); |
1453 | dw100_hw_set_destination(dw_dev, q_data: &ctx->q_data[DW100_QUEUE_DST], |
1454 | ifmt: ctx->q_data[DW100_QUEUE_SRC].fmt, |
1455 | buffer: &out_vb->vb2_buf); |
1456 | dw100_hw_set_mapping(dw_dev, addr: ctx->map_dma, |
1457 | width: ctx->map_width, height: ctx->map_height); |
1458 | dw100_hw_enable_irq(dw_dev); |
1459 | dw100_hw_dewarp_start(dw_dev); |
1460 | |
1461 | /* Enable Bus */ |
1462 | dw100_hw_master_bus_enable(dw_dev); |
1463 | } |
1464 | |
1465 | static void dw100_device_run(void *priv) |
1466 | { |
1467 | struct dw100_ctx *ctx = priv; |
1468 | struct vb2_v4l2_buffer *src_buf, *dst_buf; |
1469 | |
1470 | src_buf = v4l2_m2m_next_src_buf(m2m_ctx: ctx->fh.m2m_ctx); |
1471 | dst_buf = v4l2_m2m_next_dst_buf(m2m_ctx: ctx->fh.m2m_ctx); |
1472 | |
1473 | dw100_start(ctx, in_vb: src_buf, out_vb: dst_buf); |
1474 | } |
1475 | |
1476 | static const struct v4l2_m2m_ops dw100_m2m_ops = { |
1477 | .device_run = dw100_device_run, |
1478 | }; |
1479 | |
1480 | static struct video_device *dw100_init_video_device(struct dw100_device *dw_dev) |
1481 | { |
1482 | struct video_device *vfd = &dw_dev->vfd; |
1483 | |
1484 | vfd->vfl_dir = VFL_DIR_M2M; |
1485 | vfd->fops = &dw100_fops; |
1486 | vfd->device_caps = V4L2_CAP_VIDEO_M2M_MPLANE | V4L2_CAP_STREAMING; |
1487 | vfd->ioctl_ops = &dw100_ioctl_ops; |
1488 | vfd->minor = -1; |
1489 | vfd->release = video_device_release_empty; |
1490 | vfd->v4l2_dev = &dw_dev->v4l2_dev; |
1491 | vfd->lock = &dw_dev->vfd_mutex; |
1492 | |
1493 | strscpy(vfd->name, DRV_NAME, sizeof(vfd->name)); |
1494 | mutex_init(vfd->lock); |
1495 | video_set_drvdata(vdev: vfd, data: dw_dev); |
1496 | |
1497 | return vfd; |
1498 | } |
1499 | |
1500 | static int dw100_dump_regs_show(struct seq_file *m, void *private) |
1501 | { |
1502 | struct dw100_device *dw_dev = m->private; |
1503 | int ret; |
1504 | |
1505 | ret = pm_runtime_resume_and_get(dev: &dw_dev->pdev->dev); |
1506 | if (ret < 0) |
1507 | return ret; |
1508 | |
1509 | ret = dw100_dump_regs(m); |
1510 | |
1511 | pm_runtime_put_sync(dev: &dw_dev->pdev->dev); |
1512 | |
1513 | return ret; |
1514 | } |
1515 | DEFINE_SHOW_ATTRIBUTE(dw100_dump_regs); |
1516 | |
1517 | static void dw100_debugfs_init(struct dw100_device *dw_dev) |
1518 | { |
1519 | dw_dev->debugfs_root = |
1520 | debugfs_create_dir(name: dev_name(dev: &dw_dev->pdev->dev), NULL); |
1521 | |
1522 | debugfs_create_file(name: "dump_regs" , mode: 0600, parent: dw_dev->debugfs_root, data: dw_dev, |
1523 | fops: &dw100_dump_regs_fops); |
1524 | } |
1525 | |
1526 | static void dw100_debugfs_exit(struct dw100_device *dw_dev) |
1527 | { |
1528 | debugfs_remove_recursive(dentry: dw_dev->debugfs_root); |
1529 | } |
1530 | |
1531 | static int dw100_probe(struct platform_device *pdev) |
1532 | { |
1533 | struct dw100_device *dw_dev; |
1534 | struct video_device *vfd; |
1535 | int ret, irq; |
1536 | |
1537 | dw_dev = devm_kzalloc(dev: &pdev->dev, size: sizeof(*dw_dev), GFP_KERNEL); |
1538 | if (!dw_dev) |
1539 | return -ENOMEM; |
1540 | dw_dev->pdev = pdev; |
1541 | |
1542 | ret = devm_clk_bulk_get_all(dev: &pdev->dev, clks: &dw_dev->clks); |
1543 | if (ret < 0) { |
1544 | dev_err(&pdev->dev, "Unable to get clocks: %d\n" , ret); |
1545 | return ret; |
1546 | } |
1547 | dw_dev->num_clks = ret; |
1548 | |
1549 | dw_dev->mmio = devm_platform_get_and_ioremap_resource(pdev, index: 0, NULL); |
1550 | if (IS_ERR(ptr: dw_dev->mmio)) |
1551 | return PTR_ERR(ptr: dw_dev->mmio); |
1552 | |
1553 | irq = platform_get_irq(pdev, 0); |
1554 | if (irq < 0) |
1555 | return irq; |
1556 | |
1557 | platform_set_drvdata(pdev, data: dw_dev); |
1558 | |
1559 | pm_runtime_enable(dev: &pdev->dev); |
1560 | ret = pm_runtime_resume_and_get(dev: &pdev->dev); |
1561 | if (ret < 0) { |
1562 | dev_err(&pdev->dev, "Unable to resume the device: %d\n" , ret); |
1563 | goto err_pm; |
1564 | } |
1565 | |
1566 | pm_runtime_put_sync(dev: &pdev->dev); |
1567 | |
1568 | ret = devm_request_irq(dev: &pdev->dev, irq, handler: dw100_irq_handler, IRQF_ONESHOT, |
1569 | devname: dev_name(dev: &pdev->dev), dev_id: dw_dev); |
1570 | if (ret < 0) { |
1571 | dev_err(&pdev->dev, "Failed to request irq: %d\n" , ret); |
1572 | goto err_pm; |
1573 | } |
1574 | |
1575 | ret = v4l2_device_register(dev: &pdev->dev, v4l2_dev: &dw_dev->v4l2_dev); |
1576 | if (ret) |
1577 | goto err_pm; |
1578 | |
1579 | vfd = dw100_init_video_device(dw_dev); |
1580 | |
1581 | dw_dev->m2m_dev = v4l2_m2m_init(m2m_ops: &dw100_m2m_ops); |
1582 | if (IS_ERR(ptr: dw_dev->m2m_dev)) { |
1583 | dev_err(&pdev->dev, "Failed to init mem2mem device\n" ); |
1584 | ret = PTR_ERR(ptr: dw_dev->m2m_dev); |
1585 | goto err_v4l2; |
1586 | } |
1587 | |
1588 | dw_dev->mdev.dev = &pdev->dev; |
1589 | strscpy(dw_dev->mdev.model, "dw100" , sizeof(dw_dev->mdev.model)); |
1590 | media_device_init(mdev: &dw_dev->mdev); |
1591 | dw_dev->v4l2_dev.mdev = &dw_dev->mdev; |
1592 | |
1593 | ret = video_register_device(vdev: vfd, type: VFL_TYPE_VIDEO, nr: -1); |
1594 | if (ret) { |
1595 | dev_err(&pdev->dev, "Failed to register video device\n" ); |
1596 | goto err_m2m; |
1597 | } |
1598 | |
1599 | ret = v4l2_m2m_register_media_controller(m2m_dev: dw_dev->m2m_dev, vdev: vfd, |
1600 | MEDIA_ENT_F_PROC_VIDEO_SCALER); |
1601 | if (ret) { |
1602 | dev_err(&pdev->dev, "Failed to init mem2mem media controller\n" ); |
1603 | goto error_v4l2; |
1604 | } |
1605 | |
1606 | ret = media_device_register(&dw_dev->mdev); |
1607 | if (ret) { |
1608 | dev_err(&pdev->dev, "Failed to register mem2mem media device\n" ); |
1609 | goto error_m2m_mc; |
1610 | } |
1611 | |
1612 | dw100_debugfs_init(dw_dev); |
1613 | |
1614 | dev_info(&pdev->dev, |
1615 | "dw100 v4l2 m2m registered as /dev/video%u\n" , vfd->num); |
1616 | |
1617 | return 0; |
1618 | |
1619 | error_m2m_mc: |
1620 | v4l2_m2m_unregister_media_controller(m2m_dev: dw_dev->m2m_dev); |
1621 | error_v4l2: |
1622 | video_unregister_device(vdev: vfd); |
1623 | err_m2m: |
1624 | media_device_cleanup(mdev: &dw_dev->mdev); |
1625 | v4l2_m2m_release(m2m_dev: dw_dev->m2m_dev); |
1626 | err_v4l2: |
1627 | v4l2_device_unregister(v4l2_dev: &dw_dev->v4l2_dev); |
1628 | err_pm: |
1629 | pm_runtime_disable(dev: &pdev->dev); |
1630 | |
1631 | return ret; |
1632 | } |
1633 | |
1634 | static void dw100_remove(struct platform_device *pdev) |
1635 | { |
1636 | struct dw100_device *dw_dev = platform_get_drvdata(pdev); |
1637 | |
1638 | dw100_debugfs_exit(dw_dev); |
1639 | |
1640 | pm_runtime_disable(dev: &pdev->dev); |
1641 | |
1642 | media_device_unregister(mdev: &dw_dev->mdev); |
1643 | v4l2_m2m_unregister_media_controller(m2m_dev: dw_dev->m2m_dev); |
1644 | media_device_cleanup(mdev: &dw_dev->mdev); |
1645 | |
1646 | video_unregister_device(vdev: &dw_dev->vfd); |
1647 | mutex_destroy(lock: dw_dev->vfd.lock); |
1648 | v4l2_m2m_release(m2m_dev: dw_dev->m2m_dev); |
1649 | v4l2_device_unregister(v4l2_dev: &dw_dev->v4l2_dev); |
1650 | } |
1651 | |
1652 | static int __maybe_unused dw100_runtime_suspend(struct device *dev) |
1653 | { |
1654 | struct dw100_device *dw_dev = dev_get_drvdata(dev); |
1655 | |
1656 | clk_bulk_disable_unprepare(num_clks: dw_dev->num_clks, clks: dw_dev->clks); |
1657 | |
1658 | return 0; |
1659 | } |
1660 | |
1661 | static int __maybe_unused dw100_runtime_resume(struct device *dev) |
1662 | { |
1663 | int ret; |
1664 | struct dw100_device *dw_dev = dev_get_drvdata(dev); |
1665 | |
1666 | ret = clk_bulk_prepare_enable(num_clks: dw_dev->num_clks, clks: dw_dev->clks); |
1667 | |
1668 | if (ret) |
1669 | return ret; |
1670 | |
1671 | dw100_hw_reset(dw_dev); |
1672 | |
1673 | return 0; |
1674 | } |
1675 | |
1676 | static const struct dev_pm_ops dw100_pm = { |
1677 | SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend, |
1678 | pm_runtime_force_resume) |
1679 | SET_RUNTIME_PM_OPS(dw100_runtime_suspend, |
1680 | dw100_runtime_resume, NULL) |
1681 | }; |
1682 | |
1683 | static const struct of_device_id dw100_dt_ids[] = { |
1684 | { .compatible = "nxp,imx8mp-dw100" , .data = NULL }, |
1685 | { }, |
1686 | }; |
1687 | MODULE_DEVICE_TABLE(of, dw100_dt_ids); |
1688 | |
1689 | static struct platform_driver dw100_driver = { |
1690 | .probe = dw100_probe, |
1691 | .remove_new = dw100_remove, |
1692 | .driver = { |
1693 | .name = DRV_NAME, |
1694 | .pm = &dw100_pm, |
1695 | .of_match_table = dw100_dt_ids, |
1696 | }, |
1697 | }; |
1698 | |
1699 | module_platform_driver(dw100_driver); |
1700 | |
1701 | MODULE_DESCRIPTION("DW100 Hardware dewarper" ); |
1702 | MODULE_AUTHOR("Xavier Roumegue <Xavier.Roumegue@oss.nxp.com>" ); |
1703 | MODULE_LICENSE("GPL" ); |
1704 | |