1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * i.MX Pixel Pipeline (PXP) mem-to-mem scaler/CSC/rotator driver
4  *
5  * Copyright (c) 2018 Pengutronix, Philipp Zabel
6  *
7  * based on vim2m
8  *
9  * Copyright (c) 2009-2010 Samsung Electronics Co., Ltd.
10  * Pawel Osciak, <pawel@osciak.com>
11  * Marek Szyprowski, <m.szyprowski@samsung.com>
12  */
13 #include <linux/bitfield.h>
14 #include <linux/clk.h>
15 #include <linux/delay.h>
16 #include <linux/dma-mapping.h>
17 #include <linux/interrupt.h>
18 #include <linux/io.h>
19 #include <linux/iopoll.h>
20 #include <linux/module.h>
21 #include <linux/of.h>
22 #include <linux/platform_device.h>
23 #include <linux/regmap.h>
24 #include <linux/sched.h>
25 #include <linux/slab.h>
26 
27 #include <media/media-device.h>
28 #include <media/v4l2-ctrls.h>
29 #include <media/v4l2-device.h>
30 #include <media/v4l2-event.h>
31 #include <media/v4l2-ioctl.h>
32 #include <media/v4l2-mem2mem.h>
33 #include <media/videobuf2-dma-contig.h>
34 
35 #include "imx-pxp.h"
36 
37 static unsigned int debug;
38 module_param(debug, uint, 0644);
39 MODULE_PARM_DESC(debug, "activates debug info");
40 
41 #define MIN_W 8
42 #define MIN_H 8
43 #define MAX_W 4096
44 #define MAX_H 4096
45 #define ALIGN_W 3 /* 8x8 pixel blocks */
46 #define ALIGN_H 3
47 
48 /* Flags that indicate a format can be used for capture/output */
49 #define MEM2MEM_CAPTURE	(1 << 0)
50 #define MEM2MEM_OUTPUT	(1 << 1)
51 
52 #define MEM2MEM_NAME		"pxp"
53 
54 /* Flags that indicate processing mode */
55 #define MEM2MEM_HFLIP	(1 << 0)
56 #define MEM2MEM_VFLIP	(1 << 1)
57 
58 #define PXP_VERSION_MAJOR(version) \
59 	FIELD_GET(BM_PXP_VERSION_MAJOR, version)
60 #define PXP_VERSION_MINOR(version) \
61 	FIELD_GET(BM_PXP_VERSION_MINOR, version)
62 
63 #define dprintk(dev, fmt, arg...) \
64 	v4l2_dbg(1, debug, &dev->v4l2_dev, "%s: " fmt, __func__, ## arg)
65 
66 struct pxp_fmt {
67 	u32	fourcc;
68 	int	depth;
69 	/* Types the format can be used for */
70 	u32	types;
71 };
72 
73 static struct pxp_fmt formats[] = {
74 	{
75 		.fourcc	= V4L2_PIX_FMT_XBGR32,
76 		.depth	= 32,
77 		/* Both capture and output format */
78 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
79 	}, {
80 		.fourcc	= V4L2_PIX_FMT_ABGR32,
81 		.depth	= 32,
82 		/* Capture-only format */
83 		.types	= MEM2MEM_CAPTURE,
84 	}, {
85 		.fourcc	= V4L2_PIX_FMT_BGR24,
86 		.depth	= 24,
87 		.types	= MEM2MEM_CAPTURE,
88 	}, {
89 		.fourcc	= V4L2_PIX_FMT_RGB565,
90 		.depth	= 16,
91 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
92 	}, {
93 		.fourcc	= V4L2_PIX_FMT_RGB555,
94 		.depth	= 16,
95 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
96 	}, {
97 		.fourcc = V4L2_PIX_FMT_RGB444,
98 		.depth	= 16,
99 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
100 	}, {
101 		.fourcc = V4L2_PIX_FMT_VUYA32,
102 		.depth	= 32,
103 		.types	= MEM2MEM_CAPTURE,
104 	}, {
105 		.fourcc = V4L2_PIX_FMT_VUYX32,
106 		.depth	= 32,
107 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
108 	}, {
109 		.fourcc = V4L2_PIX_FMT_UYVY,
110 		.depth	= 16,
111 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
112 	}, {
113 		.fourcc = V4L2_PIX_FMT_YUYV,
114 		.depth	= 16,
115 		/* Output-only format */
116 		.types	= MEM2MEM_OUTPUT,
117 	}, {
118 		.fourcc = V4L2_PIX_FMT_VYUY,
119 		.depth	= 16,
120 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
121 	}, {
122 		.fourcc = V4L2_PIX_FMT_YVYU,
123 		.depth	= 16,
124 		.types	= MEM2MEM_OUTPUT,
125 	}, {
126 		.fourcc = V4L2_PIX_FMT_GREY,
127 		.depth	= 8,
128 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
129 	}, {
130 		.fourcc = V4L2_PIX_FMT_Y4,
131 		.depth	= 4,
132 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
133 	}, {
134 		.fourcc = V4L2_PIX_FMT_NV16,
135 		.depth	= 16,
136 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
137 	}, {
138 		.fourcc = V4L2_PIX_FMT_NV12,
139 		.depth	= 12,
140 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
141 	}, {
142 		.fourcc = V4L2_PIX_FMT_NV21,
143 		.depth	= 12,
144 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
145 	}, {
146 		.fourcc = V4L2_PIX_FMT_NV61,
147 		.depth	= 16,
148 		.types	= MEM2MEM_CAPTURE | MEM2MEM_OUTPUT,
149 	}, {
150 		.fourcc = V4L2_PIX_FMT_YUV422P,
151 		.depth	= 16,
152 		.types	= MEM2MEM_OUTPUT,
153 	}, {
154 		.fourcc = V4L2_PIX_FMT_YUV420,
155 		.depth	= 12,
156 		.types	= MEM2MEM_OUTPUT,
157 	},
158 };
159 
160 #define NUM_FORMATS ARRAY_SIZE(formats)
161 
162 /* Per-queue, driver-specific private data */
163 struct pxp_q_data {
164 	unsigned int		width;
165 	unsigned int		height;
166 	unsigned int		bytesperline;
167 	unsigned int		sizeimage;
168 	unsigned int		sequence;
169 	struct pxp_fmt		*fmt;
170 	enum v4l2_ycbcr_encoding ycbcr_enc;
171 	enum v4l2_quantization	quant;
172 };
173 
174 enum {
175 	V4L2_M2M_SRC = 0,
176 	V4L2_M2M_DST = 1,
177 };
178 
179 static const struct regmap_config pxp_regmap_config = {
180 	.reg_bits = 32,
181 	.reg_stride = 4,
182 	.val_bits = 32,
183 	.max_register = HW_PXP_VERSION,
184 };
185 
186 static struct pxp_fmt *find_format(unsigned int pixelformat)
187 {
188 	struct pxp_fmt *fmt;
189 	unsigned int k;
190 
191 	for (k = 0; k < NUM_FORMATS; k++) {
192 		fmt = &formats[k];
193 		if (fmt->fourcc == pixelformat)
194 			break;
195 	}
196 
197 	if (k == NUM_FORMATS)
198 		return NULL;
199 
200 	return &formats[k];
201 }
202 
203 struct pxp_ctx;
204 
205 struct pxp_pdata {
206 	u32 (*data_path_ctrl0)(struct pxp_ctx *ctx);
207 };
208 
209 struct pxp_dev {
210 	struct v4l2_device	v4l2_dev;
211 	struct video_device	vfd;
212 #ifdef CONFIG_MEDIA_CONTROLLER
213 	struct media_device	mdev;
214 #endif
215 
216 	struct clk		*clk;
217 	struct regmap		*regmap;
218 
219 	const struct pxp_pdata	*pdata;
220 
221 	atomic_t		num_inst;
222 	struct mutex		dev_mutex;
223 	spinlock_t		irqlock;
224 
225 	struct v4l2_m2m_dev	*m2m_dev;
226 };
227 
228 struct pxp_ctx {
229 	struct v4l2_fh		fh;
230 	struct pxp_dev	*dev;
231 
232 	struct v4l2_ctrl_handler hdl;
233 
234 	/* Abort requested by m2m */
235 	int			aborting;
236 
237 	/* Processing mode */
238 	int			mode;
239 	u8			alpha_component;
240 	u8			rotation;
241 
242 	enum v4l2_colorspace	colorspace;
243 	enum v4l2_xfer_func	xfer_func;
244 
245 	/* Source and destination queue data */
246 	struct pxp_q_data   q_data[2];
247 };
248 
249 static inline struct pxp_ctx *file2ctx(struct file *file)
250 {
251 	return container_of(file->private_data, struct pxp_ctx, fh);
252 }
253 
254 static struct pxp_q_data *get_q_data(struct pxp_ctx *ctx,
255 					 enum v4l2_buf_type type)
256 {
257 	if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
258 		return &ctx->q_data[V4L2_M2M_SRC];
259 	else
260 		return &ctx->q_data[V4L2_M2M_DST];
261 }
262 
263 static inline u32 pxp_read(struct pxp_dev *dev, u32 reg)
264 {
265 	u32 value;
266 
267 	regmap_read(dev->regmap, reg, &value);
268 
269 	return value;
270 }
271 
272 static inline void pxp_write(struct pxp_dev *dev, u32 reg, u32 value)
273 {
274 	regmap_write(dev->regmap, reg, value);
275 }
276 
277 static u32 pxp_v4l2_pix_fmt_to_ps_format(u32 v4l2_pix_fmt)
278 {
279 	switch (v4l2_pix_fmt) {
280 	case V4L2_PIX_FMT_XBGR32:  return BV_PXP_PS_CTRL_FORMAT__RGB888;
281 	case V4L2_PIX_FMT_RGB555:  return BV_PXP_PS_CTRL_FORMAT__RGB555;
282 	case V4L2_PIX_FMT_RGB444:  return BV_PXP_PS_CTRL_FORMAT__RGB444;
283 	case V4L2_PIX_FMT_RGB565:  return BV_PXP_PS_CTRL_FORMAT__RGB565;
284 	case V4L2_PIX_FMT_VUYX32:  return BV_PXP_PS_CTRL_FORMAT__YUV1P444;
285 	case V4L2_PIX_FMT_UYVY:    return BV_PXP_PS_CTRL_FORMAT__UYVY1P422;
286 	case V4L2_PIX_FMT_YUYV:    return BM_PXP_PS_CTRL_WB_SWAP |
287 					  BV_PXP_PS_CTRL_FORMAT__UYVY1P422;
288 	case V4L2_PIX_FMT_VYUY:    return BV_PXP_PS_CTRL_FORMAT__VYUY1P422;
289 	case V4L2_PIX_FMT_YVYU:    return BM_PXP_PS_CTRL_WB_SWAP |
290 					  BV_PXP_PS_CTRL_FORMAT__VYUY1P422;
291 	case V4L2_PIX_FMT_GREY:    return BV_PXP_PS_CTRL_FORMAT__Y8;
292 	default:
293 	case V4L2_PIX_FMT_Y4:      return BV_PXP_PS_CTRL_FORMAT__Y4;
294 	case V4L2_PIX_FMT_NV16:    return BV_PXP_PS_CTRL_FORMAT__YUV2P422;
295 	case V4L2_PIX_FMT_NV12:    return BV_PXP_PS_CTRL_FORMAT__YUV2P420;
296 	case V4L2_PIX_FMT_NV21:    return BV_PXP_PS_CTRL_FORMAT__YVU2P420;
297 	case V4L2_PIX_FMT_NV61:    return BV_PXP_PS_CTRL_FORMAT__YVU2P422;
298 	case V4L2_PIX_FMT_YUV422P: return BV_PXP_PS_CTRL_FORMAT__YUV422;
299 	case V4L2_PIX_FMT_YUV420:  return BV_PXP_PS_CTRL_FORMAT__YUV420;
300 	}
301 }
302 
303 static u32 pxp_v4l2_pix_fmt_to_out_format(u32 v4l2_pix_fmt)
304 {
305 	switch (v4l2_pix_fmt) {
306 	case V4L2_PIX_FMT_XBGR32:   return BV_PXP_OUT_CTRL_FORMAT__RGB888;
307 	case V4L2_PIX_FMT_ABGR32:   return BV_PXP_OUT_CTRL_FORMAT__ARGB8888;
308 	case V4L2_PIX_FMT_BGR24:    return BV_PXP_OUT_CTRL_FORMAT__RGB888P;
309 	/* Missing V4L2 pixel formats for ARGB1555 and ARGB4444 */
310 	case V4L2_PIX_FMT_RGB555:   return BV_PXP_OUT_CTRL_FORMAT__RGB555;
311 	case V4L2_PIX_FMT_RGB444:   return BV_PXP_OUT_CTRL_FORMAT__RGB444;
312 	case V4L2_PIX_FMT_RGB565:   return BV_PXP_OUT_CTRL_FORMAT__RGB565;
313 	case V4L2_PIX_FMT_VUYA32:
314 	case V4L2_PIX_FMT_VUYX32:   return BV_PXP_OUT_CTRL_FORMAT__YUV1P444;
315 	case V4L2_PIX_FMT_UYVY:     return BV_PXP_OUT_CTRL_FORMAT__UYVY1P422;
316 	case V4L2_PIX_FMT_VYUY:     return BV_PXP_OUT_CTRL_FORMAT__VYUY1P422;
317 	case V4L2_PIX_FMT_GREY:     return BV_PXP_OUT_CTRL_FORMAT__Y8;
318 	default:
319 	case V4L2_PIX_FMT_Y4:       return BV_PXP_OUT_CTRL_FORMAT__Y4;
320 	case V4L2_PIX_FMT_NV16:     return BV_PXP_OUT_CTRL_FORMAT__YUV2P422;
321 	case V4L2_PIX_FMT_NV12:     return BV_PXP_OUT_CTRL_FORMAT__YUV2P420;
322 	case V4L2_PIX_FMT_NV61:     return BV_PXP_OUT_CTRL_FORMAT__YVU2P422;
323 	case V4L2_PIX_FMT_NV21:     return BV_PXP_OUT_CTRL_FORMAT__YVU2P420;
324 	}
325 }
326 
327 static bool pxp_v4l2_pix_fmt_is_yuv(u32 v4l2_pix_fmt)
328 {
329 	switch (v4l2_pix_fmt) {
330 	case V4L2_PIX_FMT_VUYA32:
331 	case V4L2_PIX_FMT_VUYX32:
332 	case V4L2_PIX_FMT_UYVY:
333 	case V4L2_PIX_FMT_YUYV:
334 	case V4L2_PIX_FMT_VYUY:
335 	case V4L2_PIX_FMT_YVYU:
336 	case V4L2_PIX_FMT_NV16:
337 	case V4L2_PIX_FMT_NV12:
338 	case V4L2_PIX_FMT_NV61:
339 	case V4L2_PIX_FMT_NV21:
340 	case V4L2_PIX_FMT_YUV420:
341 	case V4L2_PIX_FMT_YUV422P:
342 	case V4L2_PIX_FMT_GREY:
343 	case V4L2_PIX_FMT_Y4:
344 		return true;
345 	default:
346 		return false;
347 	}
348 }
349 
350 static void pxp_setup_csc(struct pxp_ctx *ctx)
351 {
352 	struct pxp_dev *dev = ctx->dev;
353 	enum v4l2_ycbcr_encoding ycbcr_enc;
354 	enum v4l2_quantization quantization;
355 
356 	if (pxp_v4l2_pix_fmt_is_yuv(ctx->q_data[V4L2_M2M_SRC].fmt->fourcc) &&
357 	    !pxp_v4l2_pix_fmt_is_yuv(ctx->q_data[V4L2_M2M_DST].fmt->fourcc)) {
358 		/*
359 		 * CSC1 YUV/YCbCr to RGB conversion is implemented as follows:
360 		 *
361 		 * |R|   |C0 0  C1|   |Y  + Yoffset |
362 		 * |G| = |C0 C3 C2| * |Cb + UVoffset|
363 		 * |B|   |C0 C4 0 |   |Cr + UVoffset|
364 		 *
365 		 * Results are clamped to 0..255.
366 		 *
367 		 * BT.601 limited range:
368 		 *
369 		 * |R|   |1.1644  0.0000  1.5960|   |Y  - 16 |
370 		 * |G| = |1.1644 -0.3917 -0.8129| * |Cb - 128|
371 		 * |B|   |1.1644  2.0172  0.0000|   |Cr - 128|
372 		 */
373 		static const u32 csc1_coef_bt601_lim[3] = {
374 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
375 			BF_PXP_CSC1_COEF0_C0(0x12a) |	/*  1.1641 (-0.03 %) */
376 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
377 			BF_PXP_CSC1_COEF0_Y_OFFSET(-16),
378 			BF_PXP_CSC1_COEF1_C1(0x198) |	/*  1.5938 (-0.23 %) */
379 			BF_PXP_CSC1_COEF1_C4(0x204),	/*  2.0156 (-0.16 %) */
380 			BF_PXP_CSC1_COEF2_C2(0x730) |	/* -0.8125 (+0.04 %) */
381 			BF_PXP_CSC1_COEF2_C3(0x79c),	/* -0.3906 (+0.11 %) */
382 		};
383 		/*
384 		 * BT.601 full range:
385 		 *
386 		 * |R|   |1.0000  0.0000  1.4020|   |Y  + 0  |
387 		 * |G| = |1.0000 -0.3441 -0.7141| * |Cb - 128|
388 		 * |B|   |1.0000  1.7720  0.0000|   |Cr - 128|
389 		 */
390 		static const u32 csc1_coef_bt601_full[3] = {
391 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
392 			BF_PXP_CSC1_COEF0_C0(0x100) |	/*  1.0000 (+0.00 %) */
393 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
394 			BF_PXP_CSC1_COEF0_Y_OFFSET(0),
395 			BF_PXP_CSC1_COEF1_C1(0x166) |	/*  1.3984 (-0.36 %) */
396 			BF_PXP_CSC1_COEF1_C4(0x1c5),	/*  1.7695 (-0.25 %) */
397 			BF_PXP_CSC1_COEF2_C2(0x74a) |	/* -0.7109 (+0.32 %) */
398 			BF_PXP_CSC1_COEF2_C3(0x7a8),	/* -0.3438 (+0.04 %) */
399 		};
400 		/*
401 		 * Rec.709 limited range:
402 		 *
403 		 * |R|   |1.1644  0.0000  1.7927|   |Y  - 16 |
404 		 * |G| = |1.1644 -0.2132 -0.5329| * |Cb - 128|
405 		 * |B|   |1.1644  2.1124  0.0000|   |Cr - 128|
406 		 */
407 		static const u32 csc1_coef_rec709_lim[3] = {
408 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
409 			BF_PXP_CSC1_COEF0_C0(0x12a) |	/*  1.1641 (-0.03 %) */
410 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
411 			BF_PXP_CSC1_COEF0_Y_OFFSET(-16),
412 			BF_PXP_CSC1_COEF1_C1(0x1ca) |	/*  1.7891 (-0.37 %) */
413 			BF_PXP_CSC1_COEF1_C4(0x21c),	/*  2.1094 (-0.30 %) */
414 			BF_PXP_CSC1_COEF2_C2(0x778) |	/* -0.5312 (+0.16 %) */
415 			BF_PXP_CSC1_COEF2_C3(0x7ca),	/* -0.2109 (+0.23 %) */
416 		};
417 		/*
418 		 * Rec.709 full range:
419 		 *
420 		 * |R|   |1.0000  0.0000  1.5748|   |Y  + 0  |
421 		 * |G| = |1.0000 -0.1873 -0.4681| * |Cb - 128|
422 		 * |B|   |1.0000  1.8556  0.0000|   |Cr - 128|
423 		 */
424 		static const u32 csc1_coef_rec709_full[3] = {
425 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
426 			BF_PXP_CSC1_COEF0_C0(0x100) |	/*  1.0000 (+0.00 %) */
427 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
428 			BF_PXP_CSC1_COEF0_Y_OFFSET(0),
429 			BF_PXP_CSC1_COEF1_C1(0x193) |	/*  1.5742 (-0.06 %) */
430 			BF_PXP_CSC1_COEF1_C4(0x1db),	/*  1.8555 (-0.01 %) */
431 			BF_PXP_CSC1_COEF2_C2(0x789) |	/* -0.4648 (+0.33 %) */
432 			BF_PXP_CSC1_COEF2_C3(0x7d1),	/* -0.1836 (+0.37 %) */
433 		};
434 		/*
435 		 * BT.2020 limited range:
436 		 *
437 		 * |R|   |1.1644  0.0000  1.6787|   |Y  - 16 |
438 		 * |G| = |1.1644 -0.1874 -0.6505| * |Cb - 128|
439 		 * |B|   |1.1644  2.1418  0.0000|   |Cr - 128|
440 		 */
441 		static const u32 csc1_coef_bt2020_lim[3] = {
442 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
443 			BF_PXP_CSC1_COEF0_C0(0x12a) |	/*  1.1641 (-0.03 %) */
444 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
445 			BF_PXP_CSC1_COEF0_Y_OFFSET(-16),
446 			BF_PXP_CSC1_COEF1_C1(0x1ad) |	/*  1.6758 (-0.29 %) */
447 			BF_PXP_CSC1_COEF1_C4(0x224),	/*  2.1406 (-0.11 %) */
448 			BF_PXP_CSC1_COEF2_C2(0x75a) |	/* -0.6484 (+0.20 %) */
449 			BF_PXP_CSC1_COEF2_C3(0x7d1),	/* -0.1836 (+0.38 %) */
450 		};
451 		/*
452 		 * BT.2020 full range:
453 		 *
454 		 * |R|   |1.0000  0.0000  1.4746|   |Y  + 0  |
455 		 * |G| = |1.0000 -0.1646 -0.5714| * |Cb - 128|
456 		 * |B|   |1.0000  1.8814  0.0000|   |Cr - 128|
457 		 */
458 		static const u32 csc1_coef_bt2020_full[3] = {
459 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
460 			BF_PXP_CSC1_COEF0_C0(0x100) |	/*  1.0000 (+0.00 %) */
461 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
462 			BF_PXP_CSC1_COEF0_Y_OFFSET(0),
463 			BF_PXP_CSC1_COEF1_C1(0x179) |	/*  1.4727 (-0.19 %) */
464 			BF_PXP_CSC1_COEF1_C4(0x1e1),	/*  1.8789 (-0.25 %) */
465 			BF_PXP_CSC1_COEF2_C2(0x76e) |	/* -0.5703 (+0.11 %) */
466 			BF_PXP_CSC1_COEF2_C3(0x7d6),	/* -0.1641 (+0.05 %) */
467 		};
468 		/*
469 		 * SMPTE 240m limited range:
470 		 *
471 		 * |R|   |1.1644  0.0000  1.7937|   |Y  - 16 |
472 		 * |G| = |1.1644 -0.2565 -0.5427| * |Cb - 128|
473 		 * |B|   |1.1644  2.0798  0.0000|   |Cr - 128|
474 		 */
475 		static const u32 csc1_coef_smpte240m_lim[3] = {
476 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
477 			BF_PXP_CSC1_COEF0_C0(0x12a) |	/*  1.1641 (-0.03 %) */
478 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
479 			BF_PXP_CSC1_COEF0_Y_OFFSET(-16),
480 			BF_PXP_CSC1_COEF1_C1(0x1cb) |	/*  1.7930 (-0.07 %) */
481 			BF_PXP_CSC1_COEF1_C4(0x214),	/*  2.0781 (-0.17 %) */
482 			BF_PXP_CSC1_COEF2_C2(0x776) |	/* -0.5391 (+0.36 %) */
483 			BF_PXP_CSC1_COEF2_C3(0x7bf),	/* -0.2539 (+0.26 %) */
484 		};
485 		/*
486 		 * SMPTE 240m full range:
487 		 *
488 		 * |R|   |1.0000  0.0000  1.5756|   |Y  + 0  |
489 		 * |G| = |1.0000 -0.2253 -0.4767| * |Cb - 128|
490 		 * |B|   |1.0000  1.8270  0.0000|   |Cr - 128|
491 		 */
492 		static const u32 csc1_coef_smpte240m_full[3] = {
493 			BM_PXP_CSC1_COEF0_YCBCR_MODE |
494 			BF_PXP_CSC1_COEF0_C0(0x100) |	/*  1.0000 (+0.00 %) */
495 			BF_PXP_CSC1_COEF0_UV_OFFSET(-128) |
496 			BF_PXP_CSC1_COEF0_Y_OFFSET(0),
497 			BF_PXP_CSC1_COEF1_C1(0x193) |	/*  1.5742 (-0.14 %) */
498 			BF_PXP_CSC1_COEF1_C4(0x1d3),	/*  1.8242 (-0.28 %) */
499 			BF_PXP_CSC1_COEF2_C2(0x786) |	/* -0.4766 (+0.01 %) */
500 			BF_PXP_CSC1_COEF2_C3(0x7c7),	/* -0.2227 (+0.26 %) */
501 		};
502 		const u32 *csc1_coef;
503 
504 		ycbcr_enc = ctx->q_data[V4L2_M2M_SRC].ycbcr_enc;
505 		quantization = ctx->q_data[V4L2_M2M_SRC].quant;
506 
507 		if (ycbcr_enc == V4L2_YCBCR_ENC_601) {
508 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
509 				csc1_coef = csc1_coef_bt601_full;
510 			else
511 				csc1_coef = csc1_coef_bt601_lim;
512 		} else if (ycbcr_enc == V4L2_YCBCR_ENC_709) {
513 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
514 				csc1_coef = csc1_coef_rec709_full;
515 			else
516 				csc1_coef = csc1_coef_rec709_lim;
517 		} else if (ycbcr_enc == V4L2_YCBCR_ENC_BT2020) {
518 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
519 				csc1_coef = csc1_coef_bt2020_full;
520 			else
521 				csc1_coef = csc1_coef_bt2020_lim;
522 		} else {
523 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
524 				csc1_coef = csc1_coef_smpte240m_full;
525 			else
526 				csc1_coef = csc1_coef_smpte240m_lim;
527 		}
528 
529 		pxp_write(dev, HW_PXP_CSC1_COEF0, csc1_coef[0]);
530 		pxp_write(dev, HW_PXP_CSC1_COEF1, csc1_coef[1]);
531 		pxp_write(dev, HW_PXP_CSC1_COEF2, csc1_coef[2]);
532 	} else {
533 		pxp_write(dev, HW_PXP_CSC1_COEF0, BM_PXP_CSC1_COEF0_BYPASS);
534 	}
535 
536 	if (!pxp_v4l2_pix_fmt_is_yuv(ctx->q_data[V4L2_M2M_SRC].fmt->fourcc) &&
537 	    pxp_v4l2_pix_fmt_is_yuv(ctx->q_data[V4L2_M2M_DST].fmt->fourcc)) {
538 		/*
539 		 * CSC2 RGB to YUV/YCbCr conversion is implemented as follows:
540 		 *
541 		 * |Y |   |A1 A2 A3|   |R|   |D1|
542 		 * |Cb| = |B1 B2 B3| * |G| + |D2|
543 		 * |Cr|   |C1 C2 C3|   |B|   |D3|
544 		 *
545 		 * Results are clamped to 0..255.
546 		 *
547 		 * BT.601 limited range:
548 		 *
549 		 * |Y |   | 0.2568  0.5041  0.0979|   |R|   |16 |
550 		 * |Cb| = |-0.1482 -0.2910  0.4392| * |G| + |128|
551 		 * |Cr|   | 0.4392  0.4392 -0.3678|   |B|   |128|
552 		 */
553 		static const u32 csc2_coef_bt601_lim[6] = {
554 			BF_PXP_CSC2_COEF0_A2(0x081) |	/*  0.5039 (-0.02 %) */
555 			BF_PXP_CSC2_COEF0_A1(0x041),	/*  0.2539 (-0.29 %) */
556 			BF_PXP_CSC2_COEF1_B1(0x7db) |	/* -0.1445 (+0.37 %) */
557 			BF_PXP_CSC2_COEF1_A3(0x019),	/*  0.0977 (-0.02 %) */
558 			BF_PXP_CSC2_COEF2_B3(0x070) |	/*  0.4375 (-0.17 %) */
559 			BF_PXP_CSC2_COEF2_B2(0x7b6),	/* -0.2891 (+0.20 %) */
560 			BF_PXP_CSC2_COEF3_C2(0x7a2) |	/* -0.3672 (+0.06 %) */
561 			BF_PXP_CSC2_COEF3_C1(0x070),	/*  0.4375 (-0.17 %) */
562 			BF_PXP_CSC2_COEF4_D1(16) |
563 			BF_PXP_CSC2_COEF4_C3(0x7ee),	/* -0.0703 (+0.11 %) */
564 			BF_PXP_CSC2_COEF5_D3(128) |
565 			BF_PXP_CSC2_COEF5_D2(128),
566 		};
567 		/*
568 		 * BT.601 full range:
569 		 *
570 		 * |Y |   | 0.2990  0.5870  0.1140|   |R|   |0  |
571 		 * |Cb| = |-0.1687 -0.3313  0.5000| * |G| + |128|
572 		 * |Cr|   | 0.5000  0.5000 -0.4187|   |B|   |128|
573 		 */
574 		static const u32 csc2_coef_bt601_full[6] = {
575 			BF_PXP_CSC2_COEF0_A2(0x096) |	/*  0.5859 (-0.11 %) */
576 			BF_PXP_CSC2_COEF0_A1(0x04c),	/*  0.2969 (-0.21 %) */
577 			BF_PXP_CSC2_COEF1_B1(0x7d5) |	/* -0.1680 (+0.07 %) */
578 			BF_PXP_CSC2_COEF1_A3(0x01d),	/*  0.1133 (-0.07 %) */
579 			BF_PXP_CSC2_COEF2_B3(0x080) |	/*  0.5000 (+0.00 %) */
580 			BF_PXP_CSC2_COEF2_B2(0x7ac),	/* -0.3281 (+0.32 %) */
581 			BF_PXP_CSC2_COEF3_C2(0x795) |	/* -0.4180 (+0.07 %) */
582 			BF_PXP_CSC2_COEF3_C1(0x080),	/*  0.5000 (+0.00 %) */
583 			BF_PXP_CSC2_COEF4_D1(0) |
584 			BF_PXP_CSC2_COEF4_C3(0x7ec),	/* -0.0781 (+0.32 %) */
585 			BF_PXP_CSC2_COEF5_D3(128) |
586 			BF_PXP_CSC2_COEF5_D2(128),
587 		};
588 		/*
589 		 * Rec.709 limited range:
590 		 *
591 		 * |Y |   | 0.1826  0.6142  0.0620|   |R|   |16 |
592 		 * |Cb| = |-0.1007 -0.3385  0.4392| * |G| + |128|
593 		 * |Cr|   | 0.4392  0.4392 -0.3990|   |B|   |128|
594 		 */
595 		static const u32 csc2_coef_rec709_lim[6] = {
596 			BF_PXP_CSC2_COEF0_A2(0x09d) |	/*  0.6133 (-0.09 %) */
597 			BF_PXP_CSC2_COEF0_A1(0x02e),	/*  0.1797 (-0.29 %) */
598 			BF_PXP_CSC2_COEF1_B1(0x7e7) |	/* -0.0977 (+0.30 %) */
599 			BF_PXP_CSC2_COEF1_A3(0x00f),	/*  0.0586 (-0.34 %) */
600 			BF_PXP_CSC2_COEF2_B3(0x070) |	/*  0.4375 (-0.17 %) */
601 			BF_PXP_CSC2_COEF2_B2(0x7aa),	/* -0.3359 (+0.26 %) */
602 			BF_PXP_CSC2_COEF3_C2(0x79a) |	/* -0.3984 (+0.05 %) */
603 			BF_PXP_CSC2_COEF3_C1(0x070),	/*  0.4375 (-0.17 %) */
604 			BF_PXP_CSC2_COEF4_D1(16) |
605 			BF_PXP_CSC2_COEF4_C3(0x7f6),	/* -0.0391 (+0.12 %) */
606 			BF_PXP_CSC2_COEF5_D3(128) |
607 			BF_PXP_CSC2_COEF5_D2(128),
608 		};
609 		/*
610 		 * Rec.709 full range:
611 		 *
612 		 * |Y |   | 0.2126  0.7152  0.0722|   |R|   |0  |
613 		 * |Cb| = |-0.1146 -0.3854  0.5000| * |G| + |128|
614 		 * |Cr|   | 0.5000  0.5000 -0.4542|   |B|   |128|
615 		 */
616 		static const u32 csc2_coef_rec709_full[6] = {
617 			BF_PXP_CSC2_COEF0_A2(0x0b7) |	/*  0.7148 (-0.04 %) */
618 			BF_PXP_CSC2_COEF0_A1(0x036),	/*  0.2109 (-0.17 %) */
619 			BF_PXP_CSC2_COEF1_B1(0x7e3) |	/* -0.1133 (+0.13 %) */
620 			BF_PXP_CSC2_COEF1_A3(0x012),	/*  0.0703 (-0.19 %) */
621 			BF_PXP_CSC2_COEF2_B3(0x080) |	/*  0.5000 (+0.00 %) */
622 			BF_PXP_CSC2_COEF2_B2(0x79e),	/* -0.3828 (+0.26 %) */
623 			BF_PXP_CSC2_COEF3_C2(0x78c) |	/* -0.4531 (+0.11 %) */
624 			BF_PXP_CSC2_COEF3_C1(0x080),	/*  0.5000 (+0.00 %) */
625 			BF_PXP_CSC2_COEF4_D1(0) |
626 			BF_PXP_CSC2_COEF4_C3(0x7f5),	/* -0.0430 (+0.28 %) */
627 			BF_PXP_CSC2_COEF5_D3(128) |
628 			BF_PXP_CSC2_COEF5_D2(128),
629 		};
630 		/*
631 		 * BT.2020 limited range:
632 		 *
633 		 * |Y |   | 0.2256  0.5823  0.0509|   |R|   |16 |
634 		 * |Cb| = |-0.1226 -0.3166  0.4392| * |G| + |128|
635 		 * |Cr|   | 0.4392  0.4392 -0.4039|   |B|   |128|
636 		 */
637 		static const u32 csc2_coef_bt2020_lim[6] = {
638 			BF_PXP_CSC2_COEF0_A2(0x095) |	/*  0.5820 (-0.03 %) */
639 			BF_PXP_CSC2_COEF0_A1(0x039),	/*  0.2227 (-0.30 %) */
640 			BF_PXP_CSC2_COEF1_B1(0x7e1) |	/* -0.1211 (+0.15 %) */
641 			BF_PXP_CSC2_COEF1_A3(0x00d),	/*  0.0508 (-0.01 %) */
642 			BF_PXP_CSC2_COEF2_B3(0x070) |	/*  0.4375 (-0.17 %) */
643 			BF_PXP_CSC2_COEF2_B2(0x7af),	/* -0.3164 (+0.02 %) */
644 			BF_PXP_CSC2_COEF3_C2(0x799) |	/* -0.4023 (+0.16 %) */
645 			BF_PXP_CSC2_COEF3_C1(0x070),	/*  0.4375 (-0.17 %) */
646 			BF_PXP_CSC2_COEF4_D1(16) |
647 			BF_PXP_CSC2_COEF4_C3(0x7f7),	/* -0.0352 (+0.02 %) */
648 			BF_PXP_CSC2_COEF5_D3(128) |
649 			BF_PXP_CSC2_COEF5_D2(128),
650 		};
651 		/*
652 		 * BT.2020 full range:
653 		 *
654 		 * |Y |   | 0.2627  0.6780  0.0593|   |R|   |0  |
655 		 * |Cb| = |-0.1396 -0.3604  0.5000| * |G| + |128|
656 		 * |Cr|   | 0.5000  0.5000 -0.4598|   |B|   |128|
657 		 */
658 		static const u32 csc2_coef_bt2020_full[6] = {
659 			BF_PXP_CSC2_COEF0_A2(0x0ad) |	/*  0.6758 (-0.22 %) */
660 			BF_PXP_CSC2_COEF0_A1(0x043),	/*  0.2617 (-0.10 %) */
661 			BF_PXP_CSC2_COEF1_B1(0x7dd) |	/* -0.1367 (+0.29 %) */
662 			BF_PXP_CSC2_COEF1_A3(0x00f),	/*  0.0586 (-0.07 %) */
663 			BF_PXP_CSC2_COEF2_B3(0x080) |	/*  0.5000 (+0.00 %) */
664 			BF_PXP_CSC2_COEF2_B2(0x7a4),	/* -0.3594 (+0.10 %) */
665 			BF_PXP_CSC2_COEF3_C2(0x78b) |	/* -0.4570 (+0.28 %) */
666 			BF_PXP_CSC2_COEF3_C1(0x080),	/*  0.5000 (+0.00 %) */
667 			BF_PXP_CSC2_COEF4_D1(0) |
668 			BF_PXP_CSC2_COEF4_C3(0x7f6),	/* -0.0391 (+0.11 %) */
669 			BF_PXP_CSC2_COEF5_D3(128) |
670 			BF_PXP_CSC2_COEF5_D2(128),
671 		};
672 		/*
673 		 * SMPTE 240m limited range:
674 		 *
675 		 * |Y |   | 0.1821  0.6020  0.0747|   |R|   |16 |
676 		 * |Cb| = |-0.1019 -0.3373  0.4392| * |G| + |128|
677 		 * |Cr|   | 0.4392  0.4392 -0.3909|   |B|   |128|
678 		 */
679 		static const u32 csc2_coef_smpte240m_lim[6] = {
680 			BF_PXP_CSC2_COEF0_A2(0x09a) |	/*  0.6016 (-0.05 %) */
681 			BF_PXP_CSC2_COEF0_A1(0x02e),	/*  0.1797 (-0.24 %) */
682 			BF_PXP_CSC2_COEF1_B1(0x7e6) |	/* -0.1016 (+0.03 %) */
683 			BF_PXP_CSC2_COEF1_A3(0x013),	/*  0.0742 (-0.05 %) */
684 			BF_PXP_CSC2_COEF2_B3(0x070) |	/*  0.4375 (-0.17 %) */
685 			BF_PXP_CSC2_COEF2_B2(0x7aa),	/* -0.3359 (+0.14 %) */
686 			BF_PXP_CSC2_COEF3_C2(0x79c) |	/* -0.3906 (+0.03 %) */
687 			BF_PXP_CSC2_COEF3_C1(0x070),	/*  0.4375 (-0.17 %) */
688 			BF_PXP_CSC2_COEF4_D1(16) |
689 			BF_PXP_CSC2_COEF4_C3(0x7f4),	/* -0.0469 (+0.14 %) */
690 			BF_PXP_CSC2_COEF5_D3(128) |
691 			BF_PXP_CSC2_COEF5_D2(128),
692 		};
693 		/*
694 		 * SMPTE 240m full range:
695 		 *
696 		 * |Y |   | 0.2120  0.7010  0.0870|   |R|   |0  |
697 		 * |Cb| = |-0.1160 -0.3840  0.5000| * |G| + |128|
698 		 * |Cr|   | 0.5000  0.5000 -0.4450|   |B|   |128|
699 		 */
700 		static const u32 csc2_coef_smpte240m_full[6] = {
701 			BF_PXP_CSC2_COEF0_A2(0x0b3) |	/*  0.6992 (-0.18 %) */
702 			BF_PXP_CSC2_COEF0_A1(0x036),	/*  0.2109 (-0.11 %) */
703 			BF_PXP_CSC2_COEF1_B1(0x7e3) |	/* -0.1133 (+0.27 %) */
704 			BF_PXP_CSC2_COEF1_A3(0x016),	/*  0.0859 (-0.11 %) */
705 			BF_PXP_CSC2_COEF2_B3(0x080) |	/*  0.5000 (+0.00 %) */
706 			BF_PXP_CSC2_COEF2_B2(0x79e),	/* -0.3828 (+0.12 %) */
707 			BF_PXP_CSC2_COEF3_C2(0x78f) |	/* -0.4414 (+0.36 %) */
708 			BF_PXP_CSC2_COEF3_C1(0x080),	/*  0.5000 (+0.00 %) */
709 			BF_PXP_CSC2_COEF4_D1(0) |
710 			BF_PXP_CSC2_COEF4_C3(0x7f2),	/* -0.0547 (+0.03 %) */
711 			BF_PXP_CSC2_COEF5_D3(128) |
712 			BF_PXP_CSC2_COEF5_D2(128),
713 		};
714 		const u32 *csc2_coef;
715 		u32 csc2_ctrl;
716 
717 		ycbcr_enc = ctx->q_data[V4L2_M2M_DST].ycbcr_enc;
718 		quantization = ctx->q_data[V4L2_M2M_DST].quant;
719 
720 		if (ycbcr_enc == V4L2_YCBCR_ENC_601) {
721 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
722 				csc2_coef = csc2_coef_bt601_full;
723 			else
724 				csc2_coef = csc2_coef_bt601_lim;
725 		} else if (ycbcr_enc == V4L2_YCBCR_ENC_709) {
726 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
727 				csc2_coef = csc2_coef_rec709_full;
728 			else
729 				csc2_coef = csc2_coef_rec709_lim;
730 		} else if (ycbcr_enc == V4L2_YCBCR_ENC_BT2020) {
731 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
732 				csc2_coef = csc2_coef_bt2020_full;
733 			else
734 				csc2_coef = csc2_coef_bt2020_lim;
735 		} else {
736 			if (quantization == V4L2_QUANTIZATION_FULL_RANGE)
737 				csc2_coef = csc2_coef_smpte240m_full;
738 			else
739 				csc2_coef = csc2_coef_smpte240m_lim;
740 		}
741 		if (quantization == V4L2_QUANTIZATION_FULL_RANGE) {
742 			csc2_ctrl = BV_PXP_CSC2_CTRL_CSC_MODE__RGB2YUV <<
743 				    BP_PXP_CSC2_CTRL_CSC_MODE;
744 		} else {
745 			csc2_ctrl = BV_PXP_CSC2_CTRL_CSC_MODE__RGB2YCbCr <<
746 				    BP_PXP_CSC2_CTRL_CSC_MODE;
747 		}
748 
749 		pxp_write(dev, HW_PXP_CSC2_CTRL, csc2_ctrl);
750 		pxp_write(dev, HW_PXP_CSC2_COEF0, csc2_coef[0]);
751 		pxp_write(dev, HW_PXP_CSC2_COEF1, csc2_coef[1]);
752 		pxp_write(dev, HW_PXP_CSC2_COEF2, csc2_coef[2]);
753 		pxp_write(dev, HW_PXP_CSC2_COEF3, csc2_coef[3]);
754 		pxp_write(dev, HW_PXP_CSC2_COEF4, csc2_coef[4]);
755 		pxp_write(dev, HW_PXP_CSC2_COEF5, csc2_coef[5]);
756 	} else {
757 		pxp_write(dev, HW_PXP_CSC2_CTRL, BM_PXP_CSC2_CTRL_BYPASS);
758 	}
759 }
760 
761 static u32 pxp_imx6ull_data_path_ctrl0(struct pxp_ctx *ctx)
762 {
763 	u32 ctrl0;
764 
765 	ctrl0 = 0;
766 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX15_SEL(3);
767 	/* Bypass Dithering x3CH */
768 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX14_SEL(1);
769 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX13_SEL(3);
770 	/* Select Rotation */
771 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX12_SEL(0);
772 	/* Bypass LUT */
773 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX11_SEL(1);
774 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX10_SEL(3);
775 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX9_SEL(3);
776 	/* Select CSC 2 */
777 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX8_SEL(0);
778 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX7_SEL(3);
779 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX6_SEL(3);
780 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX5_SEL(3);
781 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX4_SEL(3);
782 	/* Bypass Rotation 2 */
783 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX3_SEL(0);
784 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX2_SEL(3);
785 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX1_SEL(3);
786 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX0_SEL(3);
787 
788 	return ctrl0;
789 }
790 
791 static u32 pxp_imx7d_data_path_ctrl0(struct pxp_ctx *ctx)
792 {
793 	u32 ctrl0;
794 
795 	ctrl0 = 0;
796 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX15_SEL(3);
797 	/* Select Rotation 0 */
798 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX14_SEL(0);
799 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX13_SEL(3);
800 	/* Select MUX11 for Rotation 0 */
801 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX12_SEL(1);
802 	/* Bypass LUT */
803 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX11_SEL(1);
804 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX10_SEL(3);
805 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX9_SEL(3);
806 	/* Select CSC 2 */
807 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX8_SEL(0);
808 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX7_SEL(3);
809 	/* Select Composite Alpha Blending/Color Key 0 for CSC 2 */
810 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX6_SEL(1);
811 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX5_SEL(3);
812 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX4_SEL(3);
813 	/* Bypass Rotation 1 */
814 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX3_SEL(0);
815 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX2_SEL(3);
816 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX1_SEL(3);
817 	ctrl0 |= BF_PXP_DATA_PATH_CTRL0_MUX0_SEL(3);
818 
819 	return ctrl0;
820 }
821 
822 static void pxp_set_data_path(struct pxp_ctx *ctx)
823 {
824 	struct pxp_dev *dev = ctx->dev;
825 	u32 ctrl0;
826 	u32 ctrl1;
827 
828 	ctrl0 = dev->pdata->data_path_ctrl0(ctx);
829 
830 	ctrl1 = 0;
831 	ctrl1 |= BF_PXP_DATA_PATH_CTRL1_MUX17_SEL(3);
832 	ctrl1 |= BF_PXP_DATA_PATH_CTRL1_MUX16_SEL(3);
833 
834 	pxp_write(dev, HW_PXP_DATA_PATH_CTRL0, ctrl0);
835 	pxp_write(dev, HW_PXP_DATA_PATH_CTRL1, ctrl1);
836 }
837 
838 static int pxp_start(struct pxp_ctx *ctx, struct vb2_v4l2_buffer *in_vb,
839 		     struct vb2_v4l2_buffer *out_vb)
840 {
841 	struct pxp_dev *dev = ctx->dev;
842 	struct pxp_q_data *q_data;
843 	u32 src_width, src_height, src_stride, src_fourcc;
844 	u32 dst_width, dst_height, dst_stride, dst_fourcc;
845 	dma_addr_t p_in, p_out;
846 	u32 ctrl, out_ctrl, out_buf, out_buf2, out_pitch, out_lrc, out_ps_ulc;
847 	u32 out_ps_lrc;
848 	u32 ps_ctrl, ps_buf, ps_ubuf, ps_vbuf, ps_pitch, ps_scale, ps_offset;
849 	u32 as_ulc, as_lrc;
850 	u32 y_size;
851 	u32 decx, decy, xscale, yscale;
852 
853 	q_data = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
854 
855 	src_width = ctx->q_data[V4L2_M2M_SRC].width;
856 	dst_width = ctx->q_data[V4L2_M2M_DST].width;
857 	src_height = ctx->q_data[V4L2_M2M_SRC].height;
858 	dst_height = ctx->q_data[V4L2_M2M_DST].height;
859 	src_stride = ctx->q_data[V4L2_M2M_SRC].bytesperline;
860 	dst_stride = ctx->q_data[V4L2_M2M_DST].bytesperline;
861 	src_fourcc = ctx->q_data[V4L2_M2M_SRC].fmt->fourcc;
862 	dst_fourcc = ctx->q_data[V4L2_M2M_DST].fmt->fourcc;
863 
864 	p_in = vb2_dma_contig_plane_dma_addr(&in_vb->vb2_buf, 0);
865 	p_out = vb2_dma_contig_plane_dma_addr(&out_vb->vb2_buf, 0);
866 
867 	if (!p_in || !p_out) {
868 		v4l2_err(&dev->v4l2_dev,
869 			 "Acquiring DMA addresses of buffers failed\n");
870 		return -EFAULT;
871 	}
872 
873 	out_vb->sequence =
874 		get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE)->sequence++;
875 	in_vb->sequence = q_data->sequence++;
876 	out_vb->vb2_buf.timestamp = in_vb->vb2_buf.timestamp;
877 
878 	if (in_vb->flags & V4L2_BUF_FLAG_TIMECODE)
879 		out_vb->timecode = in_vb->timecode;
880 	out_vb->field = in_vb->field;
881 	out_vb->flags = in_vb->flags &
882 		(V4L2_BUF_FLAG_TIMECODE |
883 		 V4L2_BUF_FLAG_KEYFRAME |
884 		 V4L2_BUF_FLAG_PFRAME |
885 		 V4L2_BUF_FLAG_BFRAME |
886 		 V4L2_BUF_FLAG_TSTAMP_SRC_MASK);
887 
888 	/* 8x8 block size */
889 	ctrl = BF_PXP_CTRL_VFLIP0(!!(ctx->mode & MEM2MEM_VFLIP)) |
890 	       BF_PXP_CTRL_HFLIP0(!!(ctx->mode & MEM2MEM_HFLIP)) |
891 	       BF_PXP_CTRL_ROTATE0(ctx->rotation);
892 	/* Always write alpha value as V4L2_CID_ALPHA_COMPONENT */
893 	out_ctrl = BF_PXP_OUT_CTRL_ALPHA(ctx->alpha_component) |
894 		   BF_PXP_OUT_CTRL_ALPHA_OUTPUT(1) |
895 		   pxp_v4l2_pix_fmt_to_out_format(dst_fourcc);
896 	out_buf = p_out;
897 
898 	if (ctx->rotation == BV_PXP_CTRL_ROTATE0__ROT_90 ||
899 	    ctx->rotation == BV_PXP_CTRL_ROTATE0__ROT_270)
900 		swap(dst_width, dst_height);
901 
902 	switch (dst_fourcc) {
903 	case V4L2_PIX_FMT_NV12:
904 	case V4L2_PIX_FMT_NV21:
905 	case V4L2_PIX_FMT_NV16:
906 	case V4L2_PIX_FMT_NV61:
907 		out_buf2 = out_buf + dst_stride * dst_height;
908 		break;
909 	default:
910 		out_buf2 = 0;
911 	}
912 
913 	out_pitch = BF_PXP_OUT_PITCH_PITCH(dst_stride);
914 	out_lrc = BF_PXP_OUT_LRC_X(dst_width - 1) |
915 		  BF_PXP_OUT_LRC_Y(dst_height - 1);
916 	/* PS covers whole output */
917 	out_ps_ulc = BF_PXP_OUT_PS_ULC_X(0) | BF_PXP_OUT_PS_ULC_Y(0);
918 	out_ps_lrc = BF_PXP_OUT_PS_LRC_X(dst_width - 1) |
919 		     BF_PXP_OUT_PS_LRC_Y(dst_height - 1);
920 	/* no AS */
921 	as_ulc = BF_PXP_OUT_AS_ULC_X(1) | BF_PXP_OUT_AS_ULC_Y(1);
922 	as_lrc = BF_PXP_OUT_AS_LRC_X(0) | BF_PXP_OUT_AS_LRC_Y(0);
923 
924 	decx = (src_width <= dst_width) ? 0 : ilog2(src_width / dst_width);
925 	decy = (src_height <= dst_height) ? 0 : ilog2(src_height / dst_height);
926 	ps_ctrl = BF_PXP_PS_CTRL_DECX(decx) | BF_PXP_PS_CTRL_DECY(decy) |
927 		  pxp_v4l2_pix_fmt_to_ps_format(src_fourcc);
928 	ps_buf = p_in;
929 	y_size = src_stride * src_height;
930 	switch (src_fourcc) {
931 	case V4L2_PIX_FMT_YUV420:
932 		ps_ubuf = ps_buf + y_size;
933 		ps_vbuf = ps_ubuf + y_size / 4;
934 		break;
935 	case V4L2_PIX_FMT_YUV422P:
936 		ps_ubuf = ps_buf + y_size;
937 		ps_vbuf = ps_ubuf + y_size / 2;
938 		break;
939 	case V4L2_PIX_FMT_NV12:
940 	case V4L2_PIX_FMT_NV21:
941 	case V4L2_PIX_FMT_NV16:
942 	case V4L2_PIX_FMT_NV61:
943 		ps_ubuf = ps_buf + y_size;
944 		ps_vbuf = 0;
945 		break;
946 	case V4L2_PIX_FMT_GREY:
947 	case V4L2_PIX_FMT_Y4:
948 		ps_ubuf = 0;
949 		/* In grayscale mode, ps_vbuf contents are reused as CbCr */
950 		ps_vbuf = 0x8080;
951 		break;
952 	default:
953 		ps_ubuf = 0;
954 		ps_vbuf = 0;
955 		break;
956 	}
957 	ps_pitch = BF_PXP_PS_PITCH_PITCH(src_stride);
958 	if (decx) {
959 		xscale = (src_width >> decx) * 0x1000 / dst_width;
960 	} else {
961 		switch (src_fourcc) {
962 		case V4L2_PIX_FMT_UYVY:
963 		case V4L2_PIX_FMT_YUYV:
964 		case V4L2_PIX_FMT_VYUY:
965 		case V4L2_PIX_FMT_YVYU:
966 		case V4L2_PIX_FMT_NV16:
967 		case V4L2_PIX_FMT_NV12:
968 		case V4L2_PIX_FMT_NV21:
969 		case V4L2_PIX_FMT_NV61:
970 		case V4L2_PIX_FMT_YUV422P:
971 		case V4L2_PIX_FMT_YUV420:
972 			/*
973 			 * This avoids sampling past the right edge for
974 			 * horizontally chroma subsampled formats.
975 			 */
976 			xscale = (src_width - 2) * 0x1000 / (dst_width - 1);
977 			break;
978 		default:
979 			xscale = (src_width - 1) * 0x1000 / (dst_width - 1);
980 			break;
981 		}
982 	}
983 	if (decy)
984 		yscale = (src_height >> decy) * 0x1000 / dst_height;
985 	else
986 		yscale = (src_height - 1) * 0x1000 / (dst_height - 1);
987 	ps_scale = BF_PXP_PS_SCALE_YSCALE(yscale) |
988 		   BF_PXP_PS_SCALE_XSCALE(xscale);
989 	ps_offset = BF_PXP_PS_OFFSET_YOFFSET(0) | BF_PXP_PS_OFFSET_XOFFSET(0);
990 
991 	pxp_write(dev, HW_PXP_CTRL, ctrl);
992 	/* skip STAT */
993 	pxp_write(dev, HW_PXP_OUT_CTRL, out_ctrl);
994 	pxp_write(dev, HW_PXP_OUT_BUF, out_buf);
995 	pxp_write(dev, HW_PXP_OUT_BUF2, out_buf2);
996 	pxp_write(dev, HW_PXP_OUT_PITCH, out_pitch);
997 	pxp_write(dev, HW_PXP_OUT_LRC, out_lrc);
998 	pxp_write(dev, HW_PXP_OUT_PS_ULC, out_ps_ulc);
999 	pxp_write(dev, HW_PXP_OUT_PS_LRC, out_ps_lrc);
1000 	pxp_write(dev, HW_PXP_OUT_AS_ULC, as_ulc);
1001 	pxp_write(dev, HW_PXP_OUT_AS_LRC, as_lrc);
1002 	pxp_write(dev, HW_PXP_PS_CTRL, ps_ctrl);
1003 	pxp_write(dev, HW_PXP_PS_BUF, ps_buf);
1004 	pxp_write(dev, HW_PXP_PS_UBUF, ps_ubuf);
1005 	pxp_write(dev, HW_PXP_PS_VBUF, ps_vbuf);
1006 	pxp_write(dev, HW_PXP_PS_PITCH, ps_pitch);
1007 	pxp_write(dev, HW_PXP_PS_BACKGROUND_0, 0x00ffffff);
1008 	pxp_write(dev, HW_PXP_PS_SCALE, ps_scale);
1009 	pxp_write(dev, HW_PXP_PS_OFFSET, ps_offset);
1010 	/* disable processed surface color keying */
1011 	pxp_write(dev, HW_PXP_PS_CLRKEYLOW_0, 0x00ffffff);
1012 	pxp_write(dev, HW_PXP_PS_CLRKEYHIGH_0, 0x00000000);
1013 
1014 	/* disable alpha surface color keying */
1015 	pxp_write(dev, HW_PXP_AS_CLRKEYLOW_0, 0x00ffffff);
1016 	pxp_write(dev, HW_PXP_AS_CLRKEYHIGH_0, 0x00000000);
1017 
1018 	/* setup CSC */
1019 	pxp_setup_csc(ctx);
1020 
1021 	/* bypass LUT */
1022 	pxp_write(dev, HW_PXP_LUT_CTRL, BM_PXP_LUT_CTRL_BYPASS);
1023 
1024 	pxp_set_data_path(ctx);
1025 
1026 	pxp_write(dev, HW_PXP_IRQ_MASK, 0xffff);
1027 
1028 	/* ungate, enable PS/AS/OUT and PXP operation */
1029 	pxp_write(dev, HW_PXP_CTRL_SET, BM_PXP_CTRL_IRQ_ENABLE);
1030 	pxp_write(dev, HW_PXP_CTRL_SET,
1031 		  BM_PXP_CTRL_ENABLE | BM_PXP_CTRL_ENABLE_CSC2 |
1032 		  BM_PXP_CTRL_ENABLE_ROTATE0 | BM_PXP_CTRL_ENABLE_PS_AS_OUT);
1033 
1034 	return 0;
1035 }
1036 
1037 static void pxp_job_finish(struct pxp_dev *dev)
1038 {
1039 	struct pxp_ctx *curr_ctx;
1040 	struct vb2_v4l2_buffer *src_vb, *dst_vb;
1041 	unsigned long flags;
1042 
1043 	curr_ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
1044 
1045 	if (curr_ctx == NULL) {
1046 		pr_err("Instance released before the end of transaction\n");
1047 		return;
1048 	}
1049 
1050 	src_vb = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
1051 	dst_vb = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
1052 
1053 	spin_lock_irqsave(&dev->irqlock, flags);
1054 	v4l2_m2m_buf_done(src_vb, VB2_BUF_STATE_DONE);
1055 	v4l2_m2m_buf_done(dst_vb, VB2_BUF_STATE_DONE);
1056 	spin_unlock_irqrestore(&dev->irqlock, flags);
1057 
1058 	dprintk(curr_ctx->dev, "Finishing transaction\n");
1059 	v4l2_m2m_job_finish(dev->m2m_dev, curr_ctx->fh.m2m_ctx);
1060 }
1061 
1062 /*
1063  * mem2mem callbacks
1064  */
1065 static void pxp_device_run(void *priv)
1066 {
1067 	struct pxp_ctx *ctx = priv;
1068 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
1069 
1070 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1071 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1072 
1073 	pxp_start(ctx, src_buf, dst_buf);
1074 }
1075 
1076 static int pxp_job_ready(void *priv)
1077 {
1078 	struct pxp_ctx *ctx = priv;
1079 
1080 	if (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) < 1 ||
1081 	    v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx) < 1) {
1082 		dprintk(ctx->dev, "Not enough buffers available\n");
1083 		return 0;
1084 	}
1085 
1086 	return 1;
1087 }
1088 
1089 static void pxp_job_abort(void *priv)
1090 {
1091 	struct pxp_ctx *ctx = priv;
1092 
1093 	/* Will cancel the transaction in the next interrupt handler */
1094 	ctx->aborting = 1;
1095 }
1096 
1097 /*
1098  * interrupt handler
1099  */
1100 static irqreturn_t pxp_irq_handler(int irq, void *dev_id)
1101 {
1102 	struct pxp_dev *dev = dev_id;
1103 	u32 stat;
1104 
1105 	stat = pxp_read(dev, HW_PXP_STAT);
1106 
1107 	if (stat & BM_PXP_STAT_IRQ0) {
1108 		/* we expect x = 0, y = height, irq0 = 1 */
1109 		if (stat & ~(BM_PXP_STAT_BLOCKX | BM_PXP_STAT_BLOCKY |
1110 			     BM_PXP_STAT_IRQ0))
1111 			dprintk(dev, "%s: stat = 0x%08x\n", __func__, stat);
1112 		pxp_write(dev, HW_PXP_STAT_CLR, BM_PXP_STAT_IRQ0);
1113 
1114 		pxp_job_finish(dev);
1115 	} else {
1116 		u32 irq = pxp_read(dev, HW_PXP_IRQ);
1117 
1118 		dprintk(dev, "%s: stat = 0x%08x\n", __func__, stat);
1119 		dprintk(dev, "%s: irq = 0x%08x\n", __func__, irq);
1120 
1121 		pxp_write(dev, HW_PXP_IRQ_CLR, irq);
1122 	}
1123 
1124 	return IRQ_HANDLED;
1125 }
1126 
1127 /*
1128  * video ioctls
1129  */
1130 static int pxp_querycap(struct file *file, void *priv,
1131 			   struct v4l2_capability *cap)
1132 {
1133 	strscpy(cap->driver, MEM2MEM_NAME, sizeof(cap->driver));
1134 	strscpy(cap->card, MEM2MEM_NAME, sizeof(cap->card));
1135 	return 0;
1136 }
1137 
1138 static int pxp_enum_fmt(struct v4l2_fmtdesc *f, u32 type)
1139 {
1140 	int i, num;
1141 	struct pxp_fmt *fmt;
1142 
1143 	num = 0;
1144 
1145 	for (i = 0; i < NUM_FORMATS; ++i) {
1146 		if (formats[i].types & type) {
1147 			/* index-th format of type type found ? */
1148 			if (num == f->index)
1149 				break;
1150 			/*
1151 			 * Correct type but haven't reached our index yet,
1152 			 * just increment per-type index
1153 			 */
1154 			++num;
1155 		}
1156 	}
1157 
1158 	if (i < NUM_FORMATS) {
1159 		/* Format found */
1160 		fmt = &formats[i];
1161 		f->pixelformat = fmt->fourcc;
1162 		return 0;
1163 	}
1164 
1165 	/* Format not found */
1166 	return -EINVAL;
1167 }
1168 
1169 static int pxp_enum_fmt_vid_cap(struct file *file, void *priv,
1170 				struct v4l2_fmtdesc *f)
1171 {
1172 	return pxp_enum_fmt(f, MEM2MEM_CAPTURE);
1173 }
1174 
1175 static int pxp_enum_fmt_vid_out(struct file *file, void *priv,
1176 				struct v4l2_fmtdesc *f)
1177 {
1178 	return pxp_enum_fmt(f, MEM2MEM_OUTPUT);
1179 }
1180 
1181 static int pxp_g_fmt(struct pxp_ctx *ctx, struct v4l2_format *f)
1182 {
1183 	struct vb2_queue *vq;
1184 	struct pxp_q_data *q_data;
1185 
1186 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1187 	if (!vq)
1188 		return -EINVAL;
1189 
1190 	q_data = get_q_data(ctx, f->type);
1191 
1192 	f->fmt.pix.width	= q_data->width;
1193 	f->fmt.pix.height	= q_data->height;
1194 	f->fmt.pix.field	= V4L2_FIELD_NONE;
1195 	f->fmt.pix.pixelformat	= q_data->fmt->fourcc;
1196 	f->fmt.pix.bytesperline	= q_data->bytesperline;
1197 	f->fmt.pix.sizeimage	= q_data->sizeimage;
1198 	f->fmt.pix.colorspace	= ctx->colorspace;
1199 	f->fmt.pix.xfer_func	= ctx->xfer_func;
1200 	f->fmt.pix.ycbcr_enc	= q_data->ycbcr_enc;
1201 	f->fmt.pix.quantization	= q_data->quant;
1202 
1203 	return 0;
1204 }
1205 
1206 static int pxp_g_fmt_vid_out(struct file *file, void *priv,
1207 				struct v4l2_format *f)
1208 {
1209 	return pxp_g_fmt(file2ctx(file), f);
1210 }
1211 
1212 static int pxp_g_fmt_vid_cap(struct file *file, void *priv,
1213 				struct v4l2_format *f)
1214 {
1215 	return pxp_g_fmt(file2ctx(file), f);
1216 }
1217 
1218 static inline u32 pxp_bytesperline(struct pxp_fmt *fmt, u32 width)
1219 {
1220 	switch (fmt->fourcc) {
1221 	case V4L2_PIX_FMT_YUV420:
1222 	case V4L2_PIX_FMT_NV12:
1223 	case V4L2_PIX_FMT_NV21:
1224 	case V4L2_PIX_FMT_YUV422P:
1225 	case V4L2_PIX_FMT_NV16:
1226 	case V4L2_PIX_FMT_NV61:
1227 		return width;
1228 	default:
1229 		return (width * fmt->depth) >> 3;
1230 	}
1231 }
1232 
1233 static inline u32 pxp_sizeimage(struct pxp_fmt *fmt, u32 width, u32 height)
1234 {
1235 	return (fmt->depth * width * height) >> 3;
1236 }
1237 
1238 static int pxp_try_fmt(struct v4l2_format *f, struct pxp_fmt *fmt)
1239 {
1240 	v4l_bound_align_image(&f->fmt.pix.width, MIN_W, MAX_W, ALIGN_W,
1241 			      &f->fmt.pix.height, MIN_H, MAX_H, ALIGN_H, 0);
1242 
1243 	f->fmt.pix.bytesperline = pxp_bytesperline(fmt, f->fmt.pix.width);
1244 	f->fmt.pix.sizeimage = pxp_sizeimage(fmt, f->fmt.pix.width,
1245 					     f->fmt.pix.height);
1246 	f->fmt.pix.field = V4L2_FIELD_NONE;
1247 
1248 	return 0;
1249 }
1250 
1251 static void
1252 pxp_fixup_colorimetry_cap(struct pxp_ctx *ctx, u32 dst_fourcc,
1253 			  enum v4l2_ycbcr_encoding *ycbcr_enc,
1254 			  enum v4l2_quantization *quantization)
1255 {
1256 	bool dst_is_yuv = pxp_v4l2_pix_fmt_is_yuv(dst_fourcc);
1257 
1258 	if (pxp_v4l2_pix_fmt_is_yuv(ctx->q_data[V4L2_M2M_SRC].fmt->fourcc) ==
1259 	    dst_is_yuv) {
1260 		/*
1261 		 * There is no support for conversion between different YCbCr
1262 		 * encodings or between RGB limited and full range.
1263 		 */
1264 		*ycbcr_enc = ctx->q_data[V4L2_M2M_SRC].ycbcr_enc;
1265 		*quantization = ctx->q_data[V4L2_M2M_SRC].quant;
1266 	} else {
1267 		*ycbcr_enc = V4L2_MAP_YCBCR_ENC_DEFAULT(ctx->colorspace);
1268 		*quantization = V4L2_MAP_QUANTIZATION_DEFAULT(!dst_is_yuv,
1269 							      ctx->colorspace,
1270 							      *ycbcr_enc);
1271 	}
1272 }
1273 
1274 static int pxp_try_fmt_vid_cap(struct file *file, void *priv,
1275 			       struct v4l2_format *f)
1276 {
1277 	struct pxp_fmt *fmt;
1278 	struct pxp_ctx *ctx = file2ctx(file);
1279 
1280 	fmt = find_format(f->fmt.pix.pixelformat);
1281 	if (!fmt) {
1282 		f->fmt.pix.pixelformat = formats[0].fourcc;
1283 		fmt = find_format(f->fmt.pix.pixelformat);
1284 	}
1285 	if (!(fmt->types & MEM2MEM_CAPTURE)) {
1286 		v4l2_err(&ctx->dev->v4l2_dev,
1287 			 "Fourcc format (0x%08x) invalid.\n",
1288 			 f->fmt.pix.pixelformat);
1289 		return -EINVAL;
1290 	}
1291 
1292 	f->fmt.pix.colorspace = ctx->colorspace;
1293 	f->fmt.pix.xfer_func = ctx->xfer_func;
1294 
1295 	pxp_fixup_colorimetry_cap(ctx, fmt->fourcc,
1296 				  &f->fmt.pix.ycbcr_enc,
1297 				  &f->fmt.pix.quantization);
1298 
1299 	return pxp_try_fmt(f, fmt);
1300 }
1301 
1302 static int pxp_try_fmt_vid_out(struct file *file, void *priv,
1303 			       struct v4l2_format *f)
1304 {
1305 	struct pxp_fmt *fmt;
1306 	struct pxp_ctx *ctx = file2ctx(file);
1307 
1308 	fmt = find_format(f->fmt.pix.pixelformat);
1309 	if (!fmt) {
1310 		f->fmt.pix.pixelformat = formats[0].fourcc;
1311 		fmt = find_format(f->fmt.pix.pixelformat);
1312 	}
1313 	if (!(fmt->types & MEM2MEM_OUTPUT)) {
1314 		v4l2_err(&ctx->dev->v4l2_dev,
1315 			 "Fourcc format (0x%08x) invalid.\n",
1316 			 f->fmt.pix.pixelformat);
1317 		return -EINVAL;
1318 	}
1319 
1320 	if (!f->fmt.pix.colorspace)
1321 		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
1322 
1323 	return pxp_try_fmt(f, fmt);
1324 }
1325 
1326 static int pxp_s_fmt(struct pxp_ctx *ctx, struct v4l2_format *f)
1327 {
1328 	struct pxp_q_data *q_data;
1329 	struct vb2_queue *vq;
1330 
1331 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
1332 	if (!vq)
1333 		return -EINVAL;
1334 
1335 	q_data = get_q_data(ctx, f->type);
1336 	if (!q_data)
1337 		return -EINVAL;
1338 
1339 	if (vb2_is_busy(vq)) {
1340 		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
1341 		return -EBUSY;
1342 	}
1343 
1344 	q_data->fmt		= find_format(f->fmt.pix.pixelformat);
1345 	q_data->width		= f->fmt.pix.width;
1346 	q_data->height		= f->fmt.pix.height;
1347 	q_data->bytesperline	= f->fmt.pix.bytesperline;
1348 	q_data->sizeimage	= f->fmt.pix.sizeimage;
1349 
1350 	dprintk(ctx->dev,
1351 		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
1352 		f->type, q_data->width, q_data->height, q_data->fmt->fourcc);
1353 
1354 	return 0;
1355 }
1356 
1357 static int pxp_s_fmt_vid_cap(struct file *file, void *priv,
1358 			     struct v4l2_format *f)
1359 {
1360 	struct pxp_ctx *ctx = file2ctx(file);
1361 	int ret;
1362 
1363 	ret = pxp_try_fmt_vid_cap(file, priv, f);
1364 	if (ret)
1365 		return ret;
1366 
1367 	ret = pxp_s_fmt(file2ctx(file), f);
1368 	if (ret)
1369 		return ret;
1370 
1371 	ctx->q_data[V4L2_M2M_DST].ycbcr_enc = f->fmt.pix.ycbcr_enc;
1372 	ctx->q_data[V4L2_M2M_DST].quant = f->fmt.pix.quantization;
1373 
1374 	return 0;
1375 }
1376 
1377 static int pxp_s_fmt_vid_out(struct file *file, void *priv,
1378 			     struct v4l2_format *f)
1379 {
1380 	struct pxp_ctx *ctx = file2ctx(file);
1381 	int ret;
1382 
1383 	ret = pxp_try_fmt_vid_out(file, priv, f);
1384 	if (ret)
1385 		return ret;
1386 
1387 	ret = pxp_s_fmt(file2ctx(file), f);
1388 	if (ret)
1389 		return ret;
1390 
1391 	ctx->colorspace = f->fmt.pix.colorspace;
1392 	ctx->xfer_func = f->fmt.pix.xfer_func;
1393 	ctx->q_data[V4L2_M2M_SRC].ycbcr_enc = f->fmt.pix.ycbcr_enc;
1394 	ctx->q_data[V4L2_M2M_SRC].quant = f->fmt.pix.quantization;
1395 
1396 	pxp_fixup_colorimetry_cap(ctx, ctx->q_data[V4L2_M2M_DST].fmt->fourcc,
1397 				  &ctx->q_data[V4L2_M2M_DST].ycbcr_enc,
1398 				  &ctx->q_data[V4L2_M2M_DST].quant);
1399 
1400 	return 0;
1401 }
1402 
1403 static int pxp_enum_framesizes(struct file *file, void *fh,
1404 			       struct v4l2_frmsizeenum *fsize)
1405 {
1406 	if (fsize->index > 0)
1407 		return -EINVAL;
1408 
1409 	if (!find_format(fsize->pixel_format))
1410 		return -EINVAL;
1411 
1412 	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
1413 	fsize->stepwise.min_width = MIN_W;
1414 	fsize->stepwise.max_width = MAX_W;
1415 	fsize->stepwise.step_width = 1 << ALIGN_W;
1416 	fsize->stepwise.min_height = MIN_H;
1417 	fsize->stepwise.max_height = MAX_H;
1418 	fsize->stepwise.step_height = 1 << ALIGN_H;
1419 
1420 	return 0;
1421 }
1422 
1423 static u8 pxp_degrees_to_rot_mode(u32 degrees)
1424 {
1425 	switch (degrees) {
1426 	case 90:
1427 		return BV_PXP_CTRL_ROTATE0__ROT_90;
1428 	case 180:
1429 		return BV_PXP_CTRL_ROTATE0__ROT_180;
1430 	case 270:
1431 		return BV_PXP_CTRL_ROTATE0__ROT_270;
1432 	case 0:
1433 	default:
1434 		return BV_PXP_CTRL_ROTATE0__ROT_0;
1435 	}
1436 }
1437 
1438 static int pxp_s_ctrl(struct v4l2_ctrl *ctrl)
1439 {
1440 	struct pxp_ctx *ctx =
1441 		container_of(ctrl->handler, struct pxp_ctx, hdl);
1442 
1443 	switch (ctrl->id) {
1444 	case V4L2_CID_HFLIP:
1445 		if (ctrl->val)
1446 			ctx->mode |= MEM2MEM_HFLIP;
1447 		else
1448 			ctx->mode &= ~MEM2MEM_HFLIP;
1449 		break;
1450 
1451 	case V4L2_CID_VFLIP:
1452 		if (ctrl->val)
1453 			ctx->mode |= MEM2MEM_VFLIP;
1454 		else
1455 			ctx->mode &= ~MEM2MEM_VFLIP;
1456 		break;
1457 
1458 	case V4L2_CID_ROTATE:
1459 		ctx->rotation = pxp_degrees_to_rot_mode(ctrl->val);
1460 		break;
1461 
1462 	case V4L2_CID_ALPHA_COMPONENT:
1463 		ctx->alpha_component = ctrl->val;
1464 		break;
1465 
1466 	default:
1467 		v4l2_err(&ctx->dev->v4l2_dev, "Invalid control\n");
1468 		return -EINVAL;
1469 	}
1470 
1471 	return 0;
1472 }
1473 
1474 static const struct v4l2_ctrl_ops pxp_ctrl_ops = {
1475 	.s_ctrl = pxp_s_ctrl,
1476 };
1477 
1478 static const struct v4l2_ioctl_ops pxp_ioctl_ops = {
1479 	.vidioc_querycap	= pxp_querycap,
1480 
1481 	.vidioc_enum_fmt_vid_cap = pxp_enum_fmt_vid_cap,
1482 	.vidioc_g_fmt_vid_cap	= pxp_g_fmt_vid_cap,
1483 	.vidioc_try_fmt_vid_cap	= pxp_try_fmt_vid_cap,
1484 	.vidioc_s_fmt_vid_cap	= pxp_s_fmt_vid_cap,
1485 
1486 	.vidioc_enum_fmt_vid_out = pxp_enum_fmt_vid_out,
1487 	.vidioc_g_fmt_vid_out	= pxp_g_fmt_vid_out,
1488 	.vidioc_try_fmt_vid_out	= pxp_try_fmt_vid_out,
1489 	.vidioc_s_fmt_vid_out	= pxp_s_fmt_vid_out,
1490 
1491 	.vidioc_enum_framesizes	= pxp_enum_framesizes,
1492 
1493 	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
1494 	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
1495 	.vidioc_qbuf		= v4l2_m2m_ioctl_qbuf,
1496 	.vidioc_dqbuf		= v4l2_m2m_ioctl_dqbuf,
1497 	.vidioc_prepare_buf	= v4l2_m2m_ioctl_prepare_buf,
1498 	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
1499 	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
1500 
1501 	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
1502 	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
1503 
1504 	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
1505 	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
1506 };
1507 
1508 /*
1509  * Queue operations
1510  */
1511 static int pxp_queue_setup(struct vb2_queue *vq,
1512 			   unsigned int *nbuffers, unsigned int *nplanes,
1513 			   unsigned int sizes[], struct device *alloc_devs[])
1514 {
1515 	struct pxp_ctx *ctx = vb2_get_drv_priv(vq);
1516 	struct pxp_q_data *q_data;
1517 	unsigned int size, count = *nbuffers;
1518 
1519 	q_data = get_q_data(ctx, vq->type);
1520 
1521 	size = q_data->sizeimage;
1522 
1523 	*nbuffers = count;
1524 
1525 	if (*nplanes)
1526 		return sizes[0] < size ? -EINVAL : 0;
1527 
1528 	*nplanes = 1;
1529 	sizes[0] = size;
1530 
1531 	dprintk(ctx->dev, "get %d buffer(s) of size %d each.\n", count, size);
1532 
1533 	return 0;
1534 }
1535 
1536 static int pxp_buf_prepare(struct vb2_buffer *vb)
1537 {
1538 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1539 	struct pxp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1540 	struct pxp_dev *dev = ctx->dev;
1541 	struct pxp_q_data *q_data;
1542 
1543 	dprintk(ctx->dev, "type: %d\n", vb->vb2_queue->type);
1544 
1545 	q_data = get_q_data(ctx, vb->vb2_queue->type);
1546 	if (V4L2_TYPE_IS_OUTPUT(vb->vb2_queue->type)) {
1547 		if (vbuf->field == V4L2_FIELD_ANY)
1548 			vbuf->field = V4L2_FIELD_NONE;
1549 		if (vbuf->field != V4L2_FIELD_NONE) {
1550 			dprintk(dev, "%s field isn't supported\n", __func__);
1551 			return -EINVAL;
1552 		}
1553 	}
1554 
1555 	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
1556 		dprintk(dev, "%s data will not fit into plane (%lu < %lu)\n",
1557 			__func__, vb2_plane_size(vb, 0),
1558 			(long)q_data->sizeimage);
1559 		return -EINVAL;
1560 	}
1561 
1562 	vb2_set_plane_payload(vb, 0, q_data->sizeimage);
1563 
1564 	return 0;
1565 }
1566 
1567 static void pxp_buf_queue(struct vb2_buffer *vb)
1568 {
1569 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1570 	struct pxp_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1571 
1572 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1573 }
1574 
1575 static int pxp_start_streaming(struct vb2_queue *q, unsigned int count)
1576 {
1577 	struct pxp_ctx *ctx = vb2_get_drv_priv(q);
1578 	struct pxp_q_data *q_data = get_q_data(ctx, q->type);
1579 
1580 	q_data->sequence = 0;
1581 	return 0;
1582 }
1583 
1584 static void pxp_stop_streaming(struct vb2_queue *q)
1585 {
1586 	struct pxp_ctx *ctx = vb2_get_drv_priv(q);
1587 	struct vb2_v4l2_buffer *vbuf;
1588 	unsigned long flags;
1589 
1590 	for (;;) {
1591 		if (V4L2_TYPE_IS_OUTPUT(q->type))
1592 			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1593 		else
1594 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1595 		if (vbuf == NULL)
1596 			return;
1597 		spin_lock_irqsave(&ctx->dev->irqlock, flags);
1598 		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
1599 		spin_unlock_irqrestore(&ctx->dev->irqlock, flags);
1600 	}
1601 }
1602 
1603 static const struct vb2_ops pxp_qops = {
1604 	.queue_setup	 = pxp_queue_setup,
1605 	.buf_prepare	 = pxp_buf_prepare,
1606 	.buf_queue	 = pxp_buf_queue,
1607 	.start_streaming = pxp_start_streaming,
1608 	.stop_streaming  = pxp_stop_streaming,
1609 	.wait_prepare	 = vb2_ops_wait_prepare,
1610 	.wait_finish	 = vb2_ops_wait_finish,
1611 };
1612 
1613 static int queue_init(void *priv, struct vb2_queue *src_vq,
1614 		      struct vb2_queue *dst_vq)
1615 {
1616 	struct pxp_ctx *ctx = priv;
1617 	int ret;
1618 
1619 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1620 	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
1621 	src_vq->drv_priv = ctx;
1622 	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1623 	src_vq->ops = &pxp_qops;
1624 	src_vq->mem_ops = &vb2_dma_contig_memops;
1625 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1626 	src_vq->lock = &ctx->dev->dev_mutex;
1627 	src_vq->dev = ctx->dev->v4l2_dev.dev;
1628 
1629 	ret = vb2_queue_init(src_vq);
1630 	if (ret)
1631 		return ret;
1632 
1633 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1634 	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
1635 	dst_vq->drv_priv = ctx;
1636 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1637 	dst_vq->ops = &pxp_qops;
1638 	dst_vq->mem_ops = &vb2_dma_contig_memops;
1639 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1640 	dst_vq->lock = &ctx->dev->dev_mutex;
1641 	dst_vq->dev = ctx->dev->v4l2_dev.dev;
1642 
1643 	return vb2_queue_init(dst_vq);
1644 }
1645 
1646 /*
1647  * File operations
1648  */
1649 static int pxp_open(struct file *file)
1650 {
1651 	struct pxp_dev *dev = video_drvdata(file);
1652 	struct pxp_ctx *ctx = NULL;
1653 	struct v4l2_ctrl_handler *hdl;
1654 	int rc = 0;
1655 
1656 	if (mutex_lock_interruptible(&dev->dev_mutex))
1657 		return -ERESTARTSYS;
1658 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1659 	if (!ctx) {
1660 		rc = -ENOMEM;
1661 		goto open_unlock;
1662 	}
1663 
1664 	v4l2_fh_init(&ctx->fh, video_devdata(file));
1665 	file->private_data = &ctx->fh;
1666 	ctx->dev = dev;
1667 	hdl = &ctx->hdl;
1668 	v4l2_ctrl_handler_init(hdl, 4);
1669 	v4l2_ctrl_new_std(hdl, &pxp_ctrl_ops, V4L2_CID_HFLIP, 0, 1, 1, 0);
1670 	v4l2_ctrl_new_std(hdl, &pxp_ctrl_ops, V4L2_CID_VFLIP, 0, 1, 1, 0);
1671 	v4l2_ctrl_new_std(hdl, &pxp_ctrl_ops, V4L2_CID_ROTATE, 0, 270, 90, 0);
1672 	v4l2_ctrl_new_std(hdl, &pxp_ctrl_ops, V4L2_CID_ALPHA_COMPONENT,
1673 			  0, 255, 1, 255);
1674 	if (hdl->error) {
1675 		rc = hdl->error;
1676 		v4l2_ctrl_handler_free(hdl);
1677 		kfree(ctx);
1678 		goto open_unlock;
1679 	}
1680 	ctx->fh.ctrl_handler = hdl;
1681 	v4l2_ctrl_handler_setup(hdl);
1682 
1683 	ctx->q_data[V4L2_M2M_SRC].fmt = &formats[0];
1684 	ctx->q_data[V4L2_M2M_SRC].width = 640;
1685 	ctx->q_data[V4L2_M2M_SRC].height = 480;
1686 	ctx->q_data[V4L2_M2M_SRC].bytesperline =
1687 		pxp_bytesperline(&formats[0], 640);
1688 	ctx->q_data[V4L2_M2M_SRC].sizeimage =
1689 		pxp_sizeimage(&formats[0], 640, 480);
1690 	ctx->q_data[V4L2_M2M_DST] = ctx->q_data[V4L2_M2M_SRC];
1691 	ctx->colorspace = V4L2_COLORSPACE_REC709;
1692 
1693 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx, &queue_init);
1694 
1695 	if (IS_ERR(ctx->fh.m2m_ctx)) {
1696 		rc = PTR_ERR(ctx->fh.m2m_ctx);
1697 
1698 		v4l2_ctrl_handler_free(hdl);
1699 		v4l2_fh_exit(&ctx->fh);
1700 		kfree(ctx);
1701 		goto open_unlock;
1702 	}
1703 
1704 	v4l2_fh_add(&ctx->fh);
1705 	atomic_inc(&dev->num_inst);
1706 
1707 	dprintk(dev, "Created instance: %p, m2m_ctx: %p\n",
1708 		ctx, ctx->fh.m2m_ctx);
1709 
1710 open_unlock:
1711 	mutex_unlock(&dev->dev_mutex);
1712 	return rc;
1713 }
1714 
1715 static int pxp_release(struct file *file)
1716 {
1717 	struct pxp_dev *dev = video_drvdata(file);
1718 	struct pxp_ctx *ctx = file2ctx(file);
1719 
1720 	dprintk(dev, "Releasing instance %p\n", ctx);
1721 
1722 	v4l2_fh_del(&ctx->fh);
1723 	v4l2_fh_exit(&ctx->fh);
1724 	v4l2_ctrl_handler_free(&ctx->hdl);
1725 	mutex_lock(&dev->dev_mutex);
1726 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1727 	mutex_unlock(&dev->dev_mutex);
1728 	kfree(ctx);
1729 
1730 	atomic_dec(&dev->num_inst);
1731 
1732 	return 0;
1733 }
1734 
1735 static const struct v4l2_file_operations pxp_fops = {
1736 	.owner		= THIS_MODULE,
1737 	.open		= pxp_open,
1738 	.release	= pxp_release,
1739 	.poll		= v4l2_m2m_fop_poll,
1740 	.unlocked_ioctl	= video_ioctl2,
1741 	.mmap		= v4l2_m2m_fop_mmap,
1742 };
1743 
1744 static const struct video_device pxp_videodev = {
1745 	.name		= MEM2MEM_NAME,
1746 	.vfl_dir	= VFL_DIR_M2M,
1747 	.fops		= &pxp_fops,
1748 	.device_caps	= V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING,
1749 	.ioctl_ops	= &pxp_ioctl_ops,
1750 	.minor		= -1,
1751 	.release	= video_device_release_empty,
1752 };
1753 
1754 static const struct v4l2_m2m_ops m2m_ops = {
1755 	.device_run	= pxp_device_run,
1756 	.job_ready	= pxp_job_ready,
1757 	.job_abort	= pxp_job_abort,
1758 };
1759 
1760 static int pxp_soft_reset(struct pxp_dev *dev)
1761 {
1762 	int ret;
1763 	u32 val;
1764 
1765 	pxp_write(dev, HW_PXP_CTRL_CLR, BM_PXP_CTRL_SFTRST);
1766 	pxp_write(dev, HW_PXP_CTRL_CLR, BM_PXP_CTRL_CLKGATE);
1767 
1768 	pxp_write(dev, HW_PXP_CTRL_SET, BM_PXP_CTRL_SFTRST);
1769 
1770 	ret = regmap_read_poll_timeout(dev->regmap, HW_PXP_CTRL, val,
1771 				       val & BM_PXP_CTRL_CLKGATE, 0, 100);
1772 	if (ret < 0)
1773 		return ret;
1774 
1775 	pxp_write(dev, HW_PXP_CTRL_CLR, BM_PXP_CTRL_SFTRST);
1776 	pxp_write(dev, HW_PXP_CTRL_CLR, BM_PXP_CTRL_CLKGATE);
1777 
1778 	return 0;
1779 }
1780 
1781 static int pxp_probe(struct platform_device *pdev)
1782 {
1783 	struct pxp_dev *dev;
1784 	struct video_device *vfd;
1785 	u32 hw_version;
1786 	int irq;
1787 	int ret;
1788 	void __iomem *mmio;
1789 
1790 	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
1791 	if (!dev)
1792 		return -ENOMEM;
1793 
1794 	dev->pdata = of_device_get_match_data(&pdev->dev);
1795 
1796 	dev->clk = devm_clk_get(&pdev->dev, "axi");
1797 	if (IS_ERR(dev->clk)) {
1798 		ret = PTR_ERR(dev->clk);
1799 		dev_err(&pdev->dev, "Failed to get clk: %d\n", ret);
1800 		return ret;
1801 	}
1802 
1803 	mmio = devm_platform_ioremap_resource(pdev, 0);
1804 	if (IS_ERR(mmio))
1805 		return PTR_ERR(mmio);
1806 	dev->regmap = devm_regmap_init_mmio(&pdev->dev, mmio,
1807 					    &pxp_regmap_config);
1808 
1809 	irq = platform_get_irq(pdev, 0);
1810 	if (irq < 0)
1811 		return irq;
1812 
1813 	spin_lock_init(&dev->irqlock);
1814 
1815 	ret = devm_request_irq(&pdev->dev, irq, pxp_irq_handler, 0,
1816 			       dev_name(&pdev->dev), dev);
1817 	if (ret < 0) {
1818 		dev_err(&pdev->dev, "Failed to request irq: %d\n", ret);
1819 		return ret;
1820 	}
1821 
1822 	ret = clk_prepare_enable(dev->clk);
1823 	if (ret < 0)
1824 		return ret;
1825 
1826 	ret = pxp_soft_reset(dev);
1827 	if (ret < 0) {
1828 		dev_err(&pdev->dev, "PXP reset timeout: %d\n", ret);
1829 		goto err_clk;
1830 	}
1831 
1832 	hw_version = pxp_read(dev, HW_PXP_VERSION);
1833 	dev_dbg(&pdev->dev, "PXP Version %u.%u\n",
1834 		PXP_VERSION_MAJOR(hw_version), PXP_VERSION_MINOR(hw_version));
1835 
1836 	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
1837 	if (ret)
1838 		goto err_clk;
1839 
1840 	atomic_set(&dev->num_inst, 0);
1841 	mutex_init(&dev->dev_mutex);
1842 
1843 	dev->vfd = pxp_videodev;
1844 	vfd = &dev->vfd;
1845 	vfd->lock = &dev->dev_mutex;
1846 	vfd->v4l2_dev = &dev->v4l2_dev;
1847 
1848 	video_set_drvdata(vfd, dev);
1849 	snprintf(vfd->name, sizeof(vfd->name), "%s", pxp_videodev.name);
1850 	v4l2_info(&dev->v4l2_dev,
1851 			"Device registered as /dev/video%d\n", vfd->num);
1852 
1853 	platform_set_drvdata(pdev, dev);
1854 
1855 	dev->m2m_dev = v4l2_m2m_init(&m2m_ops);
1856 	if (IS_ERR(dev->m2m_dev)) {
1857 		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
1858 		ret = PTR_ERR(dev->m2m_dev);
1859 		goto err_v4l2;
1860 	}
1861 
1862 	ret = video_register_device(vfd, VFL_TYPE_VIDEO, 0);
1863 	if (ret) {
1864 		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
1865 		goto err_m2m;
1866 	}
1867 
1868 #ifdef CONFIG_MEDIA_CONTROLLER
1869 	dev->mdev.dev = &pdev->dev;
1870 	strscpy(dev->mdev.model, MEM2MEM_NAME, sizeof(dev->mdev.model));
1871 	media_device_init(&dev->mdev);
1872 	dev->v4l2_dev.mdev = &dev->mdev;
1873 
1874 	ret = v4l2_m2m_register_media_controller(dev->m2m_dev, vfd,
1875 						 MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER);
1876 	if (ret) {
1877 		dev_err(&pdev->dev, "Failed to initialize media device\n");
1878 		goto err_vfd;
1879 	}
1880 
1881 	ret = media_device_register(&dev->mdev);
1882 	if (ret) {
1883 		dev_err(&pdev->dev, "Failed to register media device\n");
1884 		goto err_m2m_mc;
1885 	}
1886 #endif
1887 
1888 	return 0;
1889 
1890 #ifdef CONFIG_MEDIA_CONTROLLER
1891 err_m2m_mc:
1892 	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1893 err_vfd:
1894 	video_unregister_device(vfd);
1895 #endif
1896 err_m2m:
1897 	v4l2_m2m_release(dev->m2m_dev);
1898 err_v4l2:
1899 	v4l2_device_unregister(&dev->v4l2_dev);
1900 err_clk:
1901 	clk_disable_unprepare(dev->clk);
1902 
1903 	return ret;
1904 }
1905 
1906 static void pxp_remove(struct platform_device *pdev)
1907 {
1908 	struct pxp_dev *dev = platform_get_drvdata(pdev);
1909 
1910 	pxp_write(dev, HW_PXP_CTRL_SET, BM_PXP_CTRL_CLKGATE);
1911 	pxp_write(dev, HW_PXP_CTRL_SET, BM_PXP_CTRL_SFTRST);
1912 
1913 	clk_disable_unprepare(dev->clk);
1914 
1915 	v4l2_info(&dev->v4l2_dev, "Removing " MEM2MEM_NAME);
1916 
1917 #ifdef CONFIG_MEDIA_CONTROLLER
1918 	media_device_unregister(&dev->mdev);
1919 	v4l2_m2m_unregister_media_controller(dev->m2m_dev);
1920 #endif
1921 	video_unregister_device(&dev->vfd);
1922 	v4l2_m2m_release(dev->m2m_dev);
1923 	v4l2_device_unregister(&dev->v4l2_dev);
1924 }
1925 
1926 static const struct pxp_pdata pxp_imx6ull_pdata = {
1927 	.data_path_ctrl0 = pxp_imx6ull_data_path_ctrl0,
1928 };
1929 
1930 static const struct pxp_pdata pxp_imx7d_pdata = {
1931 	.data_path_ctrl0 = pxp_imx7d_data_path_ctrl0,
1932 };
1933 
1934 static const struct of_device_id pxp_dt_ids[] = {
1935 	{ .compatible = "fsl,imx6ull-pxp", .data = &pxp_imx6ull_pdata },
1936 	{ .compatible = "fsl,imx7d-pxp", .data = &pxp_imx7d_pdata },
1937 	{ },
1938 };
1939 MODULE_DEVICE_TABLE(of, pxp_dt_ids);
1940 
1941 static struct platform_driver pxp_driver = {
1942 	.probe		= pxp_probe,
1943 	.remove_new	= pxp_remove,
1944 	.driver		= {
1945 		.name	= MEM2MEM_NAME,
1946 		.of_match_table = pxp_dt_ids,
1947 	},
1948 };
1949 
1950 module_platform_driver(pxp_driver);
1951 
1952 MODULE_DESCRIPTION("i.MX PXP mem2mem scaler/CSC/rotator");
1953 MODULE_AUTHOR("Philipp Zabel <kernel@pengutronix.de>");
1954 MODULE_LICENSE("GPL");
1955