1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * V4L2 driver for the JPEG encoder/decoder from i.MX8QXP/i.MX8QM application
4  * processors.
5  *
6  * The multi-planar buffers API is used.
7  *
8  * Baseline and extended sequential jpeg decoding is supported.
9  * Progressive jpeg decoding is not supported by the IP.
10  * Supports encode and decode of various formats:
11  *     YUV444, YUV422, YUV420, BGR, ABGR, Gray
12  * YUV420 is the only multi-planar format supported.
13  * Minimum resolution is 64 x 64, maximum 8192 x 8192.
14  * To achieve 8192 x 8192, modify in defconfig: CONFIG_CMA_SIZE_MBYTES=320
15  * The alignment requirements for the resolution depend on the format,
16  * multiple of 16 resolutions should work for all formats.
17  * Special workarounds are made in the driver to support NV12 1080p.
18  * When decoding, the driver detects image resolution and pixel format
19  * from the jpeg stream, by parsing the jpeg markers.
20  *
21  * The IP has 4 slots available for context switching, but only slot 0
22  * was fully tested to work. Context switching is not used by the driver.
23  * Each driver instance (context) allocates a slot for itself, but this
24  * is postponed until device_run, to allow unlimited opens.
25  *
26  * The driver submits jobs to the IP by setting up a descriptor for the
27  * used slot, and then validating it. The encoder has an additional descriptor
28  * for the configuration phase. The driver expects FRM_DONE interrupt from
29  * IP to mark the job as finished.
30  *
31  * The decoder IP has some limitations regarding the component ID's,
32  * but the driver works around this by replacing them in the jpeg stream.
33  *
34  * A module parameter is available for debug purpose (jpeg_tracing), to enable
35  * it, enable dynamic debug for this module and:
36  * echo 1 > /sys/module/mxc_jpeg_encdec/parameters/jpeg_tracing
37  *
38  * This is inspired by the drivers/media/platform/samsung/s5p-jpeg driver
39  *
40  * Copyright 2018-2019 NXP
41  */
42 
43 #include <linux/kernel.h>
44 #include <linux/module.h>
45 #include <linux/io.h>
46 #include <linux/clk.h>
47 #include <linux/of_platform.h>
48 #include <linux/platform_device.h>
49 #include <linux/slab.h>
50 #include <linux/irqreturn.h>
51 #include <linux/interrupt.h>
52 #include <linux/pm_runtime.h>
53 #include <linux/pm_domain.h>
54 #include <linux/string.h>
55 
56 #include <media/v4l2-jpeg.h>
57 #include <media/v4l2-mem2mem.h>
58 #include <media/v4l2-ioctl.h>
59 #include <media/v4l2-common.h>
60 #include <media/v4l2-event.h>
61 #include <media/videobuf2-dma-contig.h>
62 
63 #include "mxc-jpeg-hw.h"
64 #include "mxc-jpeg.h"
65 
66 static const struct mxc_jpeg_fmt mxc_formats[] = {
67 	{
68 		.name		= "JPEG",
69 		.fourcc		= V4L2_PIX_FMT_JPEG,
70 		.subsampling	= -1,
71 		.nc		= -1,
72 		.mem_planes	= 1,
73 		.comp_planes	= 1,
74 		.flags		= MXC_JPEG_FMT_TYPE_ENC,
75 	},
76 	{
77 		.name		= "BGR", /*BGR packed format*/
78 		.fourcc		= V4L2_PIX_FMT_BGR24,
79 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
80 		.nc		= 3,
81 		.depth		= 24,
82 		.mem_planes	= 1,
83 		.comp_planes	= 1,
84 		.h_align	= 3,
85 		.v_align	= 3,
86 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
87 		.precision	= 8,
88 		.is_rgb		= 1,
89 	},
90 	{
91 		.name		= "ABGR", /* ABGR packed format */
92 		.fourcc		= V4L2_PIX_FMT_ABGR32,
93 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
94 		.nc		= 4,
95 		.depth		= 32,
96 		.mem_planes	= 1,
97 		.comp_planes	= 1,
98 		.h_align	= 3,
99 		.v_align	= 3,
100 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
101 		.precision	= 8,
102 		.is_rgb		= 1,
103 	},
104 	{
105 		.name		= "YUV420", /* 1st plane = Y, 2nd plane = UV */
106 		.fourcc		= V4L2_PIX_FMT_NV12M,
107 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
108 		.nc		= 3,
109 		.depth		= 12, /* 6 bytes (4Y + UV) for 4 pixels */
110 		.mem_planes	= 2,
111 		.comp_planes	= 2, /* 1 plane Y, 1 plane UV interleaved */
112 		.h_align	= 4,
113 		.v_align	= 4,
114 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
115 		.precision	= 8,
116 	},
117 	{
118 		.name		= "YUV420", /* 1st plane = Y, 2nd plane = UV */
119 		.fourcc		= V4L2_PIX_FMT_NV12,
120 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_420,
121 		.nc		= 3,
122 		.depth		= 12, /* 6 bytes (4Y + UV) for 4 pixels */
123 		.mem_planes	= 1,
124 		.comp_planes	= 2, /* 1 plane Y, 1 plane UV interleaved */
125 		.h_align	= 4,
126 		.v_align	= 4,
127 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
128 		.precision	= 8,
129 	},
130 	{
131 		.name		= "YUV422", /* YUYV */
132 		.fourcc		= V4L2_PIX_FMT_YUYV,
133 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_422,
134 		.nc		= 3,
135 		.depth		= 16,
136 		.mem_planes	= 1,
137 		.comp_planes	= 1,
138 		.h_align	= 4,
139 		.v_align	= 3,
140 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
141 		.precision	= 8,
142 	},
143 	{
144 		.name		= "YUV444", /* YUVYUV */
145 		.fourcc		= V4L2_PIX_FMT_YUV24,
146 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_444,
147 		.nc		= 3,
148 		.depth		= 24,
149 		.mem_planes	= 1,
150 		.comp_planes	= 1,
151 		.h_align	= 3,
152 		.v_align	= 3,
153 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
154 		.precision	= 8,
155 	},
156 	{
157 		.name		= "Gray", /* Gray (Y8/Y12) or Single Comp */
158 		.fourcc		= V4L2_PIX_FMT_GREY,
159 		.subsampling	= V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY,
160 		.nc		= 1,
161 		.depth		= 8,
162 		.mem_planes	= 1,
163 		.comp_planes	= 1,
164 		.h_align	= 3,
165 		.v_align	= 3,
166 		.flags		= MXC_JPEG_FMT_TYPE_RAW,
167 		.precision	= 8,
168 	},
169 };
170 
171 #define MXC_JPEG_NUM_FORMATS ARRAY_SIZE(mxc_formats)
172 
173 static const int mxc_decode_mode = MXC_JPEG_DECODE;
174 static const int mxc_encode_mode = MXC_JPEG_ENCODE;
175 
176 static const struct of_device_id mxc_jpeg_match[] = {
177 	{
178 		.compatible = "nxp,imx8qxp-jpgdec",
179 		.data       = &mxc_decode_mode,
180 	},
181 	{
182 		.compatible = "nxp,imx8qxp-jpgenc",
183 		.data       = &mxc_encode_mode,
184 	},
185 	{ },
186 };
187 
188 /*
189  * default configuration stream, 64x64 yuv422
190  * split by JPEG marker, so it's easier to modify & use
191  */
192 static const unsigned char jpeg_soi[] = {
193 	0xFF, 0xD8
194 };
195 
196 static const unsigned char jpeg_app0[] = {
197 	0xFF, 0xE0,
198 	0x00, 0x10, 0x4A, 0x46, 0x49, 0x46, 0x00,
199 	0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x01,
200 	0x00, 0x00
201 };
202 
203 static const unsigned char jpeg_app14[] = {
204 	0xFF, 0xEE,
205 	0x00, 0x0E, 0x41, 0x64, 0x6F, 0x62, 0x65,
206 	0x00, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00
207 };
208 
209 static const unsigned char jpeg_dqt[] = {
210 	0xFF, 0xDB,
211 	0x00, 0x84, 0x00, 0x10, 0x0B, 0x0C, 0x0E,
212 	0x0C, 0x0A, 0x10, 0x0E, 0x0D, 0x0E, 0x12,
213 	0x11, 0x10, 0x13, 0x18, 0x28, 0x1A, 0x18,
214 	0x16, 0x16, 0x18, 0x31, 0x23, 0x25, 0x1D,
215 	0x28, 0x3A, 0x33, 0x3D, 0x3C, 0x39, 0x33,
216 	0x38, 0x37, 0x40, 0x48, 0x5C, 0x4E, 0x40,
217 	0x44, 0x57, 0x45, 0x37, 0x38, 0x50, 0x6D,
218 	0x51, 0x57, 0x5F, 0x62, 0x67, 0x68, 0x67,
219 	0x3E, 0x4D, 0x71, 0x79, 0x70, 0x64, 0x78,
220 	0x5C, 0x65, 0x67, 0x63, 0x01, 0x11, 0x12,
221 	0x12, 0x18, 0x15, 0x18, 0x2F, 0x1A, 0x1A,
222 	0x2F, 0x63, 0x42, 0x38, 0x42, 0x63, 0x63,
223 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
224 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
225 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
226 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
227 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
228 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63, 0x63,
229 	0x63, 0x63, 0x63, 0x63, 0x63, 0x63
230 };
231 
232 static const unsigned char jpeg_sof_maximal[] = {
233 	0xFF, 0xC0,
234 	0x00, 0x14, 0x08, 0x00, 0x40, 0x00, 0x40,
235 	0x04, 0x01, 0x11, 0x00, 0x02, 0x11, 0x01,
236 	0x03, 0x11, 0x01, 0x04, 0x11, 0x01
237 };
238 
239 static const unsigned char jpeg_dht[] = {
240 	0xFF, 0xC4,
241 	0x01, 0xA2, 0x00, 0x00, 0x01, 0x05, 0x01,
242 	0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00,
243 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
244 	0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
245 	0x09, 0x0A, 0x0B, 0x10, 0x00, 0x02, 0x01,
246 	0x03, 0x03, 0x02, 0x04, 0x03, 0x05, 0x05,
247 	0x04, 0x04, 0x00, 0x00, 0x01, 0x7D, 0x01,
248 	0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
249 	0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61,
250 	0x07, 0x22, 0x71, 0x14, 0x32, 0x81, 0x91,
251 	0xA1, 0x08, 0x23, 0x42, 0xB1, 0xC1, 0x15,
252 	0x52, 0xD1, 0xF0, 0x24, 0x33, 0x62, 0x72,
253 	0x82, 0x09, 0x0A, 0x16, 0x17, 0x18, 0x19,
254 	0x1A, 0x25, 0x26, 0x27, 0x28, 0x29, 0x2A,
255 	0x34, 0x35, 0x36, 0x37, 0x38, 0x39, 0x3A,
256 	0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
257 	0x4A, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58,
258 	0x59, 0x5A, 0x63, 0x64, 0x65, 0x66, 0x67,
259 	0x68, 0x69, 0x6A, 0x73, 0x74, 0x75, 0x76,
260 	0x77, 0x78, 0x79, 0x7A, 0x83, 0x84, 0x85,
261 	0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93,
262 	0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A,
263 	0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8,
264 	0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
265 	0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4,
266 	0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2,
267 	0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9,
268 	0xDA, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6,
269 	0xE7, 0xE8, 0xE9, 0xEA, 0xF1, 0xF2, 0xF3,
270 	0xF4, 0xF5, 0xF6, 0xF7, 0xF8, 0xF9, 0xFA,
271 	0x01, 0x00, 0x03, 0x01, 0x01, 0x01, 0x01,
272 	0x01, 0x01, 0x01, 0x01, 0x01, 0x00, 0x00,
273 	0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03,
274 	0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0A,
275 	0x0B, 0x11, 0x00, 0x02, 0x01, 0x02, 0x04,
276 	0x04, 0x03, 0x04, 0x07, 0x05, 0x04, 0x04,
277 	0x00, 0x01, 0x02, 0x77, 0x00, 0x01, 0x02,
278 	0x03, 0x11, 0x04, 0x05, 0x21, 0x31, 0x06,
279 	0x12, 0x41, 0x51, 0x07, 0x61, 0x71, 0x13,
280 	0x22, 0x32, 0x81, 0x08, 0x14, 0x42, 0x91,
281 	0xA1, 0xB1, 0xC1, 0x09, 0x23, 0x33, 0x52,
282 	0xF0, 0x15, 0x62, 0x72, 0xD1, 0x0A, 0x16,
283 	0x24, 0x34, 0xE1, 0x25, 0xF1, 0x17, 0x18,
284 	0x19, 0x1A, 0x26, 0x27, 0x28, 0x29, 0x2A,
285 	0x35, 0x36, 0x37, 0x38, 0x39, 0x3A, 0x43,
286 	0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4A,
287 	0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
288 	0x5A, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
289 	0x69, 0x6A, 0x73, 0x74, 0x75, 0x76, 0x77,
290 	0x78, 0x79, 0x7A, 0x82, 0x83, 0x84, 0x85,
291 	0x86, 0x87, 0x88, 0x89, 0x8A, 0x92, 0x93,
292 	0x94, 0x95, 0x96, 0x97, 0x98, 0x99, 0x9A,
293 	0xA2, 0xA3, 0xA4, 0xA5, 0xA6, 0xA7, 0xA8,
294 	0xA9, 0xAA, 0xB2, 0xB3, 0xB4, 0xB5, 0xB6,
295 	0xB7, 0xB8, 0xB9, 0xBA, 0xC2, 0xC3, 0xC4,
296 	0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xD2,
297 	0xD3, 0xD4, 0xD5, 0xD6, 0xD7, 0xD8, 0xD9,
298 	0xDA, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7,
299 	0xE8, 0xE9, 0xEA, 0xF2, 0xF3, 0xF4, 0xF5,
300 	0xF6, 0xF7, 0xF8, 0xF9, 0xFA
301 };
302 
303 static const unsigned char jpeg_dri[] = {
304 	0xFF, 0xDD,
305 	0x00, 0x04, 0x00, 0x20
306 };
307 
308 static const unsigned char jpeg_sos_maximal[] = {
309 	0xFF, 0xDA,
310 	0x00, 0x0C, 0x04, 0x01, 0x00, 0x02, 0x11, 0x03,
311 	0x11, 0x04, 0x11, 0x00, 0x3F, 0x00
312 };
313 
314 static const unsigned char jpeg_image_red[] = {
315 	0xFC, 0x5F, 0xA2, 0xBF, 0xCA, 0x73, 0xFE, 0xFE,
316 	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
317 	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
318 	0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28,
319 	0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A,
320 	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0,
321 	0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00,
322 	0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02,
323 	0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00, 0x28,
324 	0xA0, 0x02, 0x8A, 0x00, 0x28, 0xA0, 0x02, 0x8A,
325 	0x00, 0x28, 0xA0, 0x02, 0x8A, 0x00
326 };
327 
328 static const unsigned char jpeg_eoi[] = {
329 	0xFF, 0xD9
330 };
331 
332 struct mxc_jpeg_src_buf {
333 	/* common v4l buffer stuff -- must be first */
334 	struct vb2_v4l2_buffer	b;
335 	struct list_head	list;
336 
337 	/* mxc-jpeg specific */
338 	bool			dht_needed;
339 	bool			jpeg_parse_error;
340 	const struct mxc_jpeg_fmt	*fmt;
341 	int			w;
342 	int			h;
343 };
344 
345 static inline struct mxc_jpeg_src_buf *vb2_to_mxc_buf(struct vb2_buffer *vb)
346 {
347 	return container_of(to_vb2_v4l2_buffer(vb),
348 			    struct mxc_jpeg_src_buf, b);
349 }
350 
351 static unsigned int debug;
352 module_param(debug, int, 0644);
353 MODULE_PARM_DESC(debug, "Debug level (0-3)");
354 
355 static unsigned int hw_timeout = 2000;
356 module_param(hw_timeout, int, 0644);
357 MODULE_PARM_DESC(hw_timeout, "MXC JPEG hw timeout, the number of milliseconds");
358 
359 static void mxc_jpeg_bytesperline(struct mxc_jpeg_q_data *q, u32 precision);
360 static void mxc_jpeg_sizeimage(struct mxc_jpeg_q_data *q);
361 
362 static void _bswap16(u16 *a)
363 {
364 	*a = ((*a & 0x00FF) << 8) | ((*a & 0xFF00) >> 8);
365 }
366 
367 static void print_mxc_buf(struct mxc_jpeg_dev *jpeg, struct vb2_buffer *buf,
368 			  unsigned long len)
369 {
370 	unsigned int plane_no;
371 	u32 dma_addr;
372 	void *vaddr;
373 	unsigned long payload;
374 
375 	if (debug < 3)
376 		return;
377 
378 	for (plane_no = 0; plane_no < buf->num_planes; plane_no++) {
379 		payload = vb2_get_plane_payload(buf, plane_no);
380 		if (len == 0)
381 			len = payload;
382 		dma_addr = vb2_dma_contig_plane_dma_addr(buf, plane_no);
383 		vaddr = vb2_plane_vaddr(buf, plane_no);
384 		v4l2_dbg(3, debug, &jpeg->v4l2_dev,
385 			 "plane %d (vaddr=%p dma_addr=%x payload=%ld):",
386 			  plane_no, vaddr, dma_addr, payload);
387 		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET, 32, 1,
388 			       vaddr, len, false);
389 	}
390 }
391 
392 static inline struct mxc_jpeg_ctx *mxc_jpeg_fh_to_ctx(struct v4l2_fh *fh)
393 {
394 	return container_of(fh, struct mxc_jpeg_ctx, fh);
395 }
396 
397 static int enum_fmt(const struct mxc_jpeg_fmt *mxc_formats, int n,
398 		    struct v4l2_fmtdesc *f, u32 type)
399 {
400 	int i, num = 0;
401 
402 	for (i = 0; i < n; ++i) {
403 		if (mxc_formats[i].flags == type) {
404 			/* index-th format of searched type found ? */
405 			if (num == f->index)
406 				break;
407 			/* Correct type but haven't reached our index yet,
408 			 * just increment per-type index
409 			 */
410 			++num;
411 		}
412 	}
413 
414 	/* Format not found */
415 	if (i >= n)
416 		return -EINVAL;
417 
418 	f->pixelformat = mxc_formats[i].fourcc;
419 
420 	return 0;
421 }
422 
423 static const struct mxc_jpeg_fmt *mxc_jpeg_find_format(struct mxc_jpeg_ctx *ctx,
424 						       u32 pixelformat)
425 {
426 	unsigned int k;
427 
428 	for (k = 0; k < MXC_JPEG_NUM_FORMATS; k++) {
429 		const struct mxc_jpeg_fmt *fmt = &mxc_formats[k];
430 
431 		if (fmt->fourcc == pixelformat)
432 			return fmt;
433 	}
434 	return NULL;
435 }
436 
437 static enum mxc_jpeg_image_format mxc_jpeg_fourcc_to_imgfmt(u32 fourcc)
438 {
439 	switch (fourcc) {
440 	case V4L2_PIX_FMT_GREY:
441 		return MXC_JPEG_GRAY;
442 	case V4L2_PIX_FMT_YUYV:
443 		return MXC_JPEG_YUV422;
444 	case V4L2_PIX_FMT_NV12:
445 	case V4L2_PIX_FMT_NV12M:
446 		return MXC_JPEG_YUV420;
447 	case V4L2_PIX_FMT_YUV24:
448 		return MXC_JPEG_YUV444;
449 	case V4L2_PIX_FMT_BGR24:
450 		return MXC_JPEG_BGR;
451 	case V4L2_PIX_FMT_ABGR32:
452 		return MXC_JPEG_ABGR;
453 	default:
454 		return MXC_JPEG_INVALID;
455 	}
456 }
457 
458 static struct mxc_jpeg_q_data *mxc_jpeg_get_q_data(struct mxc_jpeg_ctx *ctx,
459 						   enum v4l2_buf_type type)
460 {
461 	if (V4L2_TYPE_IS_OUTPUT(type))
462 		return &ctx->out_q;
463 	return &ctx->cap_q;
464 }
465 
466 static void mxc_jpeg_addrs(struct mxc_jpeg_desc *desc,
467 			   struct vb2_buffer *raw_buf,
468 			   struct vb2_buffer *jpeg_buf, int offset)
469 {
470 	int img_fmt = desc->stm_ctrl & STM_CTRL_IMAGE_FORMAT_MASK;
471 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(raw_buf->vb2_queue);
472 	struct mxc_jpeg_q_data *q_data;
473 
474 	q_data = mxc_jpeg_get_q_data(ctx, raw_buf->type);
475 	desc->buf_base0 = vb2_dma_contig_plane_dma_addr(raw_buf, 0);
476 	desc->buf_base1 = 0;
477 	if (img_fmt == STM_CTRL_IMAGE_FORMAT(MXC_JPEG_YUV420)) {
478 		if (raw_buf->num_planes == 2)
479 			desc->buf_base1 = vb2_dma_contig_plane_dma_addr(raw_buf, 1);
480 		else
481 			desc->buf_base1 = desc->buf_base0 + q_data->sizeimage[0];
482 	}
483 	desc->stm_bufbase = vb2_dma_contig_plane_dma_addr(jpeg_buf, 0) +
484 		offset;
485 }
486 
487 static void notify_eos(struct mxc_jpeg_ctx *ctx)
488 {
489 	const struct v4l2_event ev = {
490 		.type = V4L2_EVENT_EOS
491 	};
492 
493 	dev_dbg(ctx->mxc_jpeg->dev, "Notify app event EOS reached");
494 	v4l2_event_queue_fh(&ctx->fh, &ev);
495 }
496 
497 static void notify_src_chg(struct mxc_jpeg_ctx *ctx)
498 {
499 	const struct v4l2_event ev = {
500 			.type = V4L2_EVENT_SOURCE_CHANGE,
501 			.u.src_change.changes =
502 			V4L2_EVENT_SRC_CH_RESOLUTION,
503 		};
504 
505 	dev_dbg(ctx->mxc_jpeg->dev, "Notify app event SRC_CH_RESOLUTION");
506 	v4l2_event_queue_fh(&ctx->fh, &ev);
507 }
508 
509 static int mxc_get_free_slot(struct mxc_jpeg_slot_data slot_data[], int n)
510 {
511 	int free_slot = 0;
512 
513 	while (slot_data[free_slot].used && free_slot < n)
514 		free_slot++;
515 
516 	return free_slot; /* >=n when there are no more free slots */
517 }
518 
519 static bool mxc_jpeg_alloc_slot_data(struct mxc_jpeg_dev *jpeg,
520 				     unsigned int slot)
521 {
522 	struct mxc_jpeg_desc *desc;
523 	struct mxc_jpeg_desc *cfg_desc;
524 	void *cfg_stm;
525 
526 	if (jpeg->slot_data[slot].desc)
527 		goto skip_alloc; /* already allocated, reuse it */
528 
529 	/* allocate descriptor for decoding/encoding phase */
530 	desc = dma_alloc_coherent(jpeg->dev,
531 				  sizeof(struct mxc_jpeg_desc),
532 				  &jpeg->slot_data[slot].desc_handle,
533 				  GFP_ATOMIC);
534 	if (!desc)
535 		goto err;
536 	jpeg->slot_data[slot].desc = desc;
537 
538 	/* allocate descriptor for configuration phase (encoder only) */
539 	cfg_desc = dma_alloc_coherent(jpeg->dev,
540 				      sizeof(struct mxc_jpeg_desc),
541 				      &jpeg->slot_data[slot].cfg_desc_handle,
542 				      GFP_ATOMIC);
543 	if (!cfg_desc)
544 		goto err;
545 	jpeg->slot_data[slot].cfg_desc = cfg_desc;
546 
547 	/* allocate configuration stream */
548 	cfg_stm = dma_alloc_coherent(jpeg->dev,
549 				     MXC_JPEG_MAX_CFG_STREAM,
550 				     &jpeg->slot_data[slot].cfg_stream_handle,
551 				     GFP_ATOMIC);
552 	if (!cfg_stm)
553 		goto err;
554 	jpeg->slot_data[slot].cfg_stream_vaddr = cfg_stm;
555 
556 skip_alloc:
557 	jpeg->slot_data[slot].used = true;
558 
559 	return true;
560 err:
561 	dev_err(jpeg->dev, "Could not allocate descriptors for slot %d", slot);
562 
563 	return false;
564 }
565 
566 static void mxc_jpeg_free_slot_data(struct mxc_jpeg_dev *jpeg,
567 				    unsigned int slot)
568 {
569 	if (slot >= MXC_MAX_SLOTS) {
570 		dev_err(jpeg->dev, "Invalid slot %d, nothing to free.", slot);
571 		return;
572 	}
573 
574 	/* free descriptor for decoding/encoding phase */
575 	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
576 			  jpeg->slot_data[slot].desc,
577 			  jpeg->slot_data[slot].desc_handle);
578 
579 	/* free descriptor for encoder configuration phase / decoder DHT */
580 	dma_free_coherent(jpeg->dev, sizeof(struct mxc_jpeg_desc),
581 			  jpeg->slot_data[slot].cfg_desc,
582 			  jpeg->slot_data[slot].cfg_desc_handle);
583 
584 	/* free configuration stream */
585 	dma_free_coherent(jpeg->dev, MXC_JPEG_MAX_CFG_STREAM,
586 			  jpeg->slot_data[slot].cfg_stream_vaddr,
587 			  jpeg->slot_data[slot].cfg_stream_handle);
588 
589 	jpeg->slot_data[slot].used = false;
590 }
591 
592 static void mxc_jpeg_check_and_set_last_buffer(struct mxc_jpeg_ctx *ctx,
593 					       struct vb2_v4l2_buffer *src_buf,
594 					       struct vb2_v4l2_buffer *dst_buf)
595 {
596 	if (v4l2_m2m_is_last_draining_src_buf(ctx->fh.m2m_ctx, src_buf)) {
597 		dst_buf->flags |= V4L2_BUF_FLAG_LAST;
598 		v4l2_m2m_mark_stopped(ctx->fh.m2m_ctx);
599 		notify_eos(ctx);
600 		ctx->header_parsed = false;
601 	}
602 }
603 
604 static void mxc_jpeg_job_finish(struct mxc_jpeg_ctx *ctx, enum vb2_buffer_state state, bool reset)
605 {
606 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
607 	void __iomem *reg = jpeg->base_reg;
608 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
609 
610 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
611 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
612 	mxc_jpeg_check_and_set_last_buffer(ctx, src_buf, dst_buf);
613 	v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
614 	v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
615 	v4l2_m2m_buf_done(src_buf, state);
616 	v4l2_m2m_buf_done(dst_buf, state);
617 
618 	mxc_jpeg_disable_irq(reg, ctx->slot);
619 	ctx->mxc_jpeg->slot_data[ctx->slot].used = false;
620 	if (reset)
621 		mxc_jpeg_sw_reset(reg);
622 }
623 
624 static u32 mxc_jpeg_get_plane_size(struct mxc_jpeg_q_data *q_data, u32 plane_no)
625 {
626 	const struct mxc_jpeg_fmt *fmt = q_data->fmt;
627 	u32 size;
628 	int i;
629 
630 	if (plane_no >= fmt->mem_planes)
631 		return 0;
632 
633 	if (fmt->mem_planes == fmt->comp_planes)
634 		return q_data->sizeimage[plane_no];
635 
636 	if (plane_no < fmt->mem_planes - 1)
637 		return q_data->sizeimage[plane_no];
638 
639 	size = q_data->sizeimage[fmt->mem_planes - 1];
640 	for (i = fmt->mem_planes; i < fmt->comp_planes; i++)
641 		size += q_data->sizeimage[i];
642 
643 	return size;
644 }
645 
646 static irqreturn_t mxc_jpeg_dec_irq(int irq, void *priv)
647 {
648 	struct mxc_jpeg_dev *jpeg = priv;
649 	struct mxc_jpeg_ctx *ctx;
650 	void __iomem *reg = jpeg->base_reg;
651 	struct device *dev = jpeg->dev;
652 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
653 	struct mxc_jpeg_src_buf *jpeg_src_buf;
654 	enum vb2_buffer_state buf_state;
655 	u32 dec_ret, com_status;
656 	unsigned long payload;
657 	struct mxc_jpeg_q_data *q_data;
658 	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
659 	unsigned int slot;
660 
661 	spin_lock(&jpeg->hw_lock);
662 
663 	com_status = readl(reg + COM_STATUS);
664 	slot = COM_STATUS_CUR_SLOT(com_status);
665 	dev_dbg(dev, "Irq %d on slot %d.\n", irq, slot);
666 
667 	ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
668 	if (WARN_ON(!ctx))
669 		goto job_unlock;
670 
671 	if (slot != ctx->slot) {
672 		/* TODO investigate when adding multi-instance support */
673 		dev_warn(dev, "IRQ slot %d != context slot %d.\n",
674 			 slot, ctx->slot);
675 		goto job_unlock;
676 	}
677 
678 	if (!jpeg->slot_data[slot].used)
679 		goto job_unlock;
680 
681 	dec_ret = readl(reg + MXC_SLOT_OFFSET(slot, SLOT_STATUS));
682 	writel(dec_ret, reg + MXC_SLOT_OFFSET(slot, SLOT_STATUS)); /* w1c */
683 
684 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
685 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
686 	if (!dst_buf || !src_buf) {
687 		dev_err(dev, "No source or destination buffer.\n");
688 		goto job_unlock;
689 	}
690 	jpeg_src_buf = vb2_to_mxc_buf(&src_buf->vb2_buf);
691 
692 	if (dec_ret & SLOT_STATUS_ENC_CONFIG_ERR) {
693 		u32 ret = readl(reg + CAST_STATUS12);
694 
695 		dev_err(dev, "Encoder/decoder error, status=0x%08x", ret);
696 		mxc_jpeg_sw_reset(reg);
697 		buf_state = VB2_BUF_STATE_ERROR;
698 		goto buffers_done;
699 	}
700 
701 	if (!(dec_ret & SLOT_STATUS_FRMDONE))
702 		goto job_unlock;
703 
704 	if (jpeg->mode == MXC_JPEG_ENCODE &&
705 	    ctx->enc_state == MXC_JPEG_ENC_CONF) {
706 		ctx->enc_state = MXC_JPEG_ENCODING;
707 		dev_dbg(dev, "Encoder config finished. Start encoding...\n");
708 		mxc_jpeg_enc_set_quality(dev, reg, ctx->jpeg_quality);
709 		mxc_jpeg_enc_mode_go(dev, reg);
710 		goto job_unlock;
711 	}
712 	if (jpeg->mode == MXC_JPEG_DECODE && jpeg_src_buf->dht_needed) {
713 		jpeg_src_buf->dht_needed = false;
714 		dev_dbg(dev, "Decoder DHT cfg finished. Start decoding...\n");
715 		goto job_unlock;
716 	}
717 
718 	if (jpeg->mode == MXC_JPEG_ENCODE) {
719 		payload = readl(reg + MXC_SLOT_OFFSET(slot, SLOT_BUF_PTR));
720 		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload);
721 		dev_dbg(dev, "Encoding finished, payload size: %ld\n",
722 			payload);
723 	} else {
724 		q_data = mxc_jpeg_get_q_data(ctx, cap_type);
725 		payload = mxc_jpeg_get_plane_size(q_data, 0);
726 		vb2_set_plane_payload(&dst_buf->vb2_buf, 0, payload);
727 		vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
728 		if (q_data->fmt->mem_planes == 2) {
729 			payload = mxc_jpeg_get_plane_size(q_data, 1);
730 			vb2_set_plane_payload(&dst_buf->vb2_buf, 1, payload);
731 		}
732 		dev_dbg(dev, "Decoding finished, payload size: %ld + %ld\n",
733 			vb2_get_plane_payload(&dst_buf->vb2_buf, 0),
734 			vb2_get_plane_payload(&dst_buf->vb2_buf, 1));
735 	}
736 
737 	/* short preview of the results */
738 	dev_dbg(dev, "src_buf preview: ");
739 	print_mxc_buf(jpeg, &src_buf->vb2_buf, 32);
740 	dev_dbg(dev, "dst_buf preview: ");
741 	print_mxc_buf(jpeg, &dst_buf->vb2_buf, 32);
742 	buf_state = VB2_BUF_STATE_DONE;
743 
744 buffers_done:
745 	mxc_jpeg_job_finish(ctx, buf_state, false);
746 	spin_unlock(&jpeg->hw_lock);
747 	cancel_delayed_work(&ctx->task_timer);
748 	v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
749 	return IRQ_HANDLED;
750 job_unlock:
751 	spin_unlock(&jpeg->hw_lock);
752 	return IRQ_HANDLED;
753 }
754 
755 static int mxc_jpeg_fixup_sof(struct mxc_jpeg_sof *sof,
756 			      u32 fourcc,
757 			      u16 w, u16 h)
758 {
759 	int sof_length;
760 
761 	sof->precision = 8; /* TODO allow 8/12 bit precision*/
762 	sof->height = h;
763 	_bswap16(&sof->height);
764 	sof->width = w;
765 	_bswap16(&sof->width);
766 
767 	switch (fourcc) {
768 	case V4L2_PIX_FMT_NV12:
769 	case V4L2_PIX_FMT_NV12M:
770 		sof->components_no = 3;
771 		sof->comp[0].v = 0x2;
772 		sof->comp[0].h = 0x2;
773 		break;
774 	case V4L2_PIX_FMT_YUYV:
775 		sof->components_no = 3;
776 		sof->comp[0].v = 0x1;
777 		sof->comp[0].h = 0x2;
778 		break;
779 	case V4L2_PIX_FMT_YUV24:
780 	case V4L2_PIX_FMT_BGR24:
781 	default:
782 		sof->components_no = 3;
783 		break;
784 	case V4L2_PIX_FMT_ABGR32:
785 		sof->components_no = 4;
786 		break;
787 	case V4L2_PIX_FMT_GREY:
788 		sof->components_no = 1;
789 		break;
790 	}
791 	sof_length = 8 + 3 * sof->components_no;
792 	sof->length = sof_length;
793 	_bswap16(&sof->length);
794 
795 	return sof_length; /* not swaped */
796 }
797 
798 static int mxc_jpeg_fixup_sos(struct mxc_jpeg_sos *sos,
799 			      u32 fourcc)
800 {
801 	int sos_length;
802 	u8 *sof_u8 = (u8 *)sos;
803 
804 	switch (fourcc) {
805 	case V4L2_PIX_FMT_NV12:
806 	case V4L2_PIX_FMT_NV12M:
807 		sos->components_no = 3;
808 		break;
809 	case V4L2_PIX_FMT_YUYV:
810 		sos->components_no = 3;
811 		break;
812 	case V4L2_PIX_FMT_YUV24:
813 	case V4L2_PIX_FMT_BGR24:
814 	default:
815 		sos->components_no = 3;
816 		break;
817 	case V4L2_PIX_FMT_ABGR32:
818 		sos->components_no = 4;
819 		break;
820 	case V4L2_PIX_FMT_GREY:
821 		sos->components_no = 1;
822 		break;
823 	}
824 	sos_length = 6 + 2 * sos->components_no;
825 	sos->length = sos_length;
826 	_bswap16(&sos->length);
827 
828 	/* SOS ignorable bytes, not so ignorable after all */
829 	sof_u8[sos_length - 1] = 0x0;
830 	sof_u8[sos_length - 2] = 0x3f;
831 	sof_u8[sos_length - 3] = 0x0;
832 
833 	return sos_length; /* not swaped */
834 }
835 
836 static unsigned int mxc_jpeg_setup_cfg_stream(void *cfg_stream_vaddr,
837 					      u32 fourcc,
838 					      u16 w, u16 h)
839 {
840 	/*
841 	 * There is a hardware issue that first 128 bytes of configuration data
842 	 * can't be loaded correctly.
843 	 * To avoid this issue, we need to write the configuration from
844 	 * an offset which should be no less than 0x80 (128 bytes).
845 	 */
846 	unsigned int offset = 0x80;
847 	u8 *cfg = (u8 *)cfg_stream_vaddr;
848 	struct mxc_jpeg_sof *sof;
849 	struct mxc_jpeg_sos *sos;
850 
851 	memcpy(cfg + offset, jpeg_soi, ARRAY_SIZE(jpeg_soi));
852 	offset += ARRAY_SIZE(jpeg_soi);
853 
854 	if (fourcc == V4L2_PIX_FMT_BGR24 ||
855 	    fourcc == V4L2_PIX_FMT_ABGR32) {
856 		memcpy(cfg + offset, jpeg_app14, sizeof(jpeg_app14));
857 		offset += sizeof(jpeg_app14);
858 	} else {
859 		memcpy(cfg + offset, jpeg_app0, sizeof(jpeg_app0));
860 		offset += sizeof(jpeg_app0);
861 	}
862 
863 	memcpy(cfg + offset, jpeg_dqt, sizeof(jpeg_dqt));
864 	offset += sizeof(jpeg_dqt);
865 
866 	memcpy(cfg + offset, jpeg_sof_maximal, sizeof(jpeg_sof_maximal));
867 	offset += 2; /* skip marker ID */
868 	sof = (struct mxc_jpeg_sof *)(cfg + offset);
869 	offset += mxc_jpeg_fixup_sof(sof, fourcc, w, h);
870 
871 	memcpy(cfg + offset, jpeg_dht, sizeof(jpeg_dht));
872 	offset += sizeof(jpeg_dht);
873 
874 	memcpy(cfg + offset, jpeg_dri, sizeof(jpeg_dri));
875 	offset += sizeof(jpeg_dri);
876 
877 	memcpy(cfg + offset, jpeg_sos_maximal, sizeof(jpeg_sos_maximal));
878 	offset += 2; /* skip marker ID */
879 	sos = (struct mxc_jpeg_sos *)(cfg + offset);
880 	offset += mxc_jpeg_fixup_sos(sos, fourcc);
881 
882 	memcpy(cfg + offset, jpeg_image_red, sizeof(jpeg_image_red));
883 	offset += sizeof(jpeg_image_red);
884 
885 	memcpy(cfg + offset, jpeg_eoi, sizeof(jpeg_eoi));
886 	offset += sizeof(jpeg_eoi);
887 
888 	return offset;
889 }
890 
891 static void mxc_jpeg_config_dec_desc(struct vb2_buffer *out_buf,
892 				     struct mxc_jpeg_ctx *ctx,
893 				     struct vb2_buffer *src_buf,
894 				     struct vb2_buffer *dst_buf)
895 {
896 	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
897 	struct mxc_jpeg_q_data *q_data_cap;
898 	enum mxc_jpeg_image_format img_fmt;
899 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
900 	void __iomem *reg = jpeg->base_reg;
901 	unsigned int slot = ctx->slot;
902 	struct mxc_jpeg_desc *desc = jpeg->slot_data[slot].desc;
903 	struct mxc_jpeg_desc *cfg_desc = jpeg->slot_data[slot].cfg_desc;
904 	dma_addr_t desc_handle = jpeg->slot_data[slot].desc_handle;
905 	dma_addr_t cfg_desc_handle = jpeg->slot_data[slot].cfg_desc_handle;
906 	dma_addr_t cfg_stream_handle = jpeg->slot_data[slot].cfg_stream_handle;
907 	unsigned int *cfg_size = &jpeg->slot_data[slot].cfg_stream_size;
908 	void *cfg_stream_vaddr = jpeg->slot_data[slot].cfg_stream_vaddr;
909 	struct mxc_jpeg_src_buf *jpeg_src_buf;
910 
911 	jpeg_src_buf = vb2_to_mxc_buf(src_buf);
912 
913 	/* setup the decoding descriptor */
914 	desc->next_descpt_ptr = 0; /* end of chain */
915 	q_data_cap = mxc_jpeg_get_q_data(ctx, cap_type);
916 	desc->imgsize = q_data_cap->w_adjusted << 16 | q_data_cap->h_adjusted;
917 	img_fmt = mxc_jpeg_fourcc_to_imgfmt(q_data_cap->fmt->fourcc);
918 	desc->stm_ctrl &= ~STM_CTRL_IMAGE_FORMAT(0xF); /* clear image format */
919 	desc->stm_ctrl |= STM_CTRL_IMAGE_FORMAT(img_fmt);
920 	desc->stm_ctrl |= STM_CTRL_BITBUF_PTR_CLR(1);
921 	desc->line_pitch = q_data_cap->bytesperline[0];
922 	mxc_jpeg_addrs(desc, dst_buf, src_buf, 0);
923 	mxc_jpeg_set_bufsize(desc, ALIGN(vb2_plane_size(src_buf, 0), 1024));
924 	print_descriptor_info(jpeg->dev, desc);
925 
926 	if (!jpeg_src_buf->dht_needed) {
927 		/* validate the decoding descriptor */
928 		mxc_jpeg_set_desc(desc_handle, reg, slot);
929 		return;
930 	}
931 
932 	/*
933 	 * if a default huffman table is needed, use the config descriptor to
934 	 * inject a DHT, by chaining it before the decoding descriptor
935 	 */
936 	*cfg_size = mxc_jpeg_setup_cfg_stream(cfg_stream_vaddr,
937 					      V4L2_PIX_FMT_YUYV,
938 					      MXC_JPEG_MIN_WIDTH,
939 					      MXC_JPEG_MIN_HEIGHT);
940 	cfg_desc->next_descpt_ptr = desc_handle | MXC_NXT_DESCPT_EN;
941 	cfg_desc->buf_base0 = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
942 	cfg_desc->buf_base1 = 0;
943 	cfg_desc->imgsize = MXC_JPEG_MIN_WIDTH << 16;
944 	cfg_desc->imgsize |= MXC_JPEG_MIN_HEIGHT;
945 	cfg_desc->line_pitch = MXC_JPEG_MIN_WIDTH * 2;
946 	cfg_desc->stm_ctrl = STM_CTRL_IMAGE_FORMAT(MXC_JPEG_YUV422);
947 	cfg_desc->stm_ctrl |= STM_CTRL_BITBUF_PTR_CLR(1);
948 	cfg_desc->stm_bufbase = cfg_stream_handle;
949 	cfg_desc->stm_bufsize = ALIGN(*cfg_size, 1024);
950 	print_descriptor_info(jpeg->dev, cfg_desc);
951 
952 	/* validate the configuration descriptor */
953 	mxc_jpeg_set_desc(cfg_desc_handle, reg, slot);
954 }
955 
956 static void mxc_jpeg_config_enc_desc(struct vb2_buffer *out_buf,
957 				     struct mxc_jpeg_ctx *ctx,
958 				     struct vb2_buffer *src_buf,
959 				     struct vb2_buffer *dst_buf)
960 {
961 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
962 	void __iomem *reg = jpeg->base_reg;
963 	unsigned int slot = ctx->slot;
964 	struct mxc_jpeg_desc *desc = jpeg->slot_data[slot].desc;
965 	struct mxc_jpeg_desc *cfg_desc = jpeg->slot_data[slot].cfg_desc;
966 	dma_addr_t desc_handle = jpeg->slot_data[slot].desc_handle;
967 	dma_addr_t cfg_desc_handle = jpeg->slot_data[slot].cfg_desc_handle;
968 	void *cfg_stream_vaddr = jpeg->slot_data[slot].cfg_stream_vaddr;
969 	struct mxc_jpeg_q_data *q_data;
970 	enum mxc_jpeg_image_format img_fmt;
971 	int w, h;
972 
973 	q_data = mxc_jpeg_get_q_data(ctx, src_buf->vb2_queue->type);
974 
975 	jpeg->slot_data[slot].cfg_stream_size =
976 			mxc_jpeg_setup_cfg_stream(cfg_stream_vaddr,
977 						  q_data->fmt->fourcc,
978 						  q_data->crop.width,
979 						  q_data->crop.height);
980 
981 	/* chain the config descriptor with the encoding descriptor */
982 	cfg_desc->next_descpt_ptr = desc_handle | MXC_NXT_DESCPT_EN;
983 
984 	cfg_desc->buf_base0 = jpeg->slot_data[slot].cfg_stream_handle;
985 	cfg_desc->buf_base1 = 0;
986 	cfg_desc->line_pitch = 0;
987 	cfg_desc->stm_bufbase = 0; /* no output expected */
988 	cfg_desc->stm_bufsize = 0x0;
989 	cfg_desc->imgsize = 0;
990 	cfg_desc->stm_ctrl = STM_CTRL_CONFIG_MOD(1);
991 	cfg_desc->stm_ctrl |= STM_CTRL_BITBUF_PTR_CLR(1);
992 
993 	desc->next_descpt_ptr = 0; /* end of chain */
994 
995 	/* use adjusted resolution for CAST IP job */
996 	w = q_data->crop.width;
997 	h = q_data->crop.height;
998 	v4l_bound_align_image(&w, w, MXC_JPEG_MAX_WIDTH, q_data->fmt->h_align,
999 			      &h, h, MXC_JPEG_MAX_HEIGHT, q_data->fmt->v_align, 0);
1000 	mxc_jpeg_set_res(desc, w, h);
1001 	mxc_jpeg_set_line_pitch(desc, q_data->bytesperline[0]);
1002 	mxc_jpeg_set_bufsize(desc, ALIGN(vb2_plane_size(dst_buf, 0), 1024));
1003 	img_fmt = mxc_jpeg_fourcc_to_imgfmt(q_data->fmt->fourcc);
1004 	if (img_fmt == MXC_JPEG_INVALID)
1005 		dev_err(jpeg->dev, "No valid image format detected\n");
1006 	desc->stm_ctrl = STM_CTRL_CONFIG_MOD(0) |
1007 			 STM_CTRL_IMAGE_FORMAT(img_fmt);
1008 	desc->stm_ctrl |= STM_CTRL_BITBUF_PTR_CLR(1);
1009 	mxc_jpeg_addrs(desc, src_buf, dst_buf, 0);
1010 	dev_dbg(jpeg->dev, "cfg_desc:\n");
1011 	print_descriptor_info(jpeg->dev, cfg_desc);
1012 	dev_dbg(jpeg->dev, "enc desc:\n");
1013 	print_descriptor_info(jpeg->dev, desc);
1014 	print_wrapper_info(jpeg->dev, reg);
1015 	print_cast_status(jpeg->dev, reg, MXC_JPEG_ENCODE);
1016 
1017 	/* validate the configuration descriptor */
1018 	mxc_jpeg_set_desc(cfg_desc_handle, reg, slot);
1019 }
1020 
1021 static const struct mxc_jpeg_fmt *mxc_jpeg_get_sibling_format(const struct mxc_jpeg_fmt *fmt)
1022 {
1023 	int i;
1024 
1025 	for (i = 0; i < MXC_JPEG_NUM_FORMATS; i++) {
1026 		if (mxc_formats[i].subsampling == fmt->subsampling &&
1027 		    mxc_formats[i].nc == fmt->nc &&
1028 		    mxc_formats[i].precision == fmt->precision &&
1029 		    mxc_formats[i].is_rgb == fmt->is_rgb &&
1030 		    mxc_formats[i].fourcc != fmt->fourcc)
1031 			return &mxc_formats[i];
1032 	}
1033 
1034 	return NULL;
1035 }
1036 
1037 static bool mxc_jpeg_compare_format(const struct mxc_jpeg_fmt *fmt1,
1038 				    const struct mxc_jpeg_fmt *fmt2)
1039 {
1040 	if (fmt1 == fmt2)
1041 		return true;
1042 	if (mxc_jpeg_get_sibling_format(fmt1) == fmt2)
1043 		return true;
1044 	return false;
1045 }
1046 
1047 static bool mxc_jpeg_source_change(struct mxc_jpeg_ctx *ctx,
1048 				   struct mxc_jpeg_src_buf *jpeg_src_buf)
1049 {
1050 	struct device *dev = ctx->mxc_jpeg->dev;
1051 	struct mxc_jpeg_q_data *q_data_cap;
1052 
1053 	if (!jpeg_src_buf->fmt)
1054 		return false;
1055 
1056 	q_data_cap = mxc_jpeg_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
1057 	if (mxc_jpeg_compare_format(q_data_cap->fmt, jpeg_src_buf->fmt))
1058 		jpeg_src_buf->fmt = q_data_cap->fmt;
1059 	if (q_data_cap->fmt != jpeg_src_buf->fmt ||
1060 	    q_data_cap->w != jpeg_src_buf->w ||
1061 	    q_data_cap->h != jpeg_src_buf->h) {
1062 		dev_dbg(dev, "Detected jpeg res=(%dx%d)->(%dx%d), pixfmt=%c%c%c%c\n",
1063 			q_data_cap->w, q_data_cap->h,
1064 			jpeg_src_buf->w, jpeg_src_buf->h,
1065 			(jpeg_src_buf->fmt->fourcc & 0xff),
1066 			(jpeg_src_buf->fmt->fourcc >>  8) & 0xff,
1067 			(jpeg_src_buf->fmt->fourcc >> 16) & 0xff,
1068 			(jpeg_src_buf->fmt->fourcc >> 24) & 0xff);
1069 
1070 		/*
1071 		 * set-up the capture queue with the pixelformat and resolution
1072 		 * detected from the jpeg output stream
1073 		 */
1074 		q_data_cap->w = jpeg_src_buf->w;
1075 		q_data_cap->h = jpeg_src_buf->h;
1076 		q_data_cap->fmt = jpeg_src_buf->fmt;
1077 		q_data_cap->w_adjusted = q_data_cap->w;
1078 		q_data_cap->h_adjusted = q_data_cap->h;
1079 		q_data_cap->crop.left = 0;
1080 		q_data_cap->crop.top = 0;
1081 		q_data_cap->crop.width = jpeg_src_buf->w;
1082 		q_data_cap->crop.height = jpeg_src_buf->h;
1083 
1084 		/*
1085 		 * align up the resolution for CAST IP,
1086 		 * but leave the buffer resolution unchanged
1087 		 */
1088 		v4l_bound_align_image(&q_data_cap->w_adjusted,
1089 				      q_data_cap->w_adjusted,  /* adjust up */
1090 				      MXC_JPEG_MAX_WIDTH,
1091 				      q_data_cap->fmt->h_align,
1092 				      &q_data_cap->h_adjusted,
1093 				      q_data_cap->h_adjusted, /* adjust up */
1094 				      MXC_JPEG_MAX_HEIGHT,
1095 				      q_data_cap->fmt->v_align,
1096 				      0);
1097 
1098 		/* setup bytesperline/sizeimage for capture queue */
1099 		mxc_jpeg_bytesperline(q_data_cap, jpeg_src_buf->fmt->precision);
1100 		mxc_jpeg_sizeimage(q_data_cap);
1101 		notify_src_chg(ctx);
1102 		ctx->source_change = 1;
1103 	}
1104 
1105 	return ctx->source_change ? true : false;
1106 }
1107 
1108 static int mxc_jpeg_job_ready(void *priv)
1109 {
1110 	struct mxc_jpeg_ctx *ctx = priv;
1111 
1112 	return ctx->source_change ? 0 : 1;
1113 }
1114 
1115 static void mxc_jpeg_device_run_timeout(struct work_struct *work)
1116 {
1117 	struct delayed_work *dwork = to_delayed_work(work);
1118 	struct mxc_jpeg_ctx *ctx = container_of(dwork, struct mxc_jpeg_ctx, task_timer);
1119 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
1120 	unsigned long flags;
1121 
1122 	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
1123 	if (ctx->slot < MXC_MAX_SLOTS && ctx->mxc_jpeg->slot_data[ctx->slot].used) {
1124 		dev_warn(jpeg->dev, "%s timeout, cancel it\n",
1125 			 ctx->mxc_jpeg->mode == MXC_JPEG_DECODE ? "decode" : "encode");
1126 		mxc_jpeg_job_finish(ctx, VB2_BUF_STATE_ERROR, true);
1127 		v4l2_m2m_job_finish(ctx->mxc_jpeg->m2m_dev, ctx->fh.m2m_ctx);
1128 	}
1129 	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1130 }
1131 
1132 static void mxc_jpeg_device_run(void *priv)
1133 {
1134 	struct mxc_jpeg_ctx *ctx = priv;
1135 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
1136 	void __iomem *reg = jpeg->base_reg;
1137 	struct device *dev = jpeg->dev;
1138 	struct vb2_v4l2_buffer *src_buf, *dst_buf;
1139 	unsigned long flags;
1140 	struct mxc_jpeg_q_data *q_data_cap, *q_data_out;
1141 	struct mxc_jpeg_src_buf *jpeg_src_buf;
1142 
1143 	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
1144 	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1145 	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1146 	if (!src_buf || !dst_buf) {
1147 		dev_err(dev, "Null src or dst buf\n");
1148 		goto end;
1149 	}
1150 
1151 	q_data_cap = mxc_jpeg_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
1152 	if (!q_data_cap)
1153 		goto end;
1154 	q_data_out = mxc_jpeg_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
1155 	if (!q_data_out)
1156 		goto end;
1157 	src_buf->sequence = q_data_out->sequence++;
1158 	dst_buf->sequence = q_data_cap->sequence++;
1159 
1160 	v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
1161 
1162 	jpeg_src_buf = vb2_to_mxc_buf(&src_buf->vb2_buf);
1163 	if (q_data_cap->fmt->mem_planes != dst_buf->vb2_buf.num_planes) {
1164 		dev_err(dev, "Capture format %s has %d planes, but capture buffer has %d planes\n",
1165 			q_data_cap->fmt->name, q_data_cap->fmt->mem_planes,
1166 			dst_buf->vb2_buf.num_planes);
1167 		jpeg_src_buf->jpeg_parse_error = true;
1168 	}
1169 	if (jpeg_src_buf->jpeg_parse_error) {
1170 		mxc_jpeg_check_and_set_last_buffer(ctx, src_buf, dst_buf);
1171 		v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1172 		v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1173 		v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
1174 		v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
1175 		spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1176 		v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1177 
1178 		return;
1179 	}
1180 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE) {
1181 		if (ctx->source_change || mxc_jpeg_source_change(ctx, jpeg_src_buf)) {
1182 			spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1183 			v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1184 			return;
1185 		}
1186 	}
1187 
1188 	mxc_jpeg_enable(reg);
1189 	mxc_jpeg_set_l_endian(reg, 1);
1190 
1191 	ctx->slot = mxc_get_free_slot(jpeg->slot_data, MXC_MAX_SLOTS);
1192 	if (ctx->slot >= MXC_MAX_SLOTS) {
1193 		dev_err(dev, "No more free slots\n");
1194 		goto end;
1195 	}
1196 	if (!mxc_jpeg_alloc_slot_data(jpeg, ctx->slot)) {
1197 		dev_err(dev, "Cannot allocate slot data\n");
1198 		goto end;
1199 	}
1200 
1201 	mxc_jpeg_enable_slot(reg, ctx->slot);
1202 	mxc_jpeg_enable_irq(reg, ctx->slot);
1203 
1204 	if (jpeg->mode == MXC_JPEG_ENCODE) {
1205 		dev_dbg(dev, "Encoding on slot %d\n", ctx->slot);
1206 		ctx->enc_state = MXC_JPEG_ENC_CONF;
1207 		mxc_jpeg_config_enc_desc(&dst_buf->vb2_buf, ctx,
1208 					 &src_buf->vb2_buf, &dst_buf->vb2_buf);
1209 		mxc_jpeg_enc_mode_conf(dev, reg); /* start config phase */
1210 	} else {
1211 		dev_dbg(dev, "Decoding on slot %d\n", ctx->slot);
1212 		print_mxc_buf(jpeg, &src_buf->vb2_buf, 0);
1213 		mxc_jpeg_config_dec_desc(&dst_buf->vb2_buf, ctx,
1214 					 &src_buf->vb2_buf, &dst_buf->vb2_buf);
1215 		mxc_jpeg_dec_mode_go(dev, reg);
1216 	}
1217 	schedule_delayed_work(&ctx->task_timer, msecs_to_jiffies(hw_timeout));
1218 end:
1219 	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1220 }
1221 
1222 static int mxc_jpeg_decoder_cmd(struct file *file, void *priv,
1223 				struct v4l2_decoder_cmd *cmd)
1224 {
1225 	struct v4l2_fh *fh = file->private_data;
1226 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
1227 	unsigned long flags;
1228 	int ret;
1229 
1230 	ret = v4l2_m2m_ioctl_try_decoder_cmd(file, fh, cmd);
1231 	if (ret < 0)
1232 		return ret;
1233 
1234 	if (!vb2_is_streaming(v4l2_m2m_get_src_vq(fh->m2m_ctx)))
1235 		return 0;
1236 
1237 	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
1238 	ret = v4l2_m2m_ioctl_decoder_cmd(file, priv, cmd);
1239 	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1240 	if (ret < 0)
1241 		return ret;
1242 
1243 	if (cmd->cmd == V4L2_DEC_CMD_STOP &&
1244 	    v4l2_m2m_has_stopped(fh->m2m_ctx)) {
1245 		notify_eos(ctx);
1246 		ctx->header_parsed = false;
1247 	}
1248 
1249 	if (cmd->cmd == V4L2_DEC_CMD_START &&
1250 	    v4l2_m2m_has_stopped(fh->m2m_ctx))
1251 		vb2_clear_last_buffer_dequeued(&fh->m2m_ctx->cap_q_ctx.q);
1252 	return 0;
1253 }
1254 
1255 static int mxc_jpeg_encoder_cmd(struct file *file, void *priv,
1256 				struct v4l2_encoder_cmd *cmd)
1257 {
1258 	struct v4l2_fh *fh = file->private_data;
1259 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
1260 	unsigned long flags;
1261 	int ret;
1262 
1263 	ret = v4l2_m2m_ioctl_try_encoder_cmd(file, fh, cmd);
1264 	if (ret < 0)
1265 		return ret;
1266 
1267 	if (!vb2_is_streaming(v4l2_m2m_get_src_vq(fh->m2m_ctx)) ||
1268 	    !vb2_is_streaming(v4l2_m2m_get_dst_vq(fh->m2m_ctx)))
1269 		return 0;
1270 
1271 	spin_lock_irqsave(&ctx->mxc_jpeg->hw_lock, flags);
1272 	ret = v4l2_m2m_ioctl_encoder_cmd(file, fh, cmd);
1273 	spin_unlock_irqrestore(&ctx->mxc_jpeg->hw_lock, flags);
1274 	if (ret < 0)
1275 		return 0;
1276 
1277 	if (cmd->cmd == V4L2_ENC_CMD_STOP &&
1278 	    v4l2_m2m_has_stopped(fh->m2m_ctx))
1279 		notify_eos(ctx);
1280 
1281 	if (cmd->cmd == V4L2_ENC_CMD_START &&
1282 	    v4l2_m2m_has_stopped(fh->m2m_ctx))
1283 		vb2_clear_last_buffer_dequeued(&fh->m2m_ctx->cap_q_ctx.q);
1284 
1285 	return 0;
1286 }
1287 
1288 static int mxc_jpeg_queue_setup(struct vb2_queue *q,
1289 				unsigned int *nbuffers,
1290 				unsigned int *nplanes,
1291 				unsigned int sizes[],
1292 				struct device *alloc_ctxs[])
1293 {
1294 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1295 	struct mxc_jpeg_q_data *q_data = NULL;
1296 	int i;
1297 
1298 	q_data = mxc_jpeg_get_q_data(ctx, q->type);
1299 	if (!q_data)
1300 		return -EINVAL;
1301 
1302 	/* Handle CREATE_BUFS situation - *nplanes != 0 */
1303 	if (*nplanes) {
1304 		if (*nplanes != q_data->fmt->mem_planes)
1305 			return -EINVAL;
1306 		for (i = 0; i < *nplanes; i++) {
1307 			if (sizes[i] < mxc_jpeg_get_plane_size(q_data, i))
1308 				return -EINVAL;
1309 		}
1310 		return 0;
1311 	}
1312 
1313 	/* Handle REQBUFS situation */
1314 	*nplanes = q_data->fmt->mem_planes;
1315 	for (i = 0; i < *nplanes; i++)
1316 		sizes[i] = mxc_jpeg_get_plane_size(q_data, i);
1317 
1318 	return 0;
1319 }
1320 
1321 static int mxc_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
1322 {
1323 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1324 	struct mxc_jpeg_q_data *q_data = mxc_jpeg_get_q_data(ctx, q->type);
1325 	int ret;
1326 
1327 	v4l2_m2m_update_start_streaming_state(ctx->fh.m2m_ctx, q);
1328 
1329 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE && V4L2_TYPE_IS_CAPTURE(q->type))
1330 		ctx->source_change = 0;
1331 	dev_dbg(ctx->mxc_jpeg->dev, "Start streaming ctx=%p", ctx);
1332 	q_data->sequence = 0;
1333 
1334 	ret = pm_runtime_resume_and_get(ctx->mxc_jpeg->dev);
1335 	if (ret < 0) {
1336 		dev_err(ctx->mxc_jpeg->dev, "Failed to power up jpeg\n");
1337 		return ret;
1338 	}
1339 
1340 	return 0;
1341 }
1342 
1343 static void mxc_jpeg_stop_streaming(struct vb2_queue *q)
1344 {
1345 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1346 	struct vb2_v4l2_buffer *vbuf;
1347 
1348 	dev_dbg(ctx->mxc_jpeg->dev, "Stop streaming ctx=%p", ctx);
1349 
1350 	/* Release all active buffers */
1351 	for (;;) {
1352 		if (V4L2_TYPE_IS_OUTPUT(q->type))
1353 			vbuf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1354 		else
1355 			vbuf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1356 		if (!vbuf)
1357 			break;
1358 		v4l2_m2m_buf_done(vbuf, VB2_BUF_STATE_ERROR);
1359 	}
1360 
1361 	if (V4L2_TYPE_IS_OUTPUT(q->type) || !ctx->source_change)
1362 		v4l2_m2m_update_stop_streaming_state(ctx->fh.m2m_ctx, q);
1363 	if (V4L2_TYPE_IS_OUTPUT(q->type) &&
1364 	    v4l2_m2m_has_stopped(ctx->fh.m2m_ctx)) {
1365 		notify_eos(ctx);
1366 		ctx->header_parsed = false;
1367 	}
1368 
1369 	pm_runtime_put_sync(&ctx->mxc_jpeg->pdev->dev);
1370 }
1371 
1372 static int mxc_jpeg_valid_comp_id(struct device *dev,
1373 				  struct mxc_jpeg_sof *sof,
1374 				  struct mxc_jpeg_sos *sos)
1375 {
1376 	int valid = 1;
1377 	int i;
1378 
1379 	/*
1380 	 * there's a limitation in the IP that the component IDs must be
1381 	 * between 0..4, if they are not, let's patch them
1382 	 */
1383 	for (i = 0; i < sof->components_no; i++)
1384 		if (sof->comp[i].id > MXC_JPEG_MAX_COMPONENTS) {
1385 			valid = 0;
1386 			dev_err(dev, "Component %d has invalid ID: %d",
1387 				i, sof->comp[i].id);
1388 		}
1389 	if (!valid)
1390 		/* patch all comp IDs if at least one is invalid */
1391 		for (i = 0; i < sof->components_no; i++) {
1392 			dev_warn(dev, "Component %d ID patched to: %d",
1393 				 i, i + 1);
1394 			sof->comp[i].id = i + 1;
1395 			sos->comp[i].id = i + 1;
1396 		}
1397 
1398 	return valid;
1399 }
1400 
1401 static bool mxc_jpeg_match_image_format(const struct mxc_jpeg_fmt *fmt,
1402 					const struct v4l2_jpeg_header *header)
1403 {
1404 	if (fmt->subsampling != header->frame.subsampling ||
1405 	    fmt->nc != header->frame.num_components ||
1406 	    fmt->precision != header->frame.precision)
1407 		return false;
1408 
1409 	/*
1410 	 * If the transform flag from APP14 marker is 0, images that are
1411 	 * encoded with 3 components have RGB colorspace, see Recommendation
1412 	 * ITU-T T.872 chapter 6.5.3 APP14 marker segment for colour encoding
1413 	 */
1414 	if (header->frame.subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_444) {
1415 		u8 is_rgb = header->app14_tf == V4L2_JPEG_APP14_TF_CMYK_RGB ? 1 : 0;
1416 
1417 		if (is_rgb != fmt->is_rgb)
1418 			return false;
1419 	}
1420 	return true;
1421 }
1422 
1423 static u32 mxc_jpeg_get_image_format(struct device *dev,
1424 				     const struct v4l2_jpeg_header *header)
1425 {
1426 	int i;
1427 	u32 fourcc = 0;
1428 
1429 	for (i = 0; i < MXC_JPEG_NUM_FORMATS; i++) {
1430 		if (mxc_jpeg_match_image_format(&mxc_formats[i], header)) {
1431 			fourcc = mxc_formats[i].fourcc;
1432 			break;
1433 		}
1434 	}
1435 	if (fourcc == 0) {
1436 		dev_err(dev,
1437 			"Could not identify image format nc=%d, subsampling=%d, precision=%d\n",
1438 			header->frame.num_components,
1439 			header->frame.subsampling,
1440 			header->frame.precision);
1441 		return fourcc;
1442 	}
1443 
1444 	return fourcc;
1445 }
1446 
1447 static void mxc_jpeg_bytesperline(struct mxc_jpeg_q_data *q, u32 precision)
1448 {
1449 	/* Bytes distance between the leftmost pixels in two adjacent lines */
1450 	if (q->fmt->fourcc == V4L2_PIX_FMT_JPEG) {
1451 		/* bytesperline unused for compressed formats */
1452 		q->bytesperline[0] = 0;
1453 		q->bytesperline[1] = 0;
1454 	} else if (q->fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_420) {
1455 		/* When the image format is planar the bytesperline value
1456 		 * applies to the first plane and is divided by the same factor
1457 		 * as the width field for the other planes
1458 		 */
1459 		q->bytesperline[0] = q->w_adjusted * DIV_ROUND_UP(precision, 8);
1460 		q->bytesperline[1] = q->bytesperline[0];
1461 	} else if (q->fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_422) {
1462 		q->bytesperline[0] = q->w_adjusted * DIV_ROUND_UP(precision, 8) * 2;
1463 		q->bytesperline[1] = 0;
1464 	} else if (q->fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_444) {
1465 		q->bytesperline[0] = q->w_adjusted * DIV_ROUND_UP(precision, 8) * q->fmt->nc;
1466 		q->bytesperline[1] = 0;
1467 	} else {
1468 		/* grayscale */
1469 		q->bytesperline[0] = q->w_adjusted * DIV_ROUND_UP(precision, 8);
1470 		q->bytesperline[1] = 0;
1471 	}
1472 }
1473 
1474 static void mxc_jpeg_sizeimage(struct mxc_jpeg_q_data *q)
1475 {
1476 	if (q->fmt->fourcc == V4L2_PIX_FMT_JPEG) {
1477 		/* if no sizeimage from user, assume worst jpeg compression */
1478 		if (!q->sizeimage[0])
1479 			q->sizeimage[0] = 6 * q->w * q->h;
1480 		q->sizeimage[1] = 0;
1481 
1482 		if (q->sizeimage[0] > MXC_JPEG_MAX_SIZEIMAGE)
1483 			q->sizeimage[0] = MXC_JPEG_MAX_SIZEIMAGE;
1484 
1485 		/* jpeg stream size must be multiple of 1K */
1486 		q->sizeimage[0] = ALIGN(q->sizeimage[0], 1024);
1487 	} else {
1488 		q->sizeimage[0] = q->bytesperline[0] * q->h_adjusted;
1489 		q->sizeimage[1] = 0;
1490 		if (q->fmt->subsampling == V4L2_JPEG_CHROMA_SUBSAMPLING_420)
1491 			q->sizeimage[1] = q->sizeimage[0] / 2;
1492 	}
1493 }
1494 
1495 static int mxc_jpeg_parse(struct mxc_jpeg_ctx *ctx, struct vb2_buffer *vb)
1496 {
1497 	struct device *dev = ctx->mxc_jpeg->dev;
1498 	struct mxc_jpeg_q_data *q_data_out;
1499 	struct mxc_jpeg_q_data *q_data_cap;
1500 	u32 fourcc;
1501 	struct v4l2_jpeg_header header;
1502 	struct mxc_jpeg_sof *psof = NULL;
1503 	struct mxc_jpeg_sos *psos = NULL;
1504 	struct mxc_jpeg_src_buf *jpeg_src_buf = vb2_to_mxc_buf(vb);
1505 	u8 *src_addr = (u8 *)vb2_plane_vaddr(vb, 0);
1506 	u32 size = vb2_get_plane_payload(vb, 0);
1507 	int ret;
1508 
1509 	memset(&header, 0, sizeof(header));
1510 	ret = v4l2_jpeg_parse_header((void *)src_addr, size, &header);
1511 	if (ret < 0) {
1512 		dev_err(dev, "Error parsing JPEG stream markers\n");
1513 		return ret;
1514 	}
1515 
1516 	/* if DHT marker present, no need to inject default one */
1517 	jpeg_src_buf->dht_needed = (header.num_dht == 0);
1518 
1519 	q_data_out = mxc_jpeg_get_q_data(ctx,
1520 					 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
1521 	if (q_data_out->w == 0 && q_data_out->h == 0) {
1522 		dev_warn(dev, "Invalid user resolution 0x0");
1523 		dev_warn(dev, "Keeping resolution from JPEG: %dx%d",
1524 			 header.frame.width, header.frame.height);
1525 	} else if (header.frame.width != q_data_out->w ||
1526 		   header.frame.height != q_data_out->h) {
1527 		dev_err(dev,
1528 			"Resolution mismatch: %dx%d (JPEG) versus %dx%d(user)",
1529 			header.frame.width, header.frame.height,
1530 			q_data_out->w, q_data_out->h);
1531 	}
1532 	q_data_out->w = header.frame.width;
1533 	q_data_out->h = header.frame.height;
1534 	if (header.frame.width > MXC_JPEG_MAX_WIDTH ||
1535 	    header.frame.height > MXC_JPEG_MAX_HEIGHT) {
1536 		dev_err(dev, "JPEG width or height should be <= 8192: %dx%d\n",
1537 			header.frame.width, header.frame.height);
1538 		return -EINVAL;
1539 	}
1540 	if (header.frame.width < MXC_JPEG_MIN_WIDTH ||
1541 	    header.frame.height < MXC_JPEG_MIN_HEIGHT) {
1542 		dev_err(dev, "JPEG width or height should be > 64: %dx%d\n",
1543 			header.frame.width, header.frame.height);
1544 		return -EINVAL;
1545 	}
1546 	if (header.frame.num_components > V4L2_JPEG_MAX_COMPONENTS) {
1547 		dev_err(dev, "JPEG number of components should be <=%d",
1548 			V4L2_JPEG_MAX_COMPONENTS);
1549 		return -EINVAL;
1550 	}
1551 	/* check and, if necessary, patch component IDs*/
1552 	psof = (struct mxc_jpeg_sof *)header.sof.start;
1553 	psos = (struct mxc_jpeg_sos *)header.sos.start;
1554 	if (!mxc_jpeg_valid_comp_id(dev, psof, psos))
1555 		dev_warn(dev, "JPEG component ids should be 0-3 or 1-4");
1556 
1557 	q_data_cap = mxc_jpeg_get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
1558 	if (q_data_cap->fmt && mxc_jpeg_match_image_format(q_data_cap->fmt, &header))
1559 		fourcc = q_data_cap->fmt->fourcc;
1560 	else
1561 		fourcc = mxc_jpeg_get_image_format(dev, &header);
1562 	if (fourcc == 0)
1563 		return -EINVAL;
1564 
1565 	jpeg_src_buf->fmt = mxc_jpeg_find_format(ctx, fourcc);
1566 	jpeg_src_buf->w = header.frame.width;
1567 	jpeg_src_buf->h = header.frame.height;
1568 	ctx->header_parsed = true;
1569 
1570 	if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx))
1571 		mxc_jpeg_source_change(ctx, jpeg_src_buf);
1572 
1573 	return 0;
1574 }
1575 
1576 static void mxc_jpeg_buf_queue(struct vb2_buffer *vb)
1577 {
1578 	int ret;
1579 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1580 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1581 	struct mxc_jpeg_src_buf *jpeg_src_buf;
1582 
1583 	if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type) &&
1584 	    vb2_is_streaming(vb->vb2_queue) &&
1585 	    v4l2_m2m_dst_buf_is_last(ctx->fh.m2m_ctx)) {
1586 		struct mxc_jpeg_q_data *q_data;
1587 
1588 		q_data = mxc_jpeg_get_q_data(ctx, vb->vb2_queue->type);
1589 		vbuf->field = V4L2_FIELD_NONE;
1590 		vbuf->sequence = q_data->sequence++;
1591 		v4l2_m2m_last_buffer_done(ctx->fh.m2m_ctx, vbuf);
1592 		notify_eos(ctx);
1593 		ctx->header_parsed = false;
1594 		return;
1595 	}
1596 
1597 	if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1598 		goto end;
1599 
1600 	/* for V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE */
1601 	if (ctx->mxc_jpeg->mode != MXC_JPEG_DECODE)
1602 		goto end;
1603 
1604 	jpeg_src_buf = vb2_to_mxc_buf(vb);
1605 	jpeg_src_buf->jpeg_parse_error = false;
1606 	ret = mxc_jpeg_parse(ctx, vb);
1607 	if (ret)
1608 		jpeg_src_buf->jpeg_parse_error = true;
1609 
1610 end:
1611 	v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vbuf);
1612 }
1613 
1614 static int mxc_jpeg_buf_out_validate(struct vb2_buffer *vb)
1615 {
1616 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1617 
1618 	vbuf->field = V4L2_FIELD_NONE;
1619 
1620 	return 0;
1621 }
1622 
1623 static int mxc_jpeg_buf_prepare(struct vb2_buffer *vb)
1624 {
1625 	struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb);
1626 	struct mxc_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1627 	struct mxc_jpeg_q_data *q_data = NULL;
1628 	struct device *dev = ctx->mxc_jpeg->dev;
1629 	unsigned long sizeimage;
1630 	int i;
1631 
1632 	vbuf->field = V4L2_FIELD_NONE;
1633 
1634 	q_data = mxc_jpeg_get_q_data(ctx, vb->vb2_queue->type);
1635 	if (!q_data)
1636 		return -EINVAL;
1637 	for (i = 0; i < q_data->fmt->mem_planes; i++) {
1638 		sizeimage = mxc_jpeg_get_plane_size(q_data, i);
1639 		if (vb2_plane_size(vb, i) < sizeimage) {
1640 			dev_err(dev, "plane %d too small (%lu < %lu)",
1641 				i, vb2_plane_size(vb, i), sizeimage);
1642 			return -EINVAL;
1643 		}
1644 	}
1645 	if (V4L2_TYPE_IS_CAPTURE(vb->vb2_queue->type)) {
1646 		vb2_set_plane_payload(vb, 0, 0);
1647 		vb2_set_plane_payload(vb, 1, 0);
1648 	}
1649 	return 0;
1650 }
1651 
1652 static const struct vb2_ops mxc_jpeg_qops = {
1653 	.queue_setup		= mxc_jpeg_queue_setup,
1654 	.wait_prepare		= vb2_ops_wait_prepare,
1655 	.wait_finish		= vb2_ops_wait_finish,
1656 	.buf_out_validate	= mxc_jpeg_buf_out_validate,
1657 	.buf_prepare		= mxc_jpeg_buf_prepare,
1658 	.start_streaming	= mxc_jpeg_start_streaming,
1659 	.stop_streaming		= mxc_jpeg_stop_streaming,
1660 	.buf_queue		= mxc_jpeg_buf_queue,
1661 };
1662 
1663 static int mxc_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
1664 			       struct vb2_queue *dst_vq)
1665 {
1666 	struct mxc_jpeg_ctx *ctx = priv;
1667 	int ret;
1668 
1669 	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1670 	src_vq->io_modes = VB2_MMAP | VB2_DMABUF;
1671 	src_vq->drv_priv = ctx;
1672 	src_vq->buf_struct_size = sizeof(struct mxc_jpeg_src_buf);
1673 	src_vq->ops = &mxc_jpeg_qops;
1674 	src_vq->mem_ops = &vb2_dma_contig_memops;
1675 	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1676 	src_vq->lock = &ctx->mxc_jpeg->lock;
1677 	src_vq->dev = ctx->mxc_jpeg->dev;
1678 
1679 	ret = vb2_queue_init(src_vq);
1680 	if (ret)
1681 		return ret;
1682 
1683 	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1684 	dst_vq->io_modes = VB2_MMAP | VB2_DMABUF;
1685 	dst_vq->drv_priv = ctx;
1686 	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1687 	dst_vq->ops = &mxc_jpeg_qops;
1688 	dst_vq->mem_ops = &vb2_dma_contig_memops;
1689 	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1690 	dst_vq->lock = &ctx->mxc_jpeg->lock;
1691 	dst_vq->dev = ctx->mxc_jpeg->dev;
1692 
1693 	ret = vb2_queue_init(dst_vq);
1694 	return ret;
1695 }
1696 
1697 static void mxc_jpeg_set_default_params(struct mxc_jpeg_ctx *ctx)
1698 {
1699 	struct mxc_jpeg_q_data *out_q = &ctx->out_q;
1700 	struct mxc_jpeg_q_data *cap_q = &ctx->cap_q;
1701 	struct mxc_jpeg_q_data *q[2] = {out_q, cap_q};
1702 	int i;
1703 
1704 	if (ctx->mxc_jpeg->mode == MXC_JPEG_ENCODE) {
1705 		out_q->fmt = mxc_jpeg_find_format(ctx, MXC_JPEG_DEFAULT_PFMT);
1706 		cap_q->fmt = mxc_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG);
1707 	} else {
1708 		out_q->fmt = mxc_jpeg_find_format(ctx, V4L2_PIX_FMT_JPEG);
1709 		cap_q->fmt = mxc_jpeg_find_format(ctx, MXC_JPEG_DEFAULT_PFMT);
1710 	}
1711 
1712 	for (i = 0; i < 2; i++) {
1713 		q[i]->w = MXC_JPEG_DEFAULT_WIDTH;
1714 		q[i]->h = MXC_JPEG_DEFAULT_HEIGHT;
1715 		q[i]->w_adjusted = MXC_JPEG_DEFAULT_WIDTH;
1716 		q[i]->h_adjusted = MXC_JPEG_DEFAULT_HEIGHT;
1717 		q[i]->crop.left = 0;
1718 		q[i]->crop.top = 0;
1719 		q[i]->crop.width = MXC_JPEG_DEFAULT_WIDTH;
1720 		q[i]->crop.height = MXC_JPEG_DEFAULT_HEIGHT;
1721 		mxc_jpeg_bytesperline(q[i], q[i]->fmt->precision);
1722 		mxc_jpeg_sizeimage(q[i]);
1723 	}
1724 }
1725 
1726 static int mxc_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
1727 {
1728 	struct mxc_jpeg_ctx *ctx =
1729 		container_of(ctrl->handler, struct mxc_jpeg_ctx, ctrl_handler);
1730 
1731 	switch (ctrl->id) {
1732 	case V4L2_CID_JPEG_COMPRESSION_QUALITY:
1733 		ctx->jpeg_quality = ctrl->val;
1734 		break;
1735 	default:
1736 		dev_err(ctx->mxc_jpeg->dev, "Invalid control, id = %d, val = %d\n",
1737 			ctrl->id, ctrl->val);
1738 		return -EINVAL;
1739 	}
1740 
1741 	return 0;
1742 }
1743 
1744 static const struct v4l2_ctrl_ops mxc_jpeg_ctrl_ops = {
1745 	.s_ctrl = mxc_jpeg_s_ctrl,
1746 };
1747 
1748 static void mxc_jpeg_encode_ctrls(struct mxc_jpeg_ctx *ctx)
1749 {
1750 	v4l2_ctrl_new_std(&ctx->ctrl_handler, &mxc_jpeg_ctrl_ops,
1751 			  V4L2_CID_JPEG_COMPRESSION_QUALITY, 1, 100, 1, 75);
1752 }
1753 
1754 static int mxc_jpeg_ctrls_setup(struct mxc_jpeg_ctx *ctx)
1755 {
1756 	int err;
1757 
1758 	v4l2_ctrl_handler_init(&ctx->ctrl_handler, 2);
1759 
1760 	if (ctx->mxc_jpeg->mode == MXC_JPEG_ENCODE)
1761 		mxc_jpeg_encode_ctrls(ctx);
1762 
1763 	if (ctx->ctrl_handler.error) {
1764 		err = ctx->ctrl_handler.error;
1765 
1766 		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
1767 		return err;
1768 	}
1769 
1770 	err = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
1771 	if (err)
1772 		v4l2_ctrl_handler_free(&ctx->ctrl_handler);
1773 	return err;
1774 }
1775 
1776 static int mxc_jpeg_open(struct file *file)
1777 {
1778 	struct mxc_jpeg_dev *mxc_jpeg = video_drvdata(file);
1779 	struct video_device *mxc_vfd = video_devdata(file);
1780 	struct device *dev = mxc_jpeg->dev;
1781 	struct mxc_jpeg_ctx *ctx;
1782 	int ret = 0;
1783 
1784 	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1785 	if (!ctx)
1786 		return -ENOMEM;
1787 
1788 	if (mutex_lock_interruptible(&mxc_jpeg->lock)) {
1789 		ret = -ERESTARTSYS;
1790 		goto free;
1791 	}
1792 
1793 	v4l2_fh_init(&ctx->fh, mxc_vfd);
1794 	file->private_data = &ctx->fh;
1795 	v4l2_fh_add(&ctx->fh);
1796 
1797 	ctx->mxc_jpeg = mxc_jpeg;
1798 
1799 	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(mxc_jpeg->m2m_dev, ctx,
1800 					    mxc_jpeg_queue_init);
1801 
1802 	if (IS_ERR(ctx->fh.m2m_ctx)) {
1803 		ret = PTR_ERR(ctx->fh.m2m_ctx);
1804 		goto error;
1805 	}
1806 
1807 	ret = mxc_jpeg_ctrls_setup(ctx);
1808 	if (ret) {
1809 		dev_err(ctx->mxc_jpeg->dev, "failed to setup mxc jpeg controls\n");
1810 		goto err_ctrls_setup;
1811 	}
1812 	ctx->fh.ctrl_handler = &ctx->ctrl_handler;
1813 	mxc_jpeg_set_default_params(ctx);
1814 	ctx->slot = MXC_MAX_SLOTS; /* slot not allocated yet */
1815 	INIT_DELAYED_WORK(&ctx->task_timer, mxc_jpeg_device_run_timeout);
1816 
1817 	if (mxc_jpeg->mode == MXC_JPEG_DECODE)
1818 		dev_dbg(dev, "Opened JPEG decoder instance %p\n", ctx);
1819 	else
1820 		dev_dbg(dev, "Opened JPEG encoder instance %p\n", ctx);
1821 	mutex_unlock(&mxc_jpeg->lock);
1822 
1823 	return 0;
1824 
1825 err_ctrls_setup:
1826 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1827 error:
1828 	v4l2_fh_del(&ctx->fh);
1829 	v4l2_fh_exit(&ctx->fh);
1830 	mutex_unlock(&mxc_jpeg->lock);
1831 free:
1832 	kfree(ctx);
1833 	return ret;
1834 }
1835 
1836 static int mxc_jpeg_querycap(struct file *file, void *priv,
1837 			     struct v4l2_capability *cap)
1838 {
1839 	strscpy(cap->driver, MXC_JPEG_NAME " codec", sizeof(cap->driver));
1840 	strscpy(cap->card, MXC_JPEG_NAME " codec", sizeof(cap->card));
1841 	cap->device_caps = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M_MPLANE;
1842 	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
1843 
1844 	return 0;
1845 }
1846 
1847 static int mxc_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
1848 				     struct v4l2_fmtdesc *f)
1849 {
1850 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
1851 	struct mxc_jpeg_q_data *q_data = mxc_jpeg_get_q_data(ctx, f->type);
1852 
1853 	if (ctx->mxc_jpeg->mode == MXC_JPEG_ENCODE) {
1854 		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
1855 			MXC_JPEG_FMT_TYPE_ENC);
1856 	} else if (!ctx->header_parsed) {
1857 		return enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f,
1858 			MXC_JPEG_FMT_TYPE_RAW);
1859 	} else {
1860 		/* For the decoder CAPTURE queue, only enumerate the raw formats
1861 		 * supported for the format currently active on OUTPUT
1862 		 * (more precisely what was propagated on capture queue
1863 		 * after jpeg parse on the output buffer)
1864 		 */
1865 		int ret = -EINVAL;
1866 		const struct mxc_jpeg_fmt *sibling;
1867 
1868 		switch (f->index) {
1869 		case 0:
1870 			f->pixelformat = q_data->fmt->fourcc;
1871 			ret = 0;
1872 			break;
1873 		case 1:
1874 			sibling = mxc_jpeg_get_sibling_format(q_data->fmt);
1875 			if (sibling) {
1876 				f->pixelformat = sibling->fourcc;
1877 				ret = 0;
1878 			}
1879 			break;
1880 		default:
1881 			break;
1882 		}
1883 		return ret;
1884 	}
1885 }
1886 
1887 static int mxc_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
1888 				     struct v4l2_fmtdesc *f)
1889 {
1890 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
1891 	u32 type = ctx->mxc_jpeg->mode == MXC_JPEG_DECODE ?  MXC_JPEG_FMT_TYPE_ENC :
1892 							     MXC_JPEG_FMT_TYPE_RAW;
1893 	int ret;
1894 
1895 	ret = enum_fmt(mxc_formats, MXC_JPEG_NUM_FORMATS, f, type);
1896 	if (ret)
1897 		return ret;
1898 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE)
1899 		f->flags = V4L2_FMT_FLAG_DYN_RESOLUTION;
1900 	return 0;
1901 }
1902 
1903 static u32 mxc_jpeg_get_fmt_type(struct mxc_jpeg_ctx *ctx, u32 type)
1904 {
1905 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE)
1906 		return V4L2_TYPE_IS_OUTPUT(type) ? MXC_JPEG_FMT_TYPE_ENC : MXC_JPEG_FMT_TYPE_RAW;
1907 	else
1908 		return V4L2_TYPE_IS_CAPTURE(type) ? MXC_JPEG_FMT_TYPE_ENC : MXC_JPEG_FMT_TYPE_RAW;
1909 }
1910 
1911 static u32 mxc_jpeg_get_default_fourcc(struct mxc_jpeg_ctx *ctx, u32 type)
1912 {
1913 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE)
1914 		return V4L2_TYPE_IS_OUTPUT(type) ? V4L2_PIX_FMT_JPEG : MXC_JPEG_DEFAULT_PFMT;
1915 	else
1916 		return V4L2_TYPE_IS_CAPTURE(type) ? V4L2_PIX_FMT_JPEG : MXC_JPEG_DEFAULT_PFMT;
1917 }
1918 
1919 static u32 mxc_jpeg_try_fourcc(struct mxc_jpeg_ctx *ctx, u32 fourcc)
1920 {
1921 	const struct mxc_jpeg_fmt *sibling;
1922 	struct mxc_jpeg_q_data *q_data_cap;
1923 
1924 	if (ctx->mxc_jpeg->mode != MXC_JPEG_DECODE)
1925 		return fourcc;
1926 	if (!ctx->header_parsed)
1927 		return fourcc;
1928 
1929 	q_data_cap = &ctx->cap_q;
1930 	if (q_data_cap->fmt->fourcc == fourcc)
1931 		return fourcc;
1932 
1933 	sibling = mxc_jpeg_get_sibling_format(q_data_cap->fmt);
1934 	if (sibling && sibling->fourcc == fourcc)
1935 		return sibling->fourcc;
1936 
1937 	return q_data_cap->fmt->fourcc;
1938 }
1939 
1940 static int mxc_jpeg_try_fmt(struct v4l2_format *f,
1941 			    struct mxc_jpeg_ctx *ctx, struct mxc_jpeg_q_data *q_data)
1942 {
1943 	const struct mxc_jpeg_fmt *fmt;
1944 	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
1945 	struct v4l2_plane_pix_format *pfmt;
1946 	u32 fourcc = f->fmt.pix_mp.pixelformat;
1947 	u32 w = (pix_mp->width < MXC_JPEG_MAX_WIDTH) ?
1948 		 pix_mp->width : MXC_JPEG_MAX_WIDTH;
1949 	u32 h = (pix_mp->height < MXC_JPEG_MAX_HEIGHT) ?
1950 		 pix_mp->height : MXC_JPEG_MAX_HEIGHT;
1951 	int i;
1952 
1953 	fmt = mxc_jpeg_find_format(ctx, fourcc);
1954 	if (!fmt || fmt->flags != mxc_jpeg_get_fmt_type(ctx, f->type)) {
1955 		dev_warn(ctx->mxc_jpeg->dev, "Format not supported: %c%c%c%c, use the default.\n",
1956 			 (fourcc & 0xff),
1957 			 (fourcc >>  8) & 0xff,
1958 			 (fourcc >> 16) & 0xff,
1959 			 (fourcc >> 24) & 0xff);
1960 		fourcc = mxc_jpeg_get_default_fourcc(ctx, f->type);
1961 		fmt = mxc_jpeg_find_format(ctx, fourcc);
1962 		if (!fmt)
1963 			return -EINVAL;
1964 		f->fmt.pix_mp.pixelformat = fourcc;
1965 	}
1966 	q_data->fmt = fmt;
1967 
1968 	memset(pix_mp->reserved, 0, sizeof(pix_mp->reserved));
1969 	pix_mp->field = V4L2_FIELD_NONE;
1970 	pix_mp->num_planes = fmt->mem_planes;
1971 	pix_mp->pixelformat = fmt->fourcc;
1972 
1973 	q_data->w = w;
1974 	q_data->h = h;
1975 	q_data->w_adjusted = w;
1976 	q_data->h_adjusted = h;
1977 	v4l_bound_align_image(&q_data->w_adjusted,
1978 			      w, /* adjust upwards*/
1979 			      MXC_JPEG_MAX_WIDTH,
1980 			      fmt->h_align,
1981 			      &q_data->h_adjusted,
1982 			      h, /* adjust upwards*/
1983 			      MXC_JPEG_MAX_HEIGHT,
1984 			      fmt->v_align,
1985 			      0);
1986 	for (i = 0; i < pix_mp->num_planes; i++) {
1987 		pfmt = &pix_mp->plane_fmt[i];
1988 		q_data->bytesperline[i] = pfmt->bytesperline;
1989 		q_data->sizeimage[i] = pfmt->sizeimage;
1990 	}
1991 
1992 	/* calculate bytesperline & sizeimage */
1993 	mxc_jpeg_bytesperline(q_data, fmt->precision);
1994 	mxc_jpeg_sizeimage(q_data);
1995 
1996 	/* adjust user format according to our calculations */
1997 	for (i = 0; i < pix_mp->num_planes; i++) {
1998 		pfmt = &pix_mp->plane_fmt[i];
1999 		memset(pfmt->reserved, 0, sizeof(pfmt->reserved));
2000 		pfmt->bytesperline = q_data->bytesperline[i];
2001 		pfmt->sizeimage = mxc_jpeg_get_plane_size(q_data, i);
2002 	}
2003 
2004 	/* fix colorspace information to sRGB for both output & capture */
2005 	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
2006 	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601;
2007 	pix_mp->xfer_func = V4L2_XFER_FUNC_SRGB;
2008 	/*
2009 	 * this hardware does not change the range of the samples
2010 	 * but since inside JPEG the YUV quantization is full-range,
2011 	 * this driver will always use full-range for the raw frames, too
2012 	 */
2013 	pix_mp->quantization = V4L2_QUANTIZATION_FULL_RANGE;
2014 
2015 	if (fmt->flags == MXC_JPEG_FMT_TYPE_RAW) {
2016 		q_data->crop.left = 0;
2017 		q_data->crop.top = 0;
2018 		q_data->crop.width = q_data->w;
2019 		q_data->crop.height = q_data->h;
2020 	}
2021 
2022 	pix_mp->width = q_data->w_adjusted;
2023 	pix_mp->height = q_data->h_adjusted;
2024 
2025 	return 0;
2026 }
2027 
2028 static int mxc_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
2029 				    struct v4l2_format *f)
2030 {
2031 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
2032 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
2033 	struct device *dev = jpeg->dev;
2034 	struct mxc_jpeg_q_data tmp_q;
2035 
2036 	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
2037 		dev_err(dev, "TRY_FMT with Invalid type: %d\n", f->type);
2038 		return -EINVAL;
2039 	}
2040 
2041 	if (ctx->mxc_jpeg->mode != MXC_JPEG_DECODE && V4L2_TYPE_IS_CAPTURE(f->type))
2042 		f->fmt.pix_mp.pixelformat = mxc_jpeg_try_fourcc(ctx, f->fmt.pix_mp.pixelformat);
2043 
2044 	return mxc_jpeg_try_fmt(f, ctx, &tmp_q);
2045 }
2046 
2047 static int mxc_jpeg_try_fmt_vid_out(struct file *file, void *priv,
2048 				    struct v4l2_format *f)
2049 {
2050 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
2051 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
2052 	struct device *dev = jpeg->dev;
2053 	struct mxc_jpeg_q_data tmp_q;
2054 
2055 	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
2056 		dev_err(dev, "TRY_FMT with Invalid type: %d\n", f->type);
2057 		return -EINVAL;
2058 	}
2059 
2060 	return mxc_jpeg_try_fmt(f, ctx, &tmp_q);
2061 }
2062 
2063 static void mxc_jpeg_s_parsed_fmt(struct mxc_jpeg_ctx *ctx, struct v4l2_format *f)
2064 {
2065 	struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
2066 	struct mxc_jpeg_q_data *q_data_cap;
2067 
2068 	if (ctx->mxc_jpeg->mode != MXC_JPEG_DECODE || !V4L2_TYPE_IS_CAPTURE(f->type))
2069 		return;
2070 	if (!ctx->header_parsed)
2071 		return;
2072 
2073 	q_data_cap = mxc_jpeg_get_q_data(ctx, f->type);
2074 	pix_mp->pixelformat = mxc_jpeg_try_fourcc(ctx, pix_mp->pixelformat);
2075 	pix_mp->width = q_data_cap->w;
2076 	pix_mp->height = q_data_cap->h;
2077 }
2078 
2079 static int mxc_jpeg_s_fmt(struct mxc_jpeg_ctx *ctx,
2080 			  struct v4l2_format *f)
2081 {
2082 	struct vb2_queue *vq;
2083 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
2084 
2085 	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
2086 	if (!vq)
2087 		return -EINVAL;
2088 
2089 	if (vb2_is_busy(vq)) {
2090 		v4l2_err(&jpeg->v4l2_dev, "queue busy\n");
2091 		return -EBUSY;
2092 	}
2093 
2094 	mxc_jpeg_s_parsed_fmt(ctx, f);
2095 
2096 	return mxc_jpeg_try_fmt(f, ctx, mxc_jpeg_get_q_data(ctx, f->type));
2097 }
2098 
2099 static int mxc_jpeg_s_fmt_vid_cap(struct file *file, void *priv,
2100 				  struct v4l2_format *f)
2101 {
2102 	return mxc_jpeg_s_fmt(mxc_jpeg_fh_to_ctx(priv), f);
2103 }
2104 
2105 static int mxc_jpeg_s_fmt_vid_out(struct file *file, void *priv,
2106 				  struct v4l2_format *f)
2107 {
2108 	int ret;
2109 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
2110 	struct vb2_queue *dst_vq;
2111 	struct mxc_jpeg_q_data *q_data_cap;
2112 	enum v4l2_buf_type cap_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
2113 	struct v4l2_format fc;
2114 
2115 	ret = mxc_jpeg_s_fmt(mxc_jpeg_fh_to_ctx(priv), f);
2116 	if (ret)
2117 		return ret;
2118 
2119 	if (ctx->mxc_jpeg->mode != MXC_JPEG_DECODE)
2120 		return 0;
2121 
2122 	dst_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, cap_type);
2123 	if (!dst_vq)
2124 		return -EINVAL;
2125 
2126 	if (vb2_is_busy(dst_vq))
2127 		return 0;
2128 
2129 	q_data_cap = mxc_jpeg_get_q_data(ctx, cap_type);
2130 	if (q_data_cap->w == f->fmt.pix_mp.width && q_data_cap->h == f->fmt.pix_mp.height)
2131 		return 0;
2132 	memset(&fc, 0, sizeof(fc));
2133 	fc.type = cap_type;
2134 	fc.fmt.pix_mp.pixelformat = q_data_cap->fmt->fourcc;
2135 	fc.fmt.pix_mp.width = f->fmt.pix_mp.width;
2136 	fc.fmt.pix_mp.height = f->fmt.pix_mp.height;
2137 
2138 	return mxc_jpeg_s_fmt_vid_cap(file, priv, &fc);
2139 }
2140 
2141 static int mxc_jpeg_g_fmt_vid(struct file *file, void *priv,
2142 			      struct v4l2_format *f)
2143 {
2144 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(priv);
2145 	struct mxc_jpeg_dev *jpeg = ctx->mxc_jpeg;
2146 	struct device *dev = jpeg->dev;
2147 	struct v4l2_pix_format_mplane   *pix_mp = &f->fmt.pix_mp;
2148 	struct mxc_jpeg_q_data *q_data = mxc_jpeg_get_q_data(ctx, f->type);
2149 	int i;
2150 
2151 	if (!V4L2_TYPE_IS_MULTIPLANAR(f->type)) {
2152 		dev_err(dev, "G_FMT with Invalid type: %d\n", f->type);
2153 		return -EINVAL;
2154 	}
2155 
2156 	pix_mp->pixelformat = q_data->fmt->fourcc;
2157 	pix_mp->width = q_data->w;
2158 	pix_mp->height = q_data->h;
2159 	pix_mp->field = V4L2_FIELD_NONE;
2160 	if (q_data->fmt->flags == MXC_JPEG_FMT_TYPE_RAW) {
2161 		pix_mp->width = q_data->w_adjusted;
2162 		pix_mp->height = q_data->h_adjusted;
2163 	}
2164 
2165 	/* fix colorspace information to sRGB for both output & capture */
2166 	pix_mp->colorspace = V4L2_COLORSPACE_SRGB;
2167 	pix_mp->ycbcr_enc = V4L2_YCBCR_ENC_601;
2168 	pix_mp->xfer_func = V4L2_XFER_FUNC_SRGB;
2169 	pix_mp->quantization = V4L2_QUANTIZATION_FULL_RANGE;
2170 
2171 	pix_mp->num_planes = q_data->fmt->mem_planes;
2172 	for (i = 0; i < pix_mp->num_planes; i++) {
2173 		pix_mp->plane_fmt[i].bytesperline = q_data->bytesperline[i];
2174 		pix_mp->plane_fmt[i].sizeimage = mxc_jpeg_get_plane_size(q_data, i);
2175 	}
2176 
2177 	return 0;
2178 }
2179 
2180 static int mxc_jpeg_dec_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
2181 {
2182 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
2183 	struct mxc_jpeg_q_data *q_data_cap;
2184 
2185 	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE && s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
2186 		return -EINVAL;
2187 
2188 	q_data_cap = mxc_jpeg_get_q_data(ctx, s->type);
2189 
2190 	switch (s->target) {
2191 	case V4L2_SEL_TGT_COMPOSE:
2192 	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
2193 		s->r = q_data_cap->crop;
2194 		break;
2195 	case V4L2_SEL_TGT_COMPOSE_PADDED:
2196 	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
2197 		s->r.left = 0;
2198 		s->r.top = 0;
2199 		s->r.width = q_data_cap->w_adjusted;
2200 		s->r.height = q_data_cap->h_adjusted;
2201 		break;
2202 	default:
2203 		return -EINVAL;
2204 	}
2205 
2206 	return 0;
2207 }
2208 
2209 static int mxc_jpeg_enc_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
2210 {
2211 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
2212 	struct mxc_jpeg_q_data *q_data_out;
2213 
2214 	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT && s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
2215 		return -EINVAL;
2216 
2217 	q_data_out = mxc_jpeg_get_q_data(ctx, s->type);
2218 
2219 	switch (s->target) {
2220 	case V4L2_SEL_TGT_CROP_DEFAULT:
2221 	case V4L2_SEL_TGT_CROP_BOUNDS:
2222 		s->r.left = 0;
2223 		s->r.top = 0;
2224 		s->r.width = q_data_out->w;
2225 		s->r.height = q_data_out->h;
2226 		break;
2227 	case V4L2_SEL_TGT_CROP:
2228 		s->r = q_data_out->crop;
2229 		break;
2230 	default:
2231 		return -EINVAL;
2232 	}
2233 
2234 	return 0;
2235 }
2236 
2237 static int mxc_jpeg_g_selection(struct file *file, void *fh, struct v4l2_selection *s)
2238 {
2239 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
2240 
2241 	if (ctx->mxc_jpeg->mode == MXC_JPEG_DECODE)
2242 		return mxc_jpeg_dec_g_selection(file, fh, s);
2243 	else
2244 		return mxc_jpeg_enc_g_selection(file, fh, s);
2245 }
2246 
2247 static int mxc_jpeg_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
2248 {
2249 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(fh);
2250 	struct mxc_jpeg_q_data *q_data_out;
2251 
2252 	if (ctx->mxc_jpeg->mode != MXC_JPEG_ENCODE)
2253 		return -ENOTTY;
2254 
2255 	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT && s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
2256 		return -EINVAL;
2257 	if (s->target != V4L2_SEL_TGT_CROP)
2258 		return -EINVAL;
2259 
2260 	q_data_out = mxc_jpeg_get_q_data(ctx, s->type);
2261 	if (s->r.left || s->r.top)
2262 		return -EINVAL;
2263 	if (s->r.width > q_data_out->w || s->r.height > q_data_out->h)
2264 		return -EINVAL;
2265 
2266 	q_data_out->crop.left = 0;
2267 	q_data_out->crop.top = 0;
2268 	q_data_out->crop.width = s->r.width;
2269 	q_data_out->crop.height = s->r.height;
2270 
2271 	return 0;
2272 }
2273 
2274 static int mxc_jpeg_subscribe_event(struct v4l2_fh *fh,
2275 				    const struct v4l2_event_subscription *sub)
2276 {
2277 	switch (sub->type) {
2278 	case V4L2_EVENT_EOS:
2279 		return v4l2_event_subscribe(fh, sub, 0, NULL);
2280 	case V4L2_EVENT_SOURCE_CHANGE:
2281 		return v4l2_src_change_event_subscribe(fh, sub);
2282 	case V4L2_EVENT_CTRL:
2283 		return v4l2_ctrl_subscribe_event(fh, sub);
2284 	default:
2285 		return -EINVAL;
2286 	}
2287 }
2288 
2289 static const struct v4l2_ioctl_ops mxc_jpeg_ioctl_ops = {
2290 	.vidioc_querycap		= mxc_jpeg_querycap,
2291 	.vidioc_enum_fmt_vid_cap	= mxc_jpeg_enum_fmt_vid_cap,
2292 	.vidioc_enum_fmt_vid_out	= mxc_jpeg_enum_fmt_vid_out,
2293 
2294 	.vidioc_try_fmt_vid_cap_mplane	= mxc_jpeg_try_fmt_vid_cap,
2295 	.vidioc_try_fmt_vid_out_mplane	= mxc_jpeg_try_fmt_vid_out,
2296 
2297 	.vidioc_s_fmt_vid_cap_mplane	= mxc_jpeg_s_fmt_vid_cap,
2298 	.vidioc_s_fmt_vid_out_mplane	= mxc_jpeg_s_fmt_vid_out,
2299 
2300 	.vidioc_g_fmt_vid_cap_mplane	= mxc_jpeg_g_fmt_vid,
2301 	.vidioc_g_fmt_vid_out_mplane	= mxc_jpeg_g_fmt_vid,
2302 
2303 	.vidioc_g_selection		= mxc_jpeg_g_selection,
2304 	.vidioc_s_selection		= mxc_jpeg_s_selection,
2305 
2306 	.vidioc_subscribe_event		= mxc_jpeg_subscribe_event,
2307 	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
2308 
2309 	.vidioc_try_decoder_cmd		= v4l2_m2m_ioctl_try_decoder_cmd,
2310 	.vidioc_decoder_cmd		= mxc_jpeg_decoder_cmd,
2311 	.vidioc_try_encoder_cmd		= v4l2_m2m_ioctl_try_encoder_cmd,
2312 	.vidioc_encoder_cmd		= mxc_jpeg_encoder_cmd,
2313 
2314 	.vidioc_qbuf			= v4l2_m2m_ioctl_qbuf,
2315 	.vidioc_dqbuf			= v4l2_m2m_ioctl_dqbuf,
2316 
2317 	.vidioc_create_bufs		= v4l2_m2m_ioctl_create_bufs,
2318 	.vidioc_prepare_buf		= v4l2_m2m_ioctl_prepare_buf,
2319 	.vidioc_reqbufs			= v4l2_m2m_ioctl_reqbufs,
2320 	.vidioc_querybuf		= v4l2_m2m_ioctl_querybuf,
2321 	.vidioc_expbuf			= v4l2_m2m_ioctl_expbuf,
2322 	.vidioc_streamon		= v4l2_m2m_ioctl_streamon,
2323 	.vidioc_streamoff		= v4l2_m2m_ioctl_streamoff,
2324 };
2325 
2326 static int mxc_jpeg_release(struct file *file)
2327 {
2328 	struct mxc_jpeg_dev *mxc_jpeg = video_drvdata(file);
2329 	struct mxc_jpeg_ctx *ctx = mxc_jpeg_fh_to_ctx(file->private_data);
2330 	struct device *dev = mxc_jpeg->dev;
2331 
2332 	mutex_lock(&mxc_jpeg->lock);
2333 	if (mxc_jpeg->mode == MXC_JPEG_DECODE)
2334 		dev_dbg(dev, "Release JPEG decoder instance on slot %d.",
2335 			ctx->slot);
2336 	else
2337 		dev_dbg(dev, "Release JPEG encoder instance on slot %d.",
2338 			ctx->slot);
2339 	v4l2_ctrl_handler_free(&ctx->ctrl_handler);
2340 	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
2341 	v4l2_fh_del(&ctx->fh);
2342 	v4l2_fh_exit(&ctx->fh);
2343 	kfree(ctx);
2344 	mutex_unlock(&mxc_jpeg->lock);
2345 
2346 	return 0;
2347 }
2348 
2349 static const struct v4l2_file_operations mxc_jpeg_fops = {
2350 	.owner		= THIS_MODULE,
2351 	.open		= mxc_jpeg_open,
2352 	.release	= mxc_jpeg_release,
2353 	.poll		= v4l2_m2m_fop_poll,
2354 	.unlocked_ioctl	= video_ioctl2,
2355 	.mmap		= v4l2_m2m_fop_mmap,
2356 };
2357 
2358 static const struct v4l2_m2m_ops mxc_jpeg_m2m_ops = {
2359 	.job_ready      = mxc_jpeg_job_ready,
2360 	.device_run	= mxc_jpeg_device_run,
2361 };
2362 
2363 static void mxc_jpeg_detach_pm_domains(struct mxc_jpeg_dev *jpeg)
2364 {
2365 	int i;
2366 
2367 	for (i = 0; i < jpeg->num_domains; i++) {
2368 		if (jpeg->pd_link[i] && !IS_ERR(jpeg->pd_link[i]))
2369 			device_link_del(jpeg->pd_link[i]);
2370 		if (jpeg->pd_dev[i] && !IS_ERR(jpeg->pd_dev[i]))
2371 			dev_pm_domain_detach(jpeg->pd_dev[i], true);
2372 		jpeg->pd_dev[i] = NULL;
2373 		jpeg->pd_link[i] = NULL;
2374 	}
2375 }
2376 
2377 static int mxc_jpeg_attach_pm_domains(struct mxc_jpeg_dev *jpeg)
2378 {
2379 	struct device *dev = jpeg->dev;
2380 	struct device_node *np = jpeg->pdev->dev.of_node;
2381 	int i;
2382 	int ret;
2383 
2384 	jpeg->num_domains = of_count_phandle_with_args(np, "power-domains",
2385 						       "#power-domain-cells");
2386 	if (jpeg->num_domains < 0) {
2387 		dev_err(dev, "No power domains defined for jpeg node\n");
2388 		return jpeg->num_domains;
2389 	}
2390 
2391 	jpeg->pd_dev = devm_kmalloc_array(dev, jpeg->num_domains,
2392 					  sizeof(*jpeg->pd_dev), GFP_KERNEL);
2393 	if (!jpeg->pd_dev)
2394 		return -ENOMEM;
2395 
2396 	jpeg->pd_link = devm_kmalloc_array(dev, jpeg->num_domains,
2397 					   sizeof(*jpeg->pd_link), GFP_KERNEL);
2398 	if (!jpeg->pd_link)
2399 		return -ENOMEM;
2400 
2401 	for (i = 0; i < jpeg->num_domains; i++) {
2402 		jpeg->pd_dev[i] = dev_pm_domain_attach_by_id(dev, i);
2403 		if (IS_ERR(jpeg->pd_dev[i])) {
2404 			ret = PTR_ERR(jpeg->pd_dev[i]);
2405 			goto fail;
2406 		}
2407 
2408 		jpeg->pd_link[i] = device_link_add(dev, jpeg->pd_dev[i],
2409 						   DL_FLAG_STATELESS |
2410 						   DL_FLAG_PM_RUNTIME);
2411 		if (!jpeg->pd_link[i]) {
2412 			ret = -EINVAL;
2413 			goto fail;
2414 		}
2415 	}
2416 
2417 	return 0;
2418 fail:
2419 	mxc_jpeg_detach_pm_domains(jpeg);
2420 	return ret;
2421 }
2422 
2423 static int mxc_jpeg_probe(struct platform_device *pdev)
2424 {
2425 	struct mxc_jpeg_dev *jpeg;
2426 	struct device *dev = &pdev->dev;
2427 	int dec_irq;
2428 	int ret;
2429 	int mode;
2430 	const struct of_device_id *of_id;
2431 	unsigned int slot;
2432 
2433 	of_id = of_match_node(mxc_jpeg_match, dev->of_node);
2434 	if (!of_id)
2435 		return -ENODEV;
2436 	mode = *(const int *)of_id->data;
2437 
2438 	jpeg = devm_kzalloc(dev, sizeof(struct mxc_jpeg_dev), GFP_KERNEL);
2439 	if (!jpeg)
2440 		return -ENOMEM;
2441 
2442 	mutex_init(&jpeg->lock);
2443 	spin_lock_init(&jpeg->hw_lock);
2444 
2445 	ret = dma_set_mask_and_coherent(dev, DMA_BIT_MASK(32));
2446 	if (ret) {
2447 		dev_err(&pdev->dev, "No suitable DMA available.\n");
2448 		goto err_irq;
2449 	}
2450 
2451 	jpeg->base_reg = devm_platform_ioremap_resource(pdev, 0);
2452 	if (IS_ERR(jpeg->base_reg))
2453 		return PTR_ERR(jpeg->base_reg);
2454 
2455 	for (slot = 0; slot < MXC_MAX_SLOTS; slot++) {
2456 		dec_irq = platform_get_irq(pdev, slot);
2457 		if (dec_irq < 0) {
2458 			ret = dec_irq;
2459 			goto err_irq;
2460 		}
2461 		ret = devm_request_irq(&pdev->dev, dec_irq, mxc_jpeg_dec_irq,
2462 				       0, pdev->name, jpeg);
2463 		if (ret) {
2464 			dev_err(&pdev->dev, "Failed to request irq %d (%d)\n",
2465 				dec_irq, ret);
2466 			goto err_irq;
2467 		}
2468 	}
2469 
2470 	jpeg->pdev = pdev;
2471 	jpeg->dev = dev;
2472 	jpeg->mode = mode;
2473 
2474 	/* Get clocks */
2475 	jpeg->clk_ipg = devm_clk_get(dev, "ipg");
2476 	if (IS_ERR(jpeg->clk_ipg)) {
2477 		dev_err(dev, "failed to get clock: ipg\n");
2478 		ret = PTR_ERR(jpeg->clk_ipg);
2479 		goto err_clk;
2480 	}
2481 
2482 	jpeg->clk_per = devm_clk_get(dev, "per");
2483 	if (IS_ERR(jpeg->clk_per)) {
2484 		dev_err(dev, "failed to get clock: per\n");
2485 		ret = PTR_ERR(jpeg->clk_per);
2486 		goto err_clk;
2487 	}
2488 
2489 	ret = mxc_jpeg_attach_pm_domains(jpeg);
2490 	if (ret < 0) {
2491 		dev_err(dev, "failed to attach power domains %d\n", ret);
2492 		return ret;
2493 	}
2494 
2495 	/* v4l2 */
2496 	ret = v4l2_device_register(dev, &jpeg->v4l2_dev);
2497 	if (ret) {
2498 		dev_err(dev, "failed to register v4l2 device\n");
2499 		goto err_register;
2500 	}
2501 	jpeg->m2m_dev = v4l2_m2m_init(&mxc_jpeg_m2m_ops);
2502 	if (IS_ERR(jpeg->m2m_dev)) {
2503 		dev_err(dev, "failed to register v4l2 device\n");
2504 		ret = PTR_ERR(jpeg->m2m_dev);
2505 		goto err_m2m;
2506 	}
2507 
2508 	jpeg->dec_vdev = video_device_alloc();
2509 	if (!jpeg->dec_vdev) {
2510 		dev_err(dev, "failed to register v4l2 device\n");
2511 		ret = -ENOMEM;
2512 		goto err_vdev_alloc;
2513 	}
2514 	if (mode == MXC_JPEG_ENCODE)
2515 		snprintf(jpeg->dec_vdev->name,
2516 			 sizeof(jpeg->dec_vdev->name),
2517 			 "%s-enc", MXC_JPEG_NAME);
2518 	else
2519 		snprintf(jpeg->dec_vdev->name,
2520 			 sizeof(jpeg->dec_vdev->name),
2521 			 "%s-dec", MXC_JPEG_NAME);
2522 
2523 	jpeg->dec_vdev->fops = &mxc_jpeg_fops;
2524 	jpeg->dec_vdev->ioctl_ops = &mxc_jpeg_ioctl_ops;
2525 	jpeg->dec_vdev->minor = -1;
2526 	jpeg->dec_vdev->release = video_device_release;
2527 	jpeg->dec_vdev->lock = &jpeg->lock; /* lock for ioctl serialization */
2528 	jpeg->dec_vdev->v4l2_dev = &jpeg->v4l2_dev;
2529 	jpeg->dec_vdev->vfl_dir = VFL_DIR_M2M;
2530 	jpeg->dec_vdev->device_caps = V4L2_CAP_STREAMING |
2531 					V4L2_CAP_VIDEO_M2M_MPLANE;
2532 	if (mode == MXC_JPEG_ENCODE) {
2533 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_DECODER_CMD);
2534 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_TRY_DECODER_CMD);
2535 	} else {
2536 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_ENCODER_CMD);
2537 		v4l2_disable_ioctl(jpeg->dec_vdev, VIDIOC_TRY_ENCODER_CMD);
2538 	}
2539 	ret = video_register_device(jpeg->dec_vdev, VFL_TYPE_VIDEO, -1);
2540 	if (ret) {
2541 		dev_err(dev, "failed to register video device\n");
2542 		goto err_vdev_register;
2543 	}
2544 	video_set_drvdata(jpeg->dec_vdev, jpeg);
2545 	if (mode == MXC_JPEG_ENCODE)
2546 		v4l2_info(&jpeg->v4l2_dev,
2547 			  "encoder device registered as /dev/video%d (%d,%d)\n",
2548 			  jpeg->dec_vdev->num, VIDEO_MAJOR,
2549 			  jpeg->dec_vdev->minor);
2550 	else
2551 		v4l2_info(&jpeg->v4l2_dev,
2552 			  "decoder device registered as /dev/video%d (%d,%d)\n",
2553 			  jpeg->dec_vdev->num, VIDEO_MAJOR,
2554 			  jpeg->dec_vdev->minor);
2555 
2556 	platform_set_drvdata(pdev, jpeg);
2557 	pm_runtime_enable(dev);
2558 
2559 	return 0;
2560 
2561 err_vdev_register:
2562 	video_device_release(jpeg->dec_vdev);
2563 
2564 err_vdev_alloc:
2565 	v4l2_m2m_release(jpeg->m2m_dev);
2566 
2567 err_m2m:
2568 	v4l2_device_unregister(&jpeg->v4l2_dev);
2569 
2570 err_register:
2571 	mxc_jpeg_detach_pm_domains(jpeg);
2572 
2573 err_irq:
2574 err_clk:
2575 	return ret;
2576 }
2577 
2578 #ifdef CONFIG_PM
2579 static int mxc_jpeg_runtime_resume(struct device *dev)
2580 {
2581 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
2582 	int ret;
2583 
2584 	ret = clk_prepare_enable(jpeg->clk_ipg);
2585 	if (ret < 0) {
2586 		dev_err(dev, "failed to enable clock: ipg\n");
2587 		goto err_ipg;
2588 	}
2589 
2590 	ret = clk_prepare_enable(jpeg->clk_per);
2591 	if (ret < 0) {
2592 		dev_err(dev, "failed to enable clock: per\n");
2593 		goto err_per;
2594 	}
2595 
2596 	return 0;
2597 
2598 err_per:
2599 	clk_disable_unprepare(jpeg->clk_ipg);
2600 err_ipg:
2601 	return ret;
2602 }
2603 
2604 static int mxc_jpeg_runtime_suspend(struct device *dev)
2605 {
2606 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
2607 
2608 	clk_disable_unprepare(jpeg->clk_ipg);
2609 	clk_disable_unprepare(jpeg->clk_per);
2610 
2611 	return 0;
2612 }
2613 #endif
2614 
2615 #ifdef CONFIG_PM_SLEEP
2616 static int mxc_jpeg_suspend(struct device *dev)
2617 {
2618 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
2619 
2620 	v4l2_m2m_suspend(jpeg->m2m_dev);
2621 	return pm_runtime_force_suspend(dev);
2622 }
2623 
2624 static int mxc_jpeg_resume(struct device *dev)
2625 {
2626 	struct mxc_jpeg_dev *jpeg = dev_get_drvdata(dev);
2627 	int ret;
2628 
2629 	ret = pm_runtime_force_resume(dev);
2630 	if (ret < 0)
2631 		return ret;
2632 
2633 	v4l2_m2m_resume(jpeg->m2m_dev);
2634 	return ret;
2635 }
2636 #endif
2637 
2638 static const struct dev_pm_ops	mxc_jpeg_pm_ops = {
2639 	SET_RUNTIME_PM_OPS(mxc_jpeg_runtime_suspend,
2640 			   mxc_jpeg_runtime_resume, NULL)
2641 	SET_SYSTEM_SLEEP_PM_OPS(mxc_jpeg_suspend, mxc_jpeg_resume)
2642 };
2643 
2644 static int mxc_jpeg_remove(struct platform_device *pdev)
2645 {
2646 	unsigned int slot;
2647 	struct mxc_jpeg_dev *jpeg = platform_get_drvdata(pdev);
2648 
2649 	for (slot = 0; slot < MXC_MAX_SLOTS; slot++)
2650 		mxc_jpeg_free_slot_data(jpeg, slot);
2651 
2652 	pm_runtime_disable(&pdev->dev);
2653 	video_unregister_device(jpeg->dec_vdev);
2654 	v4l2_m2m_release(jpeg->m2m_dev);
2655 	v4l2_device_unregister(&jpeg->v4l2_dev);
2656 	mxc_jpeg_detach_pm_domains(jpeg);
2657 
2658 	return 0;
2659 }
2660 
2661 MODULE_DEVICE_TABLE(of, mxc_jpeg_match);
2662 
2663 static struct platform_driver mxc_jpeg_driver = {
2664 	.probe = mxc_jpeg_probe,
2665 	.remove = mxc_jpeg_remove,
2666 	.driver = {
2667 		.name = "mxc-jpeg",
2668 		.of_match_table = mxc_jpeg_match,
2669 		.pm = &mxc_jpeg_pm_ops,
2670 	},
2671 };
2672 module_platform_driver(mxc_jpeg_driver);
2673 
2674 MODULE_AUTHOR("Zhengyu Shen <zhengyu.shen_1@nxp.com>");
2675 MODULE_AUTHOR("Mirela Rabulea <mirela.rabulea@nxp.com>");
2676 MODULE_DESCRIPTION("V4L2 driver for i.MX8 QXP/QM JPEG encoder/decoder");
2677 MODULE_LICENSE("GPL v2");
2678