drivers/media/platform/nxp/imx-jpeg/mxc-jpeg.c

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