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