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