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 
to_dw100_fmt_type(enum v4l2_buf_type type)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 
dw100_find_pixel_format(u32 pixel_format,int fmt_type)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 
dw100_find_format(struct v4l2_format * f)197 static const struct dw100_fmt *dw100_find_format(struct v4l2_format *f)
198 {
199 	return dw100_find_pixel_format(f->fmt.pix_mp.pixelformat,
200 				       to_dw100_fmt_type(f->type));
201 }
202 
dw100_read(struct dw100_device * dw_dev,u32 reg)203 static inline u32 dw100_read(struct dw100_device *dw_dev, u32 reg)
204 {
205 	return readl(dw_dev->mmio + reg);
206 }
207 
dw100_write(struct dw100_device * dw_dev,u32 reg,u32 val)208 static inline void dw100_write(struct dw100_device *dw_dev, u32 reg, u32 val)
209 {
210 	writel(val, dw_dev->mmio + reg);
211 }
212 
dw100_dump_regs(struct seq_file * m)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, "%s: %#x\n", dw100_regs[i].name,
262 			   dw100_read(dw_dev, dw100_regs[i].addr));
263 
264 	return 0;
265 }
266 
dw100_file2ctx(struct file * file)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 
dw100_get_q_data(struct dw100_ctx * ctx,enum v4l2_buf_type type)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 
dw100_get_n_vertices_from_length(u32 length)281 static u32 dw100_get_n_vertices_from_length(u32 length)
282 {
283 	return DIV_ROUND_UP(length, DW100_BLOCK_SIZE) + 1;
284 }
285 
dw100_map_convert_to_uq12_4(u32 a)286 static u16 dw100_map_convert_to_uq12_4(u32 a)
287 {
288 	return (u16)((a & 0xfff) << 4);
289 }
290 
dw100_map_format_coordinates(u16 xq,u16 yq)291 static u32 dw100_map_format_coordinates(u16 xq, u16 yq)
292 {
293 	return (u32)((yq << 16) | xq);
294 }
295 
dw100_get_user_map(struct dw100_ctx * ctx)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  */
dw100_create_mapping(struct dw100_ctx * ctx)307 static int dw100_create_mapping(struct dw100_ctx *ctx)
308 {
309 	u32 *user_map;
310 
311 	if (ctx->map)
312 		dma_free_coherent(&ctx->dw_dev->pdev->dev, ctx->map_size,
313 				  ctx->map, ctx->map_dma);
314 
315 	ctx->map = dma_alloc_coherent(&ctx->dw_dev->pdev->dev, ctx->map_size,
316 				      &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 
dw100_destroy_mapping(struct dw100_ctx * ctx)337 static void dw100_destroy_mapping(struct dw100_ctx *ctx)
338 {
339 	if (ctx->map) {
340 		dma_free_coherent(&ctx->dw_dev->pdev->dev, ctx->map_size,
341 				  ctx->map, ctx->map_dma);
342 		ctx->map = NULL;
343 	}
344 }
345 
dw100_s_ctrl(struct v4l2_ctrl * ctrl)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  */
dw100_ctrl_dewarping_map_init(const struct v4l2_ctrl * ctrl,u32 from_idx,union v4l2_ctrl_ptr ptr)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(sw);
393 	qsh = dw100_map_convert_to_uq12_4(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(qx, 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 
dw100_queue_setup(struct vb2_queue * vq,unsigned int * nbuffers,unsigned int * nplanes,unsigned int sizes[],struct device * alloc_devs[])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(vq);
437 	const struct v4l2_pix_format_mplane *format;
438 	unsigned int i;
439 
440 	format = &dw100_get_q_data(ctx, 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 
dw100_buf_prepare(struct vb2_buffer * vb)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(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, 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, 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, i, size);
490 	}
491 
492 	return 0;
493 }
494 
dw100_buf_queue(struct vb2_buffer * vb)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(vb->vb2_queue);
499 
500 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
501 }
502 
dw100_return_all_buffers(struct vb2_queue * q,enum vb2_buffer_state state)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(ctx->fh.m2m_ctx);
512 		else
513 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
514 		if (!vbuf)
515 			return;
516 		v4l2_m2m_buf_done(vbuf, state);
517 	}
518 }
519 
dw100_start_streaming(struct vb2_queue * q,unsigned int count)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, 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(&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, VB2_BUF_STATE_QUEUED);
541 	return ret;
542 }
543 
dw100_stop_streaming(struct vb2_queue * q)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, VB2_BUF_STATE_ERROR);
549 
550 	pm_runtime_put_sync(&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 
dw100_m2m_queue_init(void * priv,struct vb2_queue * src_vq,struct vb2_queue * dst_vq)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(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(dst_vq);
596 }
597 
dw100_open(struct file * file)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(sizeof(*ctx), GFP_KERNEL);
607 	if (!ctx)
608 		return -ENOMEM;
609 
610 	mutex_init(&ctx->vq_mutex);
611 	v4l2_fh_init(&ctx->fh, 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(pix_fmt, 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, &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(dw_dev->m2m_dev,
649 					    ctx, &dw100_m2m_queue_init);
650 
651 	if (IS_ERR(ctx->fh.m2m_ctx)) {
652 		ret = PTR_ERR(ctx->fh.m2m_ctx);
653 		goto err;
654 	}
655 
656 	v4l2_fh_add(&ctx->fh);
657 
658 	return 0;
659 
660 err:
661 	v4l2_ctrl_handler_free(hdl);
662 	v4l2_fh_exit(&ctx->fh);
663 	mutex_destroy(&ctx->vq_mutex);
664 	kfree(ctx);
665 
666 	return ret;
667 }
668 
dw100_release(struct file * file)669 static int dw100_release(struct file *file)
670 {
671 	struct dw100_ctx *ctx = dw100_file2ctx(file);
672 
673 	v4l2_fh_del(&ctx->fh);
674 	v4l2_fh_exit(&ctx->fh);
675 	v4l2_ctrl_handler_free(&ctx->hdl);
676 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
677 	mutex_destroy(&ctx->vq_mutex);
678 	kfree(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 
dw100_querycap(struct file * file,void * priv,struct v4l2_capability * cap)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 
dw100_enum_fmt_vid(struct file * file,void * priv,struct v4l2_fmtdesc * f)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(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 
dw100_enum_framesizes(struct file * file,void * priv,struct v4l2_frmsizeenum * fsize)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(fsize->pixel_format,
728 				      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 
dw100_g_fmt_vid(struct file * file,void * priv,struct v4l2_format * f)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(ctx->fh.m2m_ctx, f->type);
745 	if (!vq)
746 		return -EINVAL;
747 
748 	q_data = dw100_get_q_data(ctx, f->type);
749 
750 	f->fmt.pix_mp = q_data->pix_fmt;
751 
752 	return 0;
753 }
754 
dw100_try_fmt(struct file * file,struct v4l2_format * f)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(&pix->width, &pix->height,
768 				       &dw100_frmsize_stepwise);
769 
770 	v4l2_fill_pixfmt_mp(pix, fmt->fourcc, pix->width, 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, 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 
dw100_s_fmt(struct dw100_ctx * ctx,struct v4l2_format * f)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(ctx->fh.m2m_ctx, f->type);
809 	if (!vq)
810 		return -EINVAL;
811 
812 	q_data = dw100_get_q_data(ctx, f->type);
813 	if (!q_data)
814 		return -EINVAL;
815 
816 	if (vb2_is_busy(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 					 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(q_data->pix_fmt.width);
852 		dims[1] = dw100_get_n_vertices_from_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 
dw100_try_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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 
dw100_s_fmt_vid_cap(struct file * file,void * priv,struct v4l2_format * f)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 
dw100_try_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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 
dw100_s_fmt_vid_out(struct file * file,void * priv,struct v4l2_format * f)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 
dw100_g_selection(struct file * file,void * fh,struct v4l2_selection * sel)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, 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 
dw100_s_selection(struct file * file,void * fh,struct v4l2_selection * sel)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, 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 
dw100_job_finish(struct dw100_device * dw_dev,bool with_error)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(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(curr_ctx->fh.m2m_ctx);
1083 	dst_vb = v4l2_m2m_dst_buf_remove(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(src_vb, buf_state);
1091 	v4l2_m2m_buf_done(dst_vb, 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(dw_dev->m2m_dev, curr_ctx->fh.m2m_ctx);
1097 }
1098 
dw100_hw_reset(struct dw100_device * dw_dev)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 
_dw100_hw_set_master_bus_enable(struct dw100_device * dw_dev,unsigned int enable)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 
dw100_hw_master_bus_enable(struct dw100_device * dw_dev)1129 static void dw100_hw_master_bus_enable(struct dw100_device *dw_dev)
1130 {
1131 	_dw100_hw_set_master_bus_enable(dw_dev, 1);
1132 }
1133 
dw100_hw_master_bus_disable(struct dw100_device * dw_dev)1134 static void dw100_hw_master_bus_disable(struct dw100_device *dw_dev)
1135 {
1136 	_dw100_hw_set_master_bus_enable(dw_dev, 0);
1137 }
1138 
dw100_hw_dewarp_start(struct dw100_device * dw_dev)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 | DW100_DEWARP_CTRL_START);
1147 	dw100_write(dw_dev, DW100_DEWARP_CTRL, val);
1148 }
1149 
dw100_hw_init_ctrl(struct dw100_device * dw_dev)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 
dw100_hw_set_pixel_boundary(struct dw100_device * dw_dev)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 
dw100_hw_set_scale(struct dw100_device * dw_dev,u8 scale)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, scale);
1198 }
1199 
dw100_hw_set_roi(struct dw100_device * dw_dev,u32 x,u32 y)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 
dw100_hw_set_src_crop(struct dw100_device * dw_dev,const struct dw100_q_data * src_q_data,const struct dw100_q_data * dst_q_data)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, 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, left_scale, top_scale);
1228 }
1229 
dw100_hw_set_source(struct dw100_device * dw_dev,const struct dw100_q_data * q_data,struct vb2_buffer * buffer)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(buffer, 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(buffer, 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, 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 
dw100_hw_set_destination(struct dw100_device * dw_dev,const struct dw100_q_data * q_data,const struct dw100_fmt * ifmt,struct vb2_buffer * buffer)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(buffer, 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(buffer, 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, 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 
dw100_hw_set_mapping(struct dw100_device * dw_dev,dma_addr_t addr,u32 width,u32 height)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 
dw100_hw_clear_irq(struct dw100_device * dw_dev,unsigned int irq)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 
dw100_hw_enable_irq(struct dw100_device * dw_dev)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 
dw100_hw_disable_irq(struct dw100_device * dw_dev)1381 static void dw100_hw_disable_irq(struct dw100_device *dw_dev)
1382 {
1383 	dw100_write(dw_dev, DW100_INTERRUPT_STATUS, 0);
1384 }
1385 
dw_hw_get_pending_irqs(struct dw100_device * dw_dev)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 
dw100_irq_handler(int irq,void * dev_id)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, 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 
dw100_start(struct dw100_ctx * ctx,struct vb2_v4l2_buffer * in_vb,struct vb2_v4l2_buffer * out_vb)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, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)->sequence++;
1432 	in_vb->sequence =
1433 		dw100_get_q_data(ctx, 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(in_vb, out_vb, 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, &ctx->q_data[DW100_QUEUE_SRC],
1450 			      &ctx->q_data[DW100_QUEUE_DST]);
1451 	dw100_hw_set_source(dw_dev, &ctx->q_data[DW100_QUEUE_SRC],
1452 			    &in_vb->vb2_buf);
1453 	dw100_hw_set_destination(dw_dev, &ctx->q_data[DW100_QUEUE_DST],
1454 				 ctx->q_data[DW100_QUEUE_SRC].fmt,
1455 				 &out_vb->vb2_buf);
1456 	dw100_hw_set_mapping(dw_dev, ctx->map_dma,
1457 			     ctx->map_width, 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 
dw100_device_run(void * priv)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(ctx->fh.m2m_ctx);
1471 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1472 
1473 	dw100_start(ctx, src_buf, dst_buf);
1474 }
1475 
1476 static const struct v4l2_m2m_ops dw100_m2m_ops = {
1477 	.device_run	= dw100_device_run,
1478 };
1479 
dw100_init_video_device(struct dw100_device * dw_dev)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(vfd, dw_dev);
1496 
1497 	return vfd;
1498 }
1499 
dw100_dump_regs_show(struct seq_file * m,void * private)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(&dw_dev->pdev->dev);
1506 	if (ret < 0)
1507 		return ret;
1508 
1509 	ret = dw100_dump_regs(m);
1510 
1511 	pm_runtime_put_sync(&dw_dev->pdev->dev);
1512 
1513 	return ret;
1514 }
1515 DEFINE_SHOW_ATTRIBUTE(dw100_dump_regs);
1516 
dw100_debugfs_init(struct dw100_device * dw_dev)1517 static void dw100_debugfs_init(struct dw100_device *dw_dev)
1518 {
1519 	dw_dev->debugfs_root =
1520 		debugfs_create_dir(dev_name(&dw_dev->pdev->dev), NULL);
1521 
1522 	debugfs_create_file("dump_regs", 0600, dw_dev->debugfs_root, dw_dev,
1523 			    &dw100_dump_regs_fops);
1524 }
1525 
dw100_debugfs_exit(struct dw100_device * dw_dev)1526 static void dw100_debugfs_exit(struct dw100_device *dw_dev)
1527 {
1528 	debugfs_remove_recursive(dw_dev->debugfs_root);
1529 }
1530 
dw100_probe(struct platform_device * pdev)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(&pdev->dev, 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(&pdev->dev, &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, 0, NULL);
1550 	if (IS_ERR(dw_dev->mmio))
1551 		return PTR_ERR(dw_dev->mmio);
1552 
1553 	irq = platform_get_irq(pdev, 0);
1554 	if (irq < 0)
1555 		return irq;
1556 
1557 	platform_set_drvdata(pdev, dw_dev);
1558 
1559 	pm_runtime_enable(&pdev->dev);
1560 	ret = pm_runtime_resume_and_get(&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(&pdev->dev);
1567 
1568 	ret = devm_request_irq(&pdev->dev, irq, dw100_irq_handler, IRQF_ONESHOT,
1569 			       dev_name(&pdev->dev), 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(&pdev->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(&dw100_m2m_ops);
1582 	if (IS_ERR(dw_dev->m2m_dev)) {
1583 		dev_err(&pdev->dev, "Failed to init mem2mem device\n");
1584 		ret = PTR_ERR(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(&dw_dev->mdev);
1591 	dw_dev->v4l2_dev.mdev = &dw_dev->mdev;
1592 
1593 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, -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(dw_dev->m2m_dev, 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(dw_dev->m2m_dev);
1621 error_v4l2:
1622 	video_unregister_device(vfd);
1623 err_m2m:
1624 	media_device_cleanup(&dw_dev->mdev);
1625 	v4l2_m2m_release(dw_dev->m2m_dev);
1626 err_v4l2:
1627 	v4l2_device_unregister(&dw_dev->v4l2_dev);
1628 err_pm:
1629 	pm_runtime_disable(&pdev->dev);
1630 
1631 	return ret;
1632 }
1633 
dw100_remove(struct platform_device * pdev)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(&pdev->dev);
1641 
1642 	media_device_unregister(&dw_dev->mdev);
1643 	v4l2_m2m_unregister_media_controller(dw_dev->m2m_dev);
1644 	media_device_cleanup(&dw_dev->mdev);
1645 
1646 	video_unregister_device(&dw_dev->vfd);
1647 	mutex_destroy(dw_dev->vfd.lock);
1648 	v4l2_m2m_release(dw_dev->m2m_dev);
1649 	v4l2_device_unregister(&dw_dev->v4l2_dev);
1650 }
1651 
dw100_runtime_suspend(struct device * dev)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(dw_dev->num_clks, dw_dev->clks);
1657 
1658 	return 0;
1659 }
1660 
dw100_runtime_resume(struct device * dev)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(dw_dev->num_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