drivers/media/platform/s5p-jpeg/jpeg-core.c

   1 /* linux/drivers/media/platform/s5p-jpeg/jpeg-core.c
   2  *
   3  * Copyright (c) 2011 Samsung Electronics Co., Ltd.
   4  *              http://www.samsung.com
   5  *
   6  * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License version 2 as
  10  * published by the Free Software Foundation.
  11  */
  12
  13 #include <linux/clk.h>
  14 #include <linux/err.h>
  15 #include <linux/gfp.h>
  16 #include <linux/interrupt.h>
  17 #include <linux/io.h>
  18 #include <linux/kernel.h>
  19 #include <linux/module.h>
  20 #include <linux/platform_device.h>
  21 #include <linux/pm_runtime.h>
  22 #include <linux/slab.h>
  23 #include <linux/spinlock.h>
  24 #include <linux/string.h>
  25 #include <media/v4l2-mem2mem.h>
  26 #include <media/v4l2-ioctl.h>
  27 #include <media/videobuf2-core.h>
  28 #include <media/videobuf2-dma-contig.h>
  29
  30 #include "jpeg-core.h"
  31 #include "jpeg-hw.h"
  32
  33 static struct s5p_jpeg_fmt formats_enc[] = {
  34         {
  35                 .name           = "JPEG JFIF",
  36                 .fourcc         = V4L2_PIX_FMT_JPEG,
  37                 .colplanes      = 1,
  38                 .types          = MEM2MEM_CAPTURE,
  39         },
  40         {
  41                 .name           = "YUV 4:2:2 packed, YCbYCr",
  42                 .fourcc         = V4L2_PIX_FMT_YUYV,
  43                 .depth          = 16,
  44                 .colplanes      = 1,
  45                 .types          = MEM2MEM_OUTPUT,
  46         },
  47         {
  48                 .name           = "RGB565",
  49                 .fourcc         = V4L2_PIX_FMT_RGB565,
  50                 .depth          = 16,
  51                 .colplanes      = 1,
  52                 .types          = MEM2MEM_OUTPUT,
  53         },
  54 };
  55 #define NUM_FORMATS_ENC ARRAY_SIZE(formats_enc)
  56
  57 static struct s5p_jpeg_fmt formats_dec[] = {
  58         {
  59                 .name           = "YUV 4:2:0 planar, YCbCr",
  60                 .fourcc         = V4L2_PIX_FMT_YUV420,
  61                 .depth          = 12,
  62                 .colplanes      = 3,
  63                 .h_align        = 4,
  64                 .v_align        = 4,
  65                 .types          = MEM2MEM_CAPTURE,
  66         },
  67         {
  68                 .name           = "YUV 4:2:2 packed, YCbYCr",
  69                 .fourcc         = V4L2_PIX_FMT_YUYV,
  70                 .depth          = 16,
  71                 .colplanes      = 1,
  72                 .h_align        = 4,
  73                 .v_align        = 3,
  74                 .types          = MEM2MEM_CAPTURE,
  75         },
  76         {
  77                 .name           = "JPEG JFIF",
  78                 .fourcc         = V4L2_PIX_FMT_JPEG,
  79                 .colplanes      = 1,
  80                 .types          = MEM2MEM_OUTPUT,
  81         },
  82 };
  83 #define NUM_FORMATS_DEC ARRAY_SIZE(formats_dec)
  84
  85 static const unsigned char qtbl_luminance[4][64] = {
  86         {/* level 1 - high quality */
  87                  8,  6,  6,  8, 12, 14, 16, 17,
  88                  6,  6,  6,  8, 10, 13, 12, 15,
  89                  6,  6,  7,  8, 13, 14, 18, 24,
  90                  8,  8,  8, 14, 13, 19, 24, 35,
  91                 12, 10, 13, 13, 20, 26, 34, 39,
  92                 14, 13, 14, 19, 26, 34, 39, 39,
  93                 16, 12, 18, 24, 34, 39, 39, 39,
  94                 17, 15, 24, 35, 39, 39, 39, 39
  95         },
  96         {/* level 2 */
  97                 12,  8,  8, 12, 17, 21, 24, 23,
  98                  8,  9,  9, 11, 15, 19, 18, 23,
  99                  8,  9, 10, 12, 19, 20, 27, 36,
 100                 12, 11, 12, 21, 20, 28, 36, 53,
 101                 17, 15, 19, 20, 30, 39, 51, 59,
 102                 21, 19, 20, 28, 39, 51, 59, 59,
 103                 24, 18, 27, 36, 51, 59, 59, 59,
 104                 23, 23, 36, 53, 59, 59, 59, 59
 105         },
 106         {/* level 3 */
 107                 16, 11, 11, 16, 23, 27, 31, 30,
 108                 11, 12, 12, 15, 20, 23, 23, 30,
 109                 11, 12, 13, 16, 23, 26, 35, 47,
 110                 16, 15, 16, 23, 26, 37, 47, 64,
 111                 23, 20, 23, 26, 39, 51, 64, 64,
 112                 27, 23, 26, 37, 51, 64, 64, 64,
 113                 31, 23, 35, 47, 64, 64, 64, 64,
 114                 30, 30, 47, 64, 64, 64, 64, 64
 115         },
 116         {/*level 4 - low quality */
 117                 20, 16, 25, 39, 50, 46, 62, 68,
 118                 16, 18, 23, 38, 38, 53, 65, 68,
 119                 25, 23, 31, 38, 53, 65, 68, 68,
 120                 39, 38, 38, 53, 65, 68, 68, 68,
 121                 50, 38, 53, 65, 68, 68, 68, 68,
 122                 46, 53, 65, 68, 68, 68, 68, 68,
 123                 62, 65, 68, 68, 68, 68, 68, 68,
 124                 68, 68, 68, 68, 68, 68, 68, 68
 125         }
 126 };
 127
 128 static const unsigned char qtbl_chrominance[4][64] = {
 129         {/* level 1 - high quality */
 130                  9,  8,  9, 11, 14, 17, 19, 24,
 131                  8, 10,  9, 11, 14, 13, 17, 22,
 132                  9,  9, 13, 14, 13, 15, 23, 26,
 133                 11, 11, 14, 14, 15, 20, 26, 33,
 134                 14, 14, 13, 15, 20, 24, 33, 39,
 135                 17, 13, 15, 20, 24, 32, 39, 39,
 136                 19, 17, 23, 26, 33, 39, 39, 39,
 137                 24, 22, 26, 33, 39, 39, 39, 39
 138         },
 139         {/* level 2 */
 140                 13, 11, 13, 16, 20, 20, 29, 37,
 141                 11, 14, 14, 14, 16, 20, 26, 32,
 142                 13, 14, 15, 17, 20, 23, 35, 40,
 143                 16, 14, 17, 21, 23, 30, 40, 50,
 144                 20, 16, 20, 23, 30, 37, 50, 59,
 145                 20, 20, 23, 30, 37, 48, 59, 59,
 146                 29, 26, 35, 40, 50, 59, 59, 59,
 147                 37, 32, 40, 50, 59, 59, 59, 59
 148         },
 149         {/* level 3 */
 150                 17, 15, 17, 21, 20, 26, 38, 48,
 151                 15, 19, 18, 17, 20, 26, 35, 43,
 152                 17, 18, 20, 22, 26, 30, 46, 53,
 153                 21, 17, 22, 28, 30, 39, 53, 64,
 154                 20, 20, 26, 30, 39, 48, 64, 64,
 155                 26, 26, 30, 39, 48, 63, 64, 64,
 156                 38, 35, 46, 53, 64, 64, 64, 64,
 157                 48, 43, 53, 64, 64, 64, 64, 64
 158         },
 159         {/*level 4 - low quality */
 160                 21, 25, 32, 38, 54, 68, 68, 68,
 161                 25, 28, 24, 38, 54, 68, 68, 68,
 162                 32, 24, 32, 43, 66, 68, 68, 68,
 163                 38, 38, 43, 53, 68, 68, 68, 68,
 164                 54, 54, 66, 68, 68, 68, 68, 68,
 165                 68, 68, 68, 68, 68, 68, 68, 68,
 166                 68, 68, 68, 68, 68, 68, 68, 68,
 167                 68, 68, 68, 68, 68, 68, 68, 68
 168         }
 169 };
 170
 171 static const unsigned char hdctbl0[16] = {
 172         0, 1, 5, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0
 173 };
 174
 175 static const unsigned char hdctblg0[12] = {
 176         0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0xa, 0xb
 177 };
 178 static const unsigned char hactbl0[16] = {
 179         0, 2, 1, 3, 3, 2, 4, 3, 5, 5, 4, 4, 0, 0, 1, 0x7d
 180 };
 181 static const unsigned char hactblg0[162] = {
 182         0x01, 0x02, 0x03, 0x00, 0x04, 0x11, 0x05, 0x12,
 183         0x21, 0x31, 0x41, 0x06, 0x13, 0x51, 0x61, 0x07,
 184         0x22, 0x71, 0x14, 0x32, 0x81, 0x91, 0xa1, 0x08,
 185         0x23, 0x42, 0xb1, 0xc1, 0x15, 0x52, 0xd1, 0xf0,
 186         0x24, 0x33, 0x62, 0x72, 0x82, 0x09, 0x0a, 0x16,
 187         0x17, 0x18, 0x19, 0x1a, 0x25, 0x26, 0x27, 0x28,
 188         0x29, 0x2a, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39,
 189         0x3a, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49,
 190         0x4a, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59,
 191         0x5a, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68, 0x69,
 192         0x6a, 0x73, 0x74, 0x75, 0x76, 0x77, 0x78, 0x79,
 193         0x7a, 0x83, 0x84, 0x85, 0x86, 0x87, 0x88, 0x89,
 194         0x8a, 0x92, 0x93, 0x94, 0x95, 0x96, 0x97, 0x98,
 195         0x99, 0x9a, 0xa2, 0xa3, 0xa4, 0xa5, 0xa6, 0xa7,
 196         0xa8, 0xa9, 0xaa, 0xb2, 0xb3, 0xb4, 0xb5, 0xb6,
 197         0xb7, 0xb8, 0xb9, 0xba, 0xc2, 0xc3, 0xc4, 0xc5,
 198         0xc6, 0xc7, 0xc8, 0xc9, 0xca, 0xd2, 0xd3, 0xd4,
 199         0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe1, 0xe2,
 200         0xe3, 0xe4, 0xe5, 0xe6, 0xe7, 0xe8, 0xe9, 0xea,
 201         0xf1, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8,
 202         0xf9, 0xfa
 203 };
 204
 205 static inline struct s5p_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *c)
 206 {
 207         return container_of(c->handler, struct s5p_jpeg_ctx, ctrl_handler);
 208 }
 209
 210 static inline struct s5p_jpeg_ctx *fh_to_ctx(struct v4l2_fh *fh)
 211 {
 212         return container_of(fh, struct s5p_jpeg_ctx, fh);
 213 }
 214
 215 static inline void jpeg_set_qtbl(void __iomem *regs, const unsigned char *qtbl,
 216                    unsigned long tab, int len)
 217 {
 218         int i;
 219
 220         for (i = 0; i < len; i++)
 221                 writel((unsigned int)qtbl[i], regs + tab + (i * 0x04));
 222 }
 223
 224 static inline void jpeg_set_qtbl_lum(void __iomem *regs, int quality)
 225 {
 226         /* this driver fills quantisation table 0 with data for luma */
 227         jpeg_set_qtbl(regs, qtbl_luminance[quality], S5P_JPG_QTBL_CONTENT(0),
 228                       ARRAY_SIZE(qtbl_luminance[quality]));
 229 }
 230
 231 static inline void jpeg_set_qtbl_chr(void __iomem *regs, int quality)
 232 {
 233         /* this driver fills quantisation table 1 with data for chroma */
 234         jpeg_set_qtbl(regs, qtbl_chrominance[quality], S5P_JPG_QTBL_CONTENT(1),
 235                       ARRAY_SIZE(qtbl_chrominance[quality]));
 236 }
 237
 238 static inline void jpeg_set_htbl(void __iomem *regs, const unsigned char *htbl,
 239                    unsigned long tab, int len)
 240 {
 241         int i;
 242
 243         for (i = 0; i < len; i++)
 244                 writel((unsigned int)htbl[i], regs + tab + (i * 0x04));
 245 }
 246
 247 static inline void jpeg_set_hdctbl(void __iomem *regs)
 248 {
 249         /* this driver fills table 0 for this component */
 250         jpeg_set_htbl(regs, hdctbl0, S5P_JPG_HDCTBL(0), ARRAY_SIZE(hdctbl0));
 251 }
 252
 253 static inline void jpeg_set_hdctblg(void __iomem *regs)
 254 {
 255         /* this driver fills table 0 for this component */
 256         jpeg_set_htbl(regs, hdctblg0, S5P_JPG_HDCTBLG(0), ARRAY_SIZE(hdctblg0));
 257 }
 258
 259 static inline void jpeg_set_hactbl(void __iomem *regs)
 260 {
 261         /* this driver fills table 0 for this component */
 262         jpeg_set_htbl(regs, hactbl0, S5P_JPG_HACTBL(0), ARRAY_SIZE(hactbl0));
 263 }
 264
 265 static inline void jpeg_set_hactblg(void __iomem *regs)
 266 {
 267         /* this driver fills table 0 for this component */
 268         jpeg_set_htbl(regs, hactblg0, S5P_JPG_HACTBLG(0), ARRAY_SIZE(hactblg0));
 269 }
 270
 271 /*
 272  * ============================================================================
 273  * Device file operations
 274  * ============================================================================
 275  */
 276
 277 static int queue_init(void *priv, struct vb2_queue *src_vq,
 278                       struct vb2_queue *dst_vq);
 279 static struct s5p_jpeg_fmt *s5p_jpeg_find_format(unsigned int mode,
 280                                                  __u32 pixelformat);
 281 static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx);
 282
 283 static int s5p_jpeg_open(struct file *file)
 284 {
 285         struct s5p_jpeg *jpeg = video_drvdata(file);
 286         struct video_device *vfd = video_devdata(file);
 287         struct s5p_jpeg_ctx *ctx;
 288         struct s5p_jpeg_fmt *out_fmt;
 289         int ret = 0;
 290
 291         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
 292         if (!ctx)
 293                 return -ENOMEM;
 294
 295         if (mutex_lock_interruptible(&jpeg->lock)) {
 296                 ret = -ERESTARTSYS;
 297                 goto free;
 298         }
 299
 300         v4l2_fh_init(&ctx->fh, vfd);
 301         /* Use separate control handler per file handle */
 302         ctx->fh.ctrl_handler = &ctx->ctrl_handler;
 303         file->private_data = &ctx->fh;
 304         v4l2_fh_add(&ctx->fh);
 305
 306         ctx->jpeg = jpeg;
 307         if (vfd == jpeg->vfd_encoder) {
 308                 ctx->mode = S5P_JPEG_ENCODE;
 309                 out_fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_RGB565);
 310         } else {
 311                 ctx->mode = S5P_JPEG_DECODE;
 312                 out_fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_JPEG);
 313         }
 314
 315         ret = s5p_jpeg_controls_create(ctx);
 316         if (ret < 0)
 317                 goto error;
 318
 319         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx, queue_init);
 320         if (IS_ERR(ctx->fh.m2m_ctx)) {
 321                 ret = PTR_ERR(ctx->fh.m2m_ctx);
 322                 goto error;
 323         }
 324
 325         ctx->out_q.fmt = out_fmt;
 326         ctx->cap_q.fmt = s5p_jpeg_find_format(ctx->mode, V4L2_PIX_FMT_YUYV);
 327         mutex_unlock(&jpeg->lock);
 328         return 0;
 329
 330 error:
 331         v4l2_fh_del(&ctx->fh);
 332         v4l2_fh_exit(&ctx->fh);
 333         mutex_unlock(&jpeg->lock);
 334 free:
 335         kfree(ctx);
 336         return ret;
 337 }
 338
 339 static int s5p_jpeg_release(struct file *file)
 340 {
 341         struct s5p_jpeg *jpeg = video_drvdata(file);
 342         struct s5p_jpeg_ctx *ctx = fh_to_ctx(file->private_data);
 343
 344         mutex_lock(&jpeg->lock);
 345         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
 346         mutex_unlock(&jpeg->lock);
 347         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 348         v4l2_fh_del(&ctx->fh);
 349         v4l2_fh_exit(&ctx->fh);
 350         kfree(ctx);
 351
 352         return 0;
 353 }
 354
 355 static const struct v4l2_file_operations s5p_jpeg_fops = {
 356         .owner          = THIS_MODULE,
 357         .open           = s5p_jpeg_open,
 358         .release        = s5p_jpeg_release,
 359         .poll           = v4l2_m2m_fop_poll,
 360         .unlocked_ioctl = video_ioctl2,
 361         .mmap           = v4l2_m2m_fop_mmap,
 362 };
 363
 364 /*
 365  * ============================================================================
 366  * video ioctl operations
 367  * ============================================================================
 368  */
 369
 370 static int get_byte(struct s5p_jpeg_buffer *buf)
 371 {
 372         if (buf->curr >= buf->size)
 373                 return -1;
 374
 375         return ((unsigned char *)buf->data)[buf->curr++];
 376 }
 377
 378 static int get_word_be(struct s5p_jpeg_buffer *buf, unsigned int *word)
 379 {
 380         unsigned int temp;
 381         int byte;
 382
 383         byte = get_byte(buf);
 384         if (byte == -1)
 385                 return -1;
 386         temp = byte << 8;
 387         byte = get_byte(buf);
 388         if (byte == -1)
 389                 return -1;
 390         *word = (unsigned int)byte | temp;
 391         return 0;
 392 }
 393
 394 static void skip(struct s5p_jpeg_buffer *buf, long len)
 395 {
 396         if (len <= 0)
 397                 return;
 398
 399         while (len--)
 400                 get_byte(buf);
 401 }
 402
 403 static bool s5p_jpeg_parse_hdr(struct s5p_jpeg_q_data *result,
 404                                unsigned long buffer, unsigned long size)
 405 {
 406         int c, components, notfound;
 407         unsigned int height, width, word;
 408         long length;
 409         struct s5p_jpeg_buffer jpeg_buffer;
 410
 411         jpeg_buffer.size = size;
 412         jpeg_buffer.data = buffer;
 413         jpeg_buffer.curr = 0;
 414
 415         notfound = 1;
 416         while (notfound) {
 417                 c = get_byte(&jpeg_buffer);
 418                 if (c == -1)
 419                         break;
 420                 if (c != 0xff)
 421                         continue;
 422                 do
 423                         c = get_byte(&jpeg_buffer);
 424                 while (c == 0xff);
 425                 if (c == -1)
 426                         break;
 427                 if (c == 0)
 428                         continue;
 429                 length = 0;
 430                 switch (c) {
 431                 /* SOF0: baseline JPEG */
 432                 case SOF0:
 433                         if (get_word_be(&jpeg_buffer, &word))
 434                                 break;
 435                         if (get_byte(&jpeg_buffer) == -1)
 436                                 break;
 437                         if (get_word_be(&jpeg_buffer, &height))
 438                                 break;
 439                         if (get_word_be(&jpeg_buffer, &width))
 440                                 break;
 441                         components = get_byte(&jpeg_buffer);
 442                         if (components == -1)
 443                                 break;
 444                         notfound = 0;
 445
 446                         skip(&jpeg_buffer, components * 3);
 447                         break;
 448
 449                 /* skip payload-less markers */
 450                 case RST ... RST + 7:
 451                 case SOI:
 452                 case EOI:
 453                 case TEM:
 454                         break;
 455
 456                 /* skip uninteresting payload markers */
 457                 default:
 458                         if (get_word_be(&jpeg_buffer, &word))
 459                                 break;
 460                         length = (long)word - 2;
 461                         skip(&jpeg_buffer, length);
 462                         break;
 463                 }
 464         }
 465         result->w = width;
 466         result->h = height;
 467         result->size = components;
 468         return !notfound;
 469 }
 470
 471 static int s5p_jpeg_querycap(struct file *file, void *priv,
 472                            struct v4l2_capability *cap)
 473 {
 474         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 475
 476         if (ctx->mode == S5P_JPEG_ENCODE) {
 477                 strlcpy(cap->driver, S5P_JPEG_M2M_NAME " encoder",
 478                         sizeof(cap->driver));
 479                 strlcpy(cap->card, S5P_JPEG_M2M_NAME " encoder",
 480                         sizeof(cap->card));
 481         } else {
 482                 strlcpy(cap->driver, S5P_JPEG_M2M_NAME " decoder",
 483                         sizeof(cap->driver));
 484                 strlcpy(cap->card, S5P_JPEG_M2M_NAME " decoder",
 485                         sizeof(cap->card));
 486         }
 487         cap->bus_info[0] = 0;
 488         /*
 489          * This is only a mem-to-mem video device. The capture and output
 490          * device capability flags are left only for backward compatibility
 491          * and are scheduled for removal.
 492          */
 493         cap->capabilities = V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_M2M |
 494                             V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT;
 495         return 0;
 496 }
 497
 498 static int enum_fmt(struct s5p_jpeg_fmt *formats, int n,
 499                     struct v4l2_fmtdesc *f, u32 type)
 500 {
 501         int i, num = 0;
 502
 503         for (i = 0; i < n; ++i) {
 504                 if (formats[i].types & type) {
 505                         /* index-th format of type type found ? */
 506                         if (num == f->index)
 507                                 break;
 508                         /* Correct type but haven't reached our index yet,
 509                          * just increment per-type index */
 510                         ++num;
 511                 }
 512         }
 513
 514         /* Format not found */
 515         if (i >= n)
 516                 return -EINVAL;
 517
 518         strlcpy(f->description, formats[i].name, sizeof(f->description));
 519         f->pixelformat = formats[i].fourcc;
 520
 521         return 0;
 522 }
 523
 524 static int s5p_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
 525                                    struct v4l2_fmtdesc *f)
 526 {
 527         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 528
 529         if (ctx->mode == S5P_JPEG_ENCODE)
 530                 return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
 531                                 MEM2MEM_CAPTURE);
 532
 533         return enum_fmt(formats_dec, NUM_FORMATS_DEC, f, MEM2MEM_CAPTURE);
 534 }
 535
 536 static int s5p_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
 537                                    struct v4l2_fmtdesc *f)
 538 {
 539         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 540
 541         if (ctx->mode == S5P_JPEG_ENCODE)
 542                 return enum_fmt(formats_enc, NUM_FORMATS_ENC, f,
 543                                 MEM2MEM_OUTPUT);
 544
 545         return enum_fmt(formats_dec, NUM_FORMATS_DEC, f, MEM2MEM_OUTPUT);
 546 }
 547
 548 static struct s5p_jpeg_q_data *get_q_data(struct s5p_jpeg_ctx *ctx,
 549                                           enum v4l2_buf_type type)
 550 {
 551         if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
 552                 return &ctx->out_q;
 553         if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
 554                 return &ctx->cap_q;
 555
 556         return NULL;
 557 }
 558
 559 static int s5p_jpeg_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
 560 {
 561         struct vb2_queue *vq;
 562         struct s5p_jpeg_q_data *q_data = NULL;
 563         struct v4l2_pix_format *pix = &f->fmt.pix;
 564         struct s5p_jpeg_ctx *ct = fh_to_ctx(priv);
 565
 566         vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
 567         if (!vq)
 568                 return -EINVAL;
 569
 570         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
 571             ct->mode == S5P_JPEG_DECODE && !ct->hdr_parsed)
 572                 return -EINVAL;
 573         q_data = get_q_data(ct, f->type);
 574         BUG_ON(q_data == NULL);
 575
 576         pix->width = q_data->w;
 577         pix->height = q_data->h;
 578         pix->field = V4L2_FIELD_NONE;
 579         pix->pixelformat = q_data->fmt->fourcc;
 580         pix->bytesperline = 0;
 581         if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG) {
 582                 u32 bpl = q_data->w;
 583                 if (q_data->fmt->colplanes == 1)
 584                         bpl = (bpl * q_data->fmt->depth) >> 3;
 585                 pix->bytesperline = bpl;
 586         }
 587         pix->sizeimage = q_data->size;
 588
 589         return 0;
 590 }
 591
 592 static struct s5p_jpeg_fmt *s5p_jpeg_find_format(unsigned int mode,
 593                                                  u32 pixelformat)
 594 {
 595         unsigned int k;
 596         struct s5p_jpeg_fmt *formats;
 597         int n;
 598
 599         if (mode == S5P_JPEG_ENCODE) {
 600                 formats = formats_enc;
 601                 n = NUM_FORMATS_ENC;
 602         } else {
 603                 formats = formats_dec;
 604                 n = NUM_FORMATS_DEC;
 605         }
 606
 607         for (k = 0; k < n; k++) {
 608                 struct s5p_jpeg_fmt *fmt = &formats[k];
 609                 if (fmt->fourcc == pixelformat)
 610                         return fmt;
 611         }
 612
 613         return NULL;
 614
 615 }
 616
 617 static void jpeg_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
 618                                    unsigned int walign,
 619                                    u32 *h, unsigned int hmin, unsigned int hmax,
 620                                    unsigned int halign)
 621 {
 622         int width, height, w_step, h_step;
 623
 624         width = *w;
 625         height = *h;
 626
 627         w_step = 1 << walign;
 628         h_step = 1 << halign;
 629         v4l_bound_align_image(w, wmin, wmax, walign, h, hmin, hmax, halign, 0);
 630
 631         if (*w < width && (*w + w_step) < wmax)
 632                 *w += w_step;
 633         if (*h < height && (*h + h_step) < hmax)
 634                 *h += h_step;
 635
 636 }
 637
 638 static int vidioc_try_fmt(struct v4l2_format *f, struct s5p_jpeg_fmt *fmt,
 639                           struct s5p_jpeg_ctx *ctx, int q_type)
 640 {
 641         struct v4l2_pix_format *pix = &f->fmt.pix;
 642
 643         if (pix->field == V4L2_FIELD_ANY)
 644                 pix->field = V4L2_FIELD_NONE;
 645         else if (pix->field != V4L2_FIELD_NONE)
 646                 return -EINVAL;
 647
 648         /* V4L2 specification suggests the driver corrects the format struct
 649          * if any of the dimensions is unsupported */
 650         if (q_type == MEM2MEM_OUTPUT)
 651                 jpeg_bound_align_image(&pix->width, S5P_JPEG_MIN_WIDTH,
 652                                        S5P_JPEG_MAX_WIDTH, 0,
 653                                        &pix->height, S5P_JPEG_MIN_HEIGHT,
 654                                        S5P_JPEG_MAX_HEIGHT, 0);
 655         else
 656                 jpeg_bound_align_image(&pix->width, S5P_JPEG_MIN_WIDTH,
 657                                        S5P_JPEG_MAX_WIDTH, fmt->h_align,
 658                                        &pix->height, S5P_JPEG_MIN_HEIGHT,
 659                                        S5P_JPEG_MAX_HEIGHT, fmt->v_align);
 660
 661         if (fmt->fourcc == V4L2_PIX_FMT_JPEG) {
 662                 if (pix->sizeimage <= 0)
 663                         pix->sizeimage = PAGE_SIZE;
 664                 pix->bytesperline = 0;
 665         } else {
 666                 u32 bpl = pix->bytesperline;
 667
 668                 if (fmt->colplanes > 1 && bpl < pix->width)
 669                         bpl = pix->width; /* planar */
 670
 671                 if (fmt->colplanes == 1 && /* packed */
 672                     (bpl << 3) * fmt->depth < pix->width)
 673                         bpl = (pix->width * fmt->depth) >> 3;
 674
 675                 pix->bytesperline = bpl;
 676                 pix->sizeimage = (pix->width * pix->height * fmt->depth) >> 3;
 677         }
 678
 679         return 0;
 680 }
 681
 682 static int s5p_jpeg_try_fmt_vid_cap(struct file *file, void *priv,
 683                                   struct v4l2_format *f)
 684 {
 685         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 686         struct s5p_jpeg_fmt *fmt;
 687
 688         fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
 689         if (!fmt || !(fmt->types & MEM2MEM_CAPTURE)) {
 690                 v4l2_err(&ctx->jpeg->v4l2_dev,
 691                          "Fourcc format (0x%08x) invalid.\n",
 692                          f->fmt.pix.pixelformat);
 693                 return -EINVAL;
 694         }
 695
 696         return vidioc_try_fmt(f, fmt, ctx, MEM2MEM_CAPTURE);
 697 }
 698
 699 static int s5p_jpeg_try_fmt_vid_out(struct file *file, void *priv,
 700                                   struct v4l2_format *f)
 701 {
 702         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 703         struct s5p_jpeg_fmt *fmt;
 704
 705         fmt = s5p_jpeg_find_format(ctx->mode, f->fmt.pix.pixelformat);
 706         if (!fmt || !(fmt->types & MEM2MEM_OUTPUT)) {
 707                 v4l2_err(&ctx->jpeg->v4l2_dev,
 708                          "Fourcc format (0x%08x) invalid.\n",
 709                          f->fmt.pix.pixelformat);
 710                 return -EINVAL;
 711         }
 712
 713         return vidioc_try_fmt(f, fmt, ctx, MEM2MEM_OUTPUT);
 714 }
 715
 716 static int s5p_jpeg_s_fmt(struct s5p_jpeg_ctx *ct, struct v4l2_format *f)
 717 {
 718         struct vb2_queue *vq;
 719         struct s5p_jpeg_q_data *q_data = NULL;
 720         struct v4l2_pix_format *pix = &f->fmt.pix;
 721
 722         vq = v4l2_m2m_get_vq(ct->fh.m2m_ctx, f->type);
 723         if (!vq)
 724                 return -EINVAL;
 725
 726         q_data = get_q_data(ct, f->type);
 727         BUG_ON(q_data == NULL);
 728
 729         if (vb2_is_busy(vq)) {
 730                 v4l2_err(&ct->jpeg->v4l2_dev, "%s queue busy\n", __func__);
 731                 return -EBUSY;
 732         }
 733
 734         q_data->fmt = s5p_jpeg_find_format(ct->mode, pix->pixelformat);
 735         q_data->w = pix->width;
 736         q_data->h = pix->height;
 737         if (q_data->fmt->fourcc != V4L2_PIX_FMT_JPEG)
 738                 q_data->size = q_data->w * q_data->h * q_data->fmt->depth >> 3;
 739         else
 740                 q_data->size = pix->sizeimage;
 741
 742         return 0;
 743 }
 744
 745 static int s5p_jpeg_s_fmt_vid_cap(struct file *file, void *priv,
 746                                 struct v4l2_format *f)
 747 {
 748         int ret;
 749
 750         ret = s5p_jpeg_try_fmt_vid_cap(file, priv, f);
 751         if (ret)
 752                 return ret;
 753
 754         return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
 755 }
 756
 757 static int s5p_jpeg_s_fmt_vid_out(struct file *file, void *priv,
 758                                 struct v4l2_format *f)
 759 {
 760         int ret;
 761
 762         ret = s5p_jpeg_try_fmt_vid_out(file, priv, f);
 763         if (ret)
 764                 return ret;
 765
 766         return s5p_jpeg_s_fmt(fh_to_ctx(priv), f);
 767 }
 768
 769 static int s5p_jpeg_g_selection(struct file *file, void *priv,
 770                          struct v4l2_selection *s)
 771 {
 772         struct s5p_jpeg_ctx *ctx = fh_to_ctx(priv);
 773
 774         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT &&
 775             s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 776                 return -EINVAL;
 777
 778         /* For JPEG blob active == default == bounds */
 779         switch (s->target) {
 780         case V4L2_SEL_TGT_CROP:
 781         case V4L2_SEL_TGT_CROP_BOUNDS:
 782         case V4L2_SEL_TGT_CROP_DEFAULT:
 783         case V4L2_SEL_TGT_COMPOSE:
 784         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 785                 s->r.width = ctx->out_q.w;
 786                 s->r.height = ctx->out_q.h;
 787                 break;
 788         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 789         case V4L2_SEL_TGT_COMPOSE_PADDED:
 790                 s->r.width = ctx->cap_q.w;
 791                 s->r.height = ctx->cap_q.h;
 792                 break;
 793         default:
 794                 return -EINVAL;
 795         }
 796         s->r.left = 0;
 797         s->r.top = 0;
 798         return 0;
 799 }
 800
 801 /*
 802  * V4L2 controls
 803  */
 804
 805 static int s5p_jpeg_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 806 {
 807         struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
 808         struct s5p_jpeg *jpeg = ctx->jpeg;
 809         unsigned long flags;
 810
 811         switch (ctrl->id) {
 812         case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
 813                 spin_lock_irqsave(&jpeg->slock, flags);
 814
 815                 WARN_ON(ctx->subsampling > S5P_SUBSAMPLING_MODE_GRAY);
 816                 if (ctx->subsampling > 2)
 817                         ctrl->val = V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY;
 818                 else
 819                         ctrl->val = ctx->subsampling;
 820                 spin_unlock_irqrestore(&jpeg->slock, flags);
 821                 break;
 822         }
 823
 824         return 0;
 825 }
 826
 827 static int s5p_jpeg_s_ctrl(struct v4l2_ctrl *ctrl)
 828 {
 829         struct s5p_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
 830         unsigned long flags;
 831
 832         spin_lock_irqsave(&ctx->jpeg->slock, flags);
 833
 834         switch (ctrl->id) {
 835         case V4L2_CID_JPEG_COMPRESSION_QUALITY:
 836                 ctx->compr_quality = S5P_JPEG_COMPR_QUAL_WORST - ctrl->val;
 837                 break;
 838         case V4L2_CID_JPEG_RESTART_INTERVAL:
 839                 ctx->restart_interval = ctrl->val;
 840                 break;
 841         case V4L2_CID_JPEG_CHROMA_SUBSAMPLING:
 842                 ctx->subsampling = ctrl->val;
 843                 break;
 844         }
 845
 846         spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
 847         return 0;
 848 }
 849
 850 static const struct v4l2_ctrl_ops s5p_jpeg_ctrl_ops = {
 851         .g_volatile_ctrl        = s5p_jpeg_g_volatile_ctrl,
 852         .s_ctrl                 = s5p_jpeg_s_ctrl,
 853 };
 854
 855 static int s5p_jpeg_controls_create(struct s5p_jpeg_ctx *ctx)
 856 {
 857         unsigned int mask = ~0x27; /* 444, 422, 420, GRAY */
 858         struct v4l2_ctrl *ctrl;
 859
 860         v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
 861
 862         if (ctx->mode == S5P_JPEG_ENCODE) {
 863                 v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 864                                   V4L2_CID_JPEG_COMPRESSION_QUALITY,
 865                                   0, 3, 1, 3);
 866
 867                 v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 868                                   V4L2_CID_JPEG_RESTART_INTERVAL,
 869                                   0, 3, 0xffff, 0);
 870                 mask = ~0x06; /* 422, 420 */
 871         }
 872
 873         ctrl = v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 874                                       V4L2_CID_JPEG_CHROMA_SUBSAMPLING,
 875                                       V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY, mask,
 876                                       V4L2_JPEG_CHROMA_SUBSAMPLING_422);
 877
 878         if (ctx->ctrl_handler.error)
 879                 return ctx->ctrl_handler.error;
 880
 881         if (ctx->mode == S5P_JPEG_DECODE)
 882                 ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
 883                         V4L2_CTRL_FLAG_READ_ONLY;
 884         return 0;
 885 }
 886
 887 static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
 888         .vidioc_querycap                = s5p_jpeg_querycap,
 889
 890         .vidioc_enum_fmt_vid_cap        = s5p_jpeg_enum_fmt_vid_cap,
 891         .vidioc_enum_fmt_vid_out        = s5p_jpeg_enum_fmt_vid_out,
 892
 893         .vidioc_g_fmt_vid_cap           = s5p_jpeg_g_fmt,
 894         .vidioc_g_fmt_vid_out           = s5p_jpeg_g_fmt,
 895
 896         .vidioc_try_fmt_vid_cap         = s5p_jpeg_try_fmt_vid_cap,
 897         .vidioc_try_fmt_vid_out         = s5p_jpeg_try_fmt_vid_out,
 898
 899         .vidioc_s_fmt_vid_cap           = s5p_jpeg_s_fmt_vid_cap,
 900         .vidioc_s_fmt_vid_out           = s5p_jpeg_s_fmt_vid_out,
 901
 902         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 903         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 904         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 905         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 906
 907         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 908         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 909
 910         .vidioc_g_selection             = s5p_jpeg_g_selection,
 911 };
 912
 913 /*
 914  * ============================================================================
 915  * mem2mem callbacks
 916  * ============================================================================
 917  */
 918
 919 static void s5p_jpeg_device_run(void *priv)
 920 {
 921         struct s5p_jpeg_ctx *ctx = priv;
 922         struct s5p_jpeg *jpeg = ctx->jpeg;
 923         struct vb2_buffer *src_buf, *dst_buf;
 924         unsigned long src_addr, dst_addr;
 925
 926         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 927         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 928         src_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
 929         dst_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
 930
 931         jpeg_reset(jpeg->regs);
 932         jpeg_poweron(jpeg->regs);
 933         jpeg_proc_mode(jpeg->regs, ctx->mode);
 934         if (ctx->mode == S5P_JPEG_ENCODE) {
 935                 if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565)
 936                         jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_565);
 937                 else
 938                         jpeg_input_raw_mode(jpeg->regs, S5P_JPEG_RAW_IN_422);
 939                 jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
 940                 jpeg_dri(jpeg->regs, ctx->restart_interval);
 941                 jpeg_x(jpeg->regs, ctx->out_q.w);
 942                 jpeg_y(jpeg->regs, ctx->out_q.h);
 943                 jpeg_imgadr(jpeg->regs, src_addr);
 944                 jpeg_jpgadr(jpeg->regs, dst_addr);
 945
 946                 /* ultimately comes from sizeimage from userspace */
 947                 jpeg_enc_stream_int(jpeg->regs, ctx->cap_q.size);
 948
 949                 /* JPEG RGB to YCbCr conversion matrix */
 950                 jpeg_coef(jpeg->regs, 1, 1, S5P_JPEG_COEF11);
 951                 jpeg_coef(jpeg->regs, 1, 2, S5P_JPEG_COEF12);
 952                 jpeg_coef(jpeg->regs, 1, 3, S5P_JPEG_COEF13);
 953                 jpeg_coef(jpeg->regs, 2, 1, S5P_JPEG_COEF21);
 954                 jpeg_coef(jpeg->regs, 2, 2, S5P_JPEG_COEF22);
 955                 jpeg_coef(jpeg->regs, 2, 3, S5P_JPEG_COEF23);
 956                 jpeg_coef(jpeg->regs, 3, 1, S5P_JPEG_COEF31);
 957                 jpeg_coef(jpeg->regs, 3, 2, S5P_JPEG_COEF32);
 958                 jpeg_coef(jpeg->regs, 3, 3, S5P_JPEG_COEF33);
 959
 960                 /*
 961                  * JPEG IP allows storing 4 quantization tables
 962                  * We fill table 0 for luma and table 1 for chroma
 963                  */
 964                 jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
 965                 jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
 966                 /* use table 0 for Y */
 967                 jpeg_qtbl(jpeg->regs, 1, 0);
 968                 /* use table 1 for Cb and Cr*/
 969                 jpeg_qtbl(jpeg->regs, 2, 1);
 970                 jpeg_qtbl(jpeg->regs, 3, 1);
 971
 972                 /* Y, Cb, Cr use Huffman table 0 */
 973                 jpeg_htbl_ac(jpeg->regs, 1);
 974                 jpeg_htbl_dc(jpeg->regs, 1);
 975                 jpeg_htbl_ac(jpeg->regs, 2);
 976                 jpeg_htbl_dc(jpeg->regs, 2);
 977                 jpeg_htbl_ac(jpeg->regs, 3);
 978                 jpeg_htbl_dc(jpeg->regs, 3);
 979         } else { /* S5P_JPEG_DECODE */
 980                 jpeg_rst_int_enable(jpeg->regs, true);
 981                 jpeg_data_num_int_enable(jpeg->regs, true);
 982                 jpeg_final_mcu_num_int_enable(jpeg->regs, true);
 983                 if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
 984                         jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
 985                 else
 986                         jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
 987                 jpeg_jpgadr(jpeg->regs, src_addr);
 988                 jpeg_imgadr(jpeg->regs, dst_addr);
 989         }
 990
 991         jpeg_start(jpeg->regs);
 992 }
 993
 994 static int s5p_jpeg_job_ready(void *priv)
 995 {
 996         struct s5p_jpeg_ctx *ctx = priv;
 997
 998         if (ctx->mode == S5P_JPEG_DECODE)
 999                 return ctx->hdr_parsed;
1000         return 1;
1001 }
1002
1003 static void s5p_jpeg_job_abort(void *priv)
1004 {
1005 }
1006
1007 static struct v4l2_m2m_ops s5p_jpeg_m2m_ops = {
1008         .device_run     = s5p_jpeg_device_run,
1009         .job_ready      = s5p_jpeg_job_ready,
1010         .job_abort      = s5p_jpeg_job_abort,
1011 };
1012
1013 /*
1014  * ============================================================================
1015  * Queue operations
1016  * ============================================================================
1017  */
1018
1019 static int s5p_jpeg_queue_setup(struct vb2_queue *vq,
1020                            const struct v4l2_format *fmt,
1021                            unsigned int *nbuffers, unsigned int *nplanes,
1022                            unsigned int sizes[], void *alloc_ctxs[])
1023 {
1024         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vq);
1025         struct s5p_jpeg_q_data *q_data = NULL;
1026         unsigned int size, count = *nbuffers;
1027
1028         q_data = get_q_data(ctx, vq->type);
1029         BUG_ON(q_data == NULL);
1030
1031         size = q_data->size;
1032
1033         /*
1034          * header is parsed during decoding and parsed information stored
1035          * in the context so we do not allow another buffer to overwrite it
1036          */
1037         if (ctx->mode == S5P_JPEG_DECODE)
1038                 count = 1;
1039
1040         *nbuffers = count;
1041         *nplanes = 1;
1042         sizes[0] = size;
1043         alloc_ctxs[0] = ctx->jpeg->alloc_ctx;
1044
1045         return 0;
1046 }
1047
1048 static int s5p_jpeg_buf_prepare(struct vb2_buffer *vb)
1049 {
1050         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1051         struct s5p_jpeg_q_data *q_data = NULL;
1052
1053         q_data = get_q_data(ctx, vb->vb2_queue->type);
1054         BUG_ON(q_data == NULL);
1055
1056         if (vb2_plane_size(vb, 0) < q_data->size) {
1057                 pr_err("%s data will not fit into plane (%lu < %lu)\n",
1058                                 __func__, vb2_plane_size(vb, 0),
1059                                 (long)q_data->size);
1060                 return -EINVAL;
1061         }
1062
1063         vb2_set_plane_payload(vb, 0, q_data->size);
1064
1065         return 0;
1066 }
1067
1068 static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
1069 {
1070         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1071
1072         if (ctx->mode == S5P_JPEG_DECODE &&
1073             vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
1074                 struct s5p_jpeg_q_data tmp, *q_data;
1075                 ctx->hdr_parsed = s5p_jpeg_parse_hdr(&tmp,
1076                      (unsigned long)vb2_plane_vaddr(vb, 0),
1077                      min((unsigned long)ctx->out_q.size,
1078                          vb2_get_plane_payload(vb, 0)));
1079                 if (!ctx->hdr_parsed) {
1080                         vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
1081                         return;
1082                 }
1083
1084                 q_data = &ctx->out_q;
1085                 q_data->w = tmp.w;
1086                 q_data->h = tmp.h;
1087
1088                 q_data = &ctx->cap_q;
1089                 q_data->w = tmp.w;
1090                 q_data->h = tmp.h;
1091
1092                 jpeg_bound_align_image(&q_data->w, S5P_JPEG_MIN_WIDTH,
1093                                        S5P_JPEG_MAX_WIDTH, q_data->fmt->h_align,
1094                                        &q_data->h, S5P_JPEG_MIN_HEIGHT,
1095                                        S5P_JPEG_MAX_HEIGHT, q_data->fmt->v_align
1096                                       );
1097                 q_data->size = q_data->w * q_data->h * q_data->fmt->depth >> 3;
1098         }
1099
1100         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
1101 }
1102
1103 static int s5p_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
1104 {
1105         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1106         int ret;
1107
1108         ret = pm_runtime_get_sync(ctx->jpeg->dev);
1109
1110         return ret > 0 ? 0 : ret;
1111 }
1112
1113 static int s5p_jpeg_stop_streaming(struct vb2_queue *q)
1114 {
1115         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1116
1117         pm_runtime_put(ctx->jpeg->dev);
1118
1119         return 0;
1120 }
1121
1122 static struct vb2_ops s5p_jpeg_qops = {
1123         .queue_setup            = s5p_jpeg_queue_setup,
1124         .buf_prepare            = s5p_jpeg_buf_prepare,
1125         .buf_queue              = s5p_jpeg_buf_queue,
1126         .wait_prepare           = vb2_ops_wait_prepare,
1127         .wait_finish            = vb2_ops_wait_finish,
1128         .start_streaming        = s5p_jpeg_start_streaming,
1129         .stop_streaming         = s5p_jpeg_stop_streaming,
1130 };
1131
1132 static int queue_init(void *priv, struct vb2_queue *src_vq,
1133                       struct vb2_queue *dst_vq)
1134 {
1135         struct s5p_jpeg_ctx *ctx = priv;
1136         int ret;
1137
1138         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1139         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
1140         src_vq->drv_priv = ctx;
1141         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1142         src_vq->ops = &s5p_jpeg_qops;
1143         src_vq->mem_ops = &vb2_dma_contig_memops;
1144         src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1145         src_vq->lock = &ctx->jpeg->lock;
1146
1147         ret = vb2_queue_init(src_vq);
1148         if (ret)
1149                 return ret;
1150
1151         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1152         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
1153         dst_vq->drv_priv = ctx;
1154         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1155         dst_vq->ops = &s5p_jpeg_qops;
1156         dst_vq->mem_ops = &vb2_dma_contig_memops;
1157         dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1158         dst_vq->lock = &ctx->jpeg->lock;
1159
1160         return vb2_queue_init(dst_vq);
1161 }
1162
1163 /*
1164  * ============================================================================
1165  * ISR
1166  * ============================================================================
1167  */
1168
1169 static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
1170 {
1171         struct s5p_jpeg *jpeg = dev_id;
1172         struct s5p_jpeg_ctx *curr_ctx;
1173         struct vb2_buffer *src_buf, *dst_buf;
1174         unsigned long payload_size = 0;
1175         enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
1176         bool enc_jpeg_too_large = false;
1177         bool timer_elapsed = false;
1178         bool op_completed = false;
1179
1180         spin_lock(&jpeg->slock);
1181
1182         curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1183
1184         src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
1185         dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
1186
1187         if (curr_ctx->mode == S5P_JPEG_ENCODE)
1188                 enc_jpeg_too_large = jpeg_enc_stream_stat(jpeg->regs);
1189         timer_elapsed = jpeg_timer_stat(jpeg->regs);
1190         op_completed = jpeg_result_stat_ok(jpeg->regs);
1191         if (curr_ctx->mode == S5P_JPEG_DECODE)
1192                 op_completed = op_completed && jpeg_stream_stat_ok(jpeg->regs);
1193
1194         if (enc_jpeg_too_large) {
1195                 state = VB2_BUF_STATE_ERROR;
1196                 jpeg_clear_enc_stream_stat(jpeg->regs);
1197         } else if (timer_elapsed) {
1198                 state = VB2_BUF_STATE_ERROR;
1199                 jpeg_clear_timer_stat(jpeg->regs);
1200         } else if (!op_completed) {
1201                 state = VB2_BUF_STATE_ERROR;
1202         } else {
1203                 payload_size = jpeg_compressed_size(jpeg->regs);
1204         }
1205
1206         dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
1207         dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
1208
1209         v4l2_m2m_buf_done(src_buf, state);
1210         if (curr_ctx->mode == S5P_JPEG_ENCODE)
1211                 vb2_set_plane_payload(dst_buf, 0, payload_size);
1212         v4l2_m2m_buf_done(dst_buf, state);
1213         v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
1214
1215         curr_ctx->subsampling = jpeg_get_subsampling_mode(jpeg->regs);
1216         spin_unlock(&jpeg->slock);
1217
1218         jpeg_clear_int(jpeg->regs);
1219
1220         return IRQ_HANDLED;
1221 }
1222
1223 /*
1224  * ============================================================================
1225  * Driver basic infrastructure
1226  * ============================================================================
1227  */
1228
1229 static int s5p_jpeg_probe(struct platform_device *pdev)
1230 {
1231         struct s5p_jpeg *jpeg;
1232         struct resource *res;
1233         int ret;
1234
1235         /* JPEG IP abstraction struct */
1236         jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
1237         if (!jpeg)
1238                 return -ENOMEM;
1239
1240         mutex_init(&jpeg->lock);
1241         spin_lock_init(&jpeg->slock);
1242         jpeg->dev = &pdev->dev;
1243
1244         /* memory-mapped registers */
1245         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1246
1247         jpeg->regs = devm_ioremap_resource(&pdev->dev, res);
1248         if (IS_ERR(jpeg->regs))
1249                 return PTR_ERR(jpeg->regs);
1250
1251         /* interrupt service routine registration */
1252         jpeg->irq = ret = platform_get_irq(pdev, 0);
1253         if (ret < 0) {
1254                 dev_err(&pdev->dev, "cannot find IRQ\n");
1255                 return ret;
1256         }
1257
1258         ret = devm_request_irq(&pdev->dev, jpeg->irq, s5p_jpeg_irq, 0,
1259                         dev_name(&pdev->dev), jpeg);
1260         if (ret) {
1261                 dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
1262                 return ret;
1263         }
1264
1265         /* clocks */
1266         jpeg->clk = clk_get(&pdev->dev, "jpeg");
1267         if (IS_ERR(jpeg->clk)) {
1268                 dev_err(&pdev->dev, "cannot get clock\n");
1269                 ret = PTR_ERR(jpeg->clk);
1270                 return ret;
1271         }
1272         dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
1273         clk_prepare_enable(jpeg->clk);
1274
1275         /* v4l2 device */
1276         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
1277         if (ret) {
1278                 dev_err(&pdev->dev, "Failed to register v4l2 device\n");
1279                 goto clk_get_rollback;
1280         }
1281
1282         /* mem2mem device */
1283         jpeg->m2m_dev = v4l2_m2m_init(&s5p_jpeg_m2m_ops);
1284         if (IS_ERR(jpeg->m2m_dev)) {
1285                 v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
1286                 ret = PTR_ERR(jpeg->m2m_dev);
1287                 goto device_register_rollback;
1288         }
1289
1290         jpeg->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
1291         if (IS_ERR(jpeg->alloc_ctx)) {
1292                 v4l2_err(&jpeg->v4l2_dev, "Failed to init memory allocator\n");
1293                 ret = PTR_ERR(jpeg->alloc_ctx);
1294                 goto m2m_init_rollback;
1295         }
1296
1297         /* JPEG encoder /dev/videoX node */
1298         jpeg->vfd_encoder = video_device_alloc();
1299         if (!jpeg->vfd_encoder) {
1300                 v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
1301                 ret = -ENOMEM;
1302                 goto vb2_allocator_rollback;
1303         }
1304         strlcpy(jpeg->vfd_encoder->name, S5P_JPEG_M2M_NAME,
1305                 sizeof(jpeg->vfd_encoder->name));
1306         jpeg->vfd_encoder->fops         = &s5p_jpeg_fops;
1307         jpeg->vfd_encoder->ioctl_ops    = &s5p_jpeg_ioctl_ops;
1308         jpeg->vfd_encoder->minor        = -1;
1309         jpeg->vfd_encoder->release      = video_device_release;
1310         jpeg->vfd_encoder->lock         = &jpeg->lock;
1311         jpeg->vfd_encoder->v4l2_dev     = &jpeg->v4l2_dev;
1312         jpeg->vfd_encoder->vfl_dir      = VFL_DIR_M2M;
1313
1314         ret = video_register_device(jpeg->vfd_encoder, VFL_TYPE_GRABBER, -1);
1315         if (ret) {
1316                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1317                 goto enc_vdev_alloc_rollback;
1318         }
1319
1320         video_set_drvdata(jpeg->vfd_encoder, jpeg);
1321         v4l2_info(&jpeg->v4l2_dev,
1322                   "encoder device registered as /dev/video%d\n",
1323                   jpeg->vfd_encoder->num);
1324
1325         /* JPEG decoder /dev/videoX node */
1326         jpeg->vfd_decoder = video_device_alloc();
1327         if (!jpeg->vfd_decoder) {
1328                 v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
1329                 ret = -ENOMEM;
1330                 goto enc_vdev_register_rollback;
1331         }
1332         strlcpy(jpeg->vfd_decoder->name, S5P_JPEG_M2M_NAME,
1333                 sizeof(jpeg->vfd_decoder->name));
1334         jpeg->vfd_decoder->fops         = &s5p_jpeg_fops;
1335         jpeg->vfd_decoder->ioctl_ops    = &s5p_jpeg_ioctl_ops;
1336         jpeg->vfd_decoder->minor        = -1;
1337         jpeg->vfd_decoder->release      = video_device_release;
1338         jpeg->vfd_decoder->lock         = &jpeg->lock;
1339         jpeg->vfd_decoder->v4l2_dev     = &jpeg->v4l2_dev;
1340         jpeg->vfd_decoder->vfl_dir      = VFL_DIR_M2M;
1341
1342         ret = video_register_device(jpeg->vfd_decoder, VFL_TYPE_GRABBER, -1);
1343         if (ret) {
1344                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1345                 goto dec_vdev_alloc_rollback;
1346         }
1347
1348         video_set_drvdata(jpeg->vfd_decoder, jpeg);
1349         v4l2_info(&jpeg->v4l2_dev,
1350                   "decoder device registered as /dev/video%d\n",
1351                   jpeg->vfd_decoder->num);
1352
1353         /* final statements & power management */
1354         platform_set_drvdata(pdev, jpeg);
1355
1356         pm_runtime_enable(&pdev->dev);
1357
1358         v4l2_info(&jpeg->v4l2_dev, "Samsung S5P JPEG codec\n");
1359
1360         return 0;
1361
1362 dec_vdev_alloc_rollback:
1363         video_device_release(jpeg->vfd_decoder);
1364
1365 enc_vdev_register_rollback:
1366         video_unregister_device(jpeg->vfd_encoder);
1367
1368 enc_vdev_alloc_rollback:
1369         video_device_release(jpeg->vfd_encoder);
1370
1371 vb2_allocator_rollback:
1372         vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
1373
1374 m2m_init_rollback:
1375         v4l2_m2m_release(jpeg->m2m_dev);
1376
1377 device_register_rollback:
1378         v4l2_device_unregister(&jpeg->v4l2_dev);
1379
1380 clk_get_rollback:
1381         clk_disable_unprepare(jpeg->clk);
1382         clk_put(jpeg->clk);
1383
1384         return ret;
1385 }
1386
1387 static int s5p_jpeg_remove(struct platform_device *pdev)
1388 {
1389         struct s5p_jpeg *jpeg = platform_get_drvdata(pdev);
1390
1391         pm_runtime_disable(jpeg->dev);
1392
1393         video_unregister_device(jpeg->vfd_decoder);
1394         video_device_release(jpeg->vfd_decoder);
1395         video_unregister_device(jpeg->vfd_encoder);
1396         video_device_release(jpeg->vfd_encoder);
1397         vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
1398         v4l2_m2m_release(jpeg->m2m_dev);
1399         v4l2_device_unregister(&jpeg->v4l2_dev);
1400
1401         clk_disable_unprepare(jpeg->clk);
1402         clk_put(jpeg->clk);
1403
1404         return 0;
1405 }
1406
1407 static int s5p_jpeg_runtime_suspend(struct device *dev)
1408 {
1409         return 0;
1410 }
1411
1412 static int s5p_jpeg_runtime_resume(struct device *dev)
1413 {
1414         struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
1415         /*
1416          * JPEG IP allows storing two Huffman tables for each component
1417          * We fill table 0 for each component
1418          */
1419         jpeg_set_hdctbl(jpeg->regs);
1420         jpeg_set_hdctblg(jpeg->regs);
1421         jpeg_set_hactbl(jpeg->regs);
1422         jpeg_set_hactblg(jpeg->regs);
1423         return 0;
1424 }
1425
1426 static const struct dev_pm_ops s5p_jpeg_pm_ops = {
1427         .runtime_suspend = s5p_jpeg_runtime_suspend,
1428         .runtime_resume  = s5p_jpeg_runtime_resume,
1429 };
1430
1431 static struct platform_driver s5p_jpeg_driver = {
1432         .probe = s5p_jpeg_probe,
1433         .remove = s5p_jpeg_remove,
1434         .driver = {
1435                 .owner = THIS_MODULE,
1436                 .name = S5P_JPEG_M2M_NAME,
1437                 .pm = &s5p_jpeg_pm_ops,
1438         },
1439 };
1440
1441 module_platform_driver(s5p_jpeg_driver);
1442
1443 MODULE_AUTHOR("Andrzej Pietrasiewicz <andrzej.p@samsung.com>");
1444 MODULE_DESCRIPTION("Samsung JPEG codec driver");
1445 MODULE_LICENSE("GPL");
1446