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-s5p.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         int ret;
 869
 870         v4l2_ctrl_handler_init(&ctx->ctrl_handler, 3);
 871
 872         if (ctx->mode == S5P_JPEG_ENCODE) {
 873                 v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 874                                   V4L2_CID_JPEG_COMPRESSION_QUALITY,
 875                                   0, 3, 1, S5P_JPEG_COMPR_QUAL_WORST);
 876
 877                 v4l2_ctrl_new_std(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 878                                   V4L2_CID_JPEG_RESTART_INTERVAL,
 879                                   0, 3, 0xffff, 0);
 880                 mask = ~0x06; /* 422, 420 */
 881         }
 882
 883         ctrl = v4l2_ctrl_new_std_menu(&ctx->ctrl_handler, &s5p_jpeg_ctrl_ops,
 884                                       V4L2_CID_JPEG_CHROMA_SUBSAMPLING,
 885                                       V4L2_JPEG_CHROMA_SUBSAMPLING_GRAY, mask,
 886                                       V4L2_JPEG_CHROMA_SUBSAMPLING_422);
 887
 888         if (ctx->ctrl_handler.error) {
 889                 ret = ctx->ctrl_handler.error;
 890                 goto error_free;
 891         }
 892
 893         if (ctx->mode == S5P_JPEG_DECODE)
 894                 ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE |
 895                         V4L2_CTRL_FLAG_READ_ONLY;
 896
 897         ret = v4l2_ctrl_handler_setup(&ctx->ctrl_handler);
 898         if (ret < 0)
 899                 goto error_free;
 900
 901         return ret;
 902
 903 error_free:
 904         v4l2_ctrl_handler_free(&ctx->ctrl_handler);
 905         return ret;
 906 }
 907
 908 static const struct v4l2_ioctl_ops s5p_jpeg_ioctl_ops = {
 909         .vidioc_querycap                = s5p_jpeg_querycap,
 910
 911         .vidioc_enum_fmt_vid_cap        = s5p_jpeg_enum_fmt_vid_cap,
 912         .vidioc_enum_fmt_vid_out        = s5p_jpeg_enum_fmt_vid_out,
 913
 914         .vidioc_g_fmt_vid_cap           = s5p_jpeg_g_fmt,
 915         .vidioc_g_fmt_vid_out           = s5p_jpeg_g_fmt,
 916
 917         .vidioc_try_fmt_vid_cap         = s5p_jpeg_try_fmt_vid_cap,
 918         .vidioc_try_fmt_vid_out         = s5p_jpeg_try_fmt_vid_out,
 919
 920         .vidioc_s_fmt_vid_cap           = s5p_jpeg_s_fmt_vid_cap,
 921         .vidioc_s_fmt_vid_out           = s5p_jpeg_s_fmt_vid_out,
 922
 923         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 924         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 925         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 926         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 927
 928         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 929         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 930
 931         .vidioc_g_selection             = s5p_jpeg_g_selection,
 932 };
 933
 934 /*
 935  * ============================================================================
 936  * mem2mem callbacks
 937  * ============================================================================
 938  */
 939
 940 static void s5p_jpeg_device_run(void *priv)
 941 {
 942         struct s5p_jpeg_ctx *ctx = priv;
 943         struct s5p_jpeg *jpeg = ctx->jpeg;
 944         struct vb2_buffer *src_buf, *dst_buf;
 945         unsigned long src_addr, dst_addr, flags;
 946
 947         spin_lock_irqsave(&ctx->jpeg->slock, flags);
 948
 949         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 950         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 951         src_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
 952         dst_addr = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
 953
 954         s5p_jpeg_reset(jpeg->regs);
 955         s5p_jpeg_poweron(jpeg->regs);
 956         s5p_jpeg_proc_mode(jpeg->regs, ctx->mode);
 957         if (ctx->mode == S5P_JPEG_ENCODE) {
 958                 if (ctx->out_q.fmt->fourcc == V4L2_PIX_FMT_RGB565)
 959                         s5p_jpeg_input_raw_mode(jpeg->regs,
 960                                                         S5P_JPEG_RAW_IN_565);
 961                 else
 962                         s5p_jpeg_input_raw_mode(jpeg->regs,
 963                                                         S5P_JPEG_RAW_IN_422);
 964                 s5p_jpeg_subsampling_mode(jpeg->regs, ctx->subsampling);
 965                 s5p_jpeg_dri(jpeg->regs, ctx->restart_interval);
 966                 s5p_jpeg_x(jpeg->regs, ctx->out_q.w);
 967                 s5p_jpeg_y(jpeg->regs, ctx->out_q.h);
 968                 s5p_jpeg_imgadr(jpeg->regs, src_addr);
 969                 s5p_jpeg_jpgadr(jpeg->regs, dst_addr);
 970
 971                 /* ultimately comes from sizeimage from userspace */
 972                 s5p_jpeg_enc_stream_int(jpeg->regs, ctx->cap_q.size);
 973
 974                 /* JPEG RGB to YCbCr conversion matrix */
 975                 s5p_jpeg_coef(jpeg->regs, 1, 1, S5P_JPEG_COEF11);
 976                 s5p_jpeg_coef(jpeg->regs, 1, 2, S5P_JPEG_COEF12);
 977                 s5p_jpeg_coef(jpeg->regs, 1, 3, S5P_JPEG_COEF13);
 978                 s5p_jpeg_coef(jpeg->regs, 2, 1, S5P_JPEG_COEF21);
 979                 s5p_jpeg_coef(jpeg->regs, 2, 2, S5P_JPEG_COEF22);
 980                 s5p_jpeg_coef(jpeg->regs, 2, 3, S5P_JPEG_COEF23);
 981                 s5p_jpeg_coef(jpeg->regs, 3, 1, S5P_JPEG_COEF31);
 982                 s5p_jpeg_coef(jpeg->regs, 3, 2, S5P_JPEG_COEF32);
 983                 s5p_jpeg_coef(jpeg->regs, 3, 3, S5P_JPEG_COEF33);
 984
 985                 /*
 986                  * JPEG IP allows storing 4 quantization tables
 987                  * We fill table 0 for luma and table 1 for chroma
 988                  */
 989                 s5p_jpeg_set_qtbl_lum(jpeg->regs, ctx->compr_quality);
 990                 s5p_jpeg_set_qtbl_chr(jpeg->regs, ctx->compr_quality);
 991                 /* use table 0 for Y */
 992                 s5p_jpeg_qtbl(jpeg->regs, 1, 0);
 993                 /* use table 1 for Cb and Cr*/
 994                 s5p_jpeg_qtbl(jpeg->regs, 2, 1);
 995                 s5p_jpeg_qtbl(jpeg->regs, 3, 1);
 996
 997                 /* Y, Cb, Cr use Huffman table 0 */
 998                 s5p_jpeg_htbl_ac(jpeg->regs, 1);
 999                 s5p_jpeg_htbl_dc(jpeg->regs, 1);
1000                 s5p_jpeg_htbl_ac(jpeg->regs, 2);
1001                 s5p_jpeg_htbl_dc(jpeg->regs, 2);
1002                 s5p_jpeg_htbl_ac(jpeg->regs, 3);
1003                 s5p_jpeg_htbl_dc(jpeg->regs, 3);
1004         } else { /* S5P_JPEG_DECODE */
1005                 s5p_jpeg_rst_int_enable(jpeg->regs, true);
1006                 s5p_jpeg_data_num_int_enable(jpeg->regs, true);
1007                 s5p_jpeg_final_mcu_num_int_enable(jpeg->regs, true);
1008                 if (ctx->cap_q.fmt->fourcc == V4L2_PIX_FMT_YUYV)
1009                         s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_422);
1010                 else
1011                         s5p_jpeg_outform_raw(jpeg->regs, S5P_JPEG_RAW_OUT_420);
1012                 s5p_jpeg_jpgadr(jpeg->regs, src_addr);
1013                 s5p_jpeg_imgadr(jpeg->regs, dst_addr);
1014         }
1015
1016         s5p_jpeg_start(jpeg->regs);
1017
1018         spin_unlock_irqrestore(&ctx->jpeg->slock, flags);
1019 }
1020
1021 static int s5p_jpeg_job_ready(void *priv)
1022 {
1023         struct s5p_jpeg_ctx *ctx = priv;
1024
1025         if (ctx->mode == S5P_JPEG_DECODE)
1026                 return ctx->hdr_parsed;
1027         return 1;
1028 }
1029
1030 static void s5p_jpeg_job_abort(void *priv)
1031 {
1032 }
1033
1034 static struct v4l2_m2m_ops s5p_jpeg_m2m_ops = {
1035         .device_run     = s5p_jpeg_device_run,
1036         .job_ready      = s5p_jpeg_job_ready,
1037         .job_abort      = s5p_jpeg_job_abort,
1038 };
1039
1040 /*
1041  * ============================================================================
1042  * Queue operations
1043  * ============================================================================
1044  */
1045
1046 static int s5p_jpeg_queue_setup(struct vb2_queue *vq,
1047                            const struct v4l2_format *fmt,
1048                            unsigned int *nbuffers, unsigned int *nplanes,
1049                            unsigned int sizes[], void *alloc_ctxs[])
1050 {
1051         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vq);
1052         struct s5p_jpeg_q_data *q_data = NULL;
1053         unsigned int size, count = *nbuffers;
1054
1055         q_data = get_q_data(ctx, vq->type);
1056         BUG_ON(q_data == NULL);
1057
1058         size = q_data->size;
1059
1060         /*
1061          * header is parsed during decoding and parsed information stored
1062          * in the context so we do not allow another buffer to overwrite it
1063          */
1064         if (ctx->mode == S5P_JPEG_DECODE)
1065                 count = 1;
1066
1067         *nbuffers = count;
1068         *nplanes = 1;
1069         sizes[0] = size;
1070         alloc_ctxs[0] = ctx->jpeg->alloc_ctx;
1071
1072         return 0;
1073 }
1074
1075 static int s5p_jpeg_buf_prepare(struct vb2_buffer *vb)
1076 {
1077         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1078         struct s5p_jpeg_q_data *q_data = NULL;
1079
1080         q_data = get_q_data(ctx, vb->vb2_queue->type);
1081         BUG_ON(q_data == NULL);
1082
1083         if (vb2_plane_size(vb, 0) < q_data->size) {
1084                 pr_err("%s data will not fit into plane (%lu < %lu)\n",
1085                                 __func__, vb2_plane_size(vb, 0),
1086                                 (long)q_data->size);
1087                 return -EINVAL;
1088         }
1089
1090         vb2_set_plane_payload(vb, 0, q_data->size);
1091
1092         return 0;
1093 }
1094
1095 static void s5p_jpeg_buf_queue(struct vb2_buffer *vb)
1096 {
1097         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1098
1099         if (ctx->mode == S5P_JPEG_DECODE &&
1100             vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
1101                 struct s5p_jpeg_q_data tmp, *q_data;
1102                 ctx->hdr_parsed = s5p_jpeg_parse_hdr(&tmp,
1103                      (unsigned long)vb2_plane_vaddr(vb, 0),
1104                      min((unsigned long)ctx->out_q.size,
1105                          vb2_get_plane_payload(vb, 0)));
1106                 if (!ctx->hdr_parsed) {
1107                         vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
1108                         return;
1109                 }
1110
1111                 q_data = &ctx->out_q;
1112                 q_data->w = tmp.w;
1113                 q_data->h = tmp.h;
1114
1115                 q_data = &ctx->cap_q;
1116                 q_data->w = tmp.w;
1117                 q_data->h = tmp.h;
1118         }
1119
1120         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
1121 }
1122
1123 static int s5p_jpeg_start_streaming(struct vb2_queue *q, unsigned int count)
1124 {
1125         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1126         int ret;
1127
1128         ret = pm_runtime_get_sync(ctx->jpeg->dev);
1129
1130         return ret > 0 ? 0 : ret;
1131 }
1132
1133 static int s5p_jpeg_stop_streaming(struct vb2_queue *q)
1134 {
1135         struct s5p_jpeg_ctx *ctx = vb2_get_drv_priv(q);
1136
1137         pm_runtime_put(ctx->jpeg->dev);
1138
1139         return 0;
1140 }
1141
1142 static struct vb2_ops s5p_jpeg_qops = {
1143         .queue_setup            = s5p_jpeg_queue_setup,
1144         .buf_prepare            = s5p_jpeg_buf_prepare,
1145         .buf_queue              = s5p_jpeg_buf_queue,
1146         .wait_prepare           = vb2_ops_wait_prepare,
1147         .wait_finish            = vb2_ops_wait_finish,
1148         .start_streaming        = s5p_jpeg_start_streaming,
1149         .stop_streaming         = s5p_jpeg_stop_streaming,
1150 };
1151
1152 static int queue_init(void *priv, struct vb2_queue *src_vq,
1153                       struct vb2_queue *dst_vq)
1154 {
1155         struct s5p_jpeg_ctx *ctx = priv;
1156         int ret;
1157
1158         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
1159         src_vq->io_modes = VB2_MMAP | VB2_USERPTR;
1160         src_vq->drv_priv = ctx;
1161         src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1162         src_vq->ops = &s5p_jpeg_qops;
1163         src_vq->mem_ops = &vb2_dma_contig_memops;
1164         src_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1165         src_vq->lock = &ctx->jpeg->lock;
1166
1167         ret = vb2_queue_init(src_vq);
1168         if (ret)
1169                 return ret;
1170
1171         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1172         dst_vq->io_modes = VB2_MMAP | VB2_USERPTR;
1173         dst_vq->drv_priv = ctx;
1174         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1175         dst_vq->ops = &s5p_jpeg_qops;
1176         dst_vq->mem_ops = &vb2_dma_contig_memops;
1177         dst_vq->timestamp_type = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1178         dst_vq->lock = &ctx->jpeg->lock;
1179
1180         return vb2_queue_init(dst_vq);
1181 }
1182
1183 /*
1184  * ============================================================================
1185  * ISR
1186  * ============================================================================
1187  */
1188
1189 static irqreturn_t s5p_jpeg_irq(int irq, void *dev_id)
1190 {
1191         struct s5p_jpeg *jpeg = dev_id;
1192         struct s5p_jpeg_ctx *curr_ctx;
1193         struct vb2_buffer *src_buf, *dst_buf;
1194         unsigned long payload_size = 0;
1195         enum vb2_buffer_state state = VB2_BUF_STATE_DONE;
1196         bool enc_jpeg_too_large = false;
1197         bool timer_elapsed = false;
1198         bool op_completed = false;
1199
1200         spin_lock(&jpeg->slock);
1201
1202         curr_ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1203
1204         src_buf = v4l2_m2m_src_buf_remove(curr_ctx->fh.m2m_ctx);
1205         dst_buf = v4l2_m2m_dst_buf_remove(curr_ctx->fh.m2m_ctx);
1206
1207         if (curr_ctx->mode == S5P_JPEG_ENCODE)
1208                 enc_jpeg_too_large = s5p_jpeg_enc_stream_stat(jpeg->regs);
1209         timer_elapsed = s5p_jpeg_timer_stat(jpeg->regs);
1210         op_completed = s5p_jpeg_result_stat_ok(jpeg->regs);
1211         if (curr_ctx->mode == S5P_JPEG_DECODE)
1212                 op_completed = op_completed &&
1213                                         s5p_jpeg_stream_stat_ok(jpeg->regs);
1214
1215         if (enc_jpeg_too_large) {
1216                 state = VB2_BUF_STATE_ERROR;
1217                 s5p_jpeg_clear_enc_stream_stat(jpeg->regs);
1218         } else if (timer_elapsed) {
1219                 state = VB2_BUF_STATE_ERROR;
1220                 s5p_jpeg_clear_timer_stat(jpeg->regs);
1221         } else if (!op_completed) {
1222                 state = VB2_BUF_STATE_ERROR;
1223         } else {
1224                 payload_size = s5p_jpeg_compressed_size(jpeg->regs);
1225         }
1226
1227         dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
1228         dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
1229
1230         v4l2_m2m_buf_done(src_buf, state);
1231         if (curr_ctx->mode == S5P_JPEG_ENCODE)
1232                 vb2_set_plane_payload(dst_buf, 0, payload_size);
1233         v4l2_m2m_buf_done(dst_buf, state);
1234         v4l2_m2m_job_finish(jpeg->m2m_dev, curr_ctx->fh.m2m_ctx);
1235
1236         curr_ctx->subsampling = s5p_jpeg_get_subsampling_mode(jpeg->regs);
1237         spin_unlock(&jpeg->slock);
1238
1239         s5p_jpeg_clear_int(jpeg->regs);
1240
1241         return IRQ_HANDLED;
1242 }
1243
1244 /*
1245  * ============================================================================
1246  * Driver basic infrastructure
1247  * ============================================================================
1248  */
1249
1250 static int s5p_jpeg_probe(struct platform_device *pdev)
1251 {
1252         struct s5p_jpeg *jpeg;
1253         struct resource *res;
1254         int ret;
1255
1256         /* JPEG IP abstraction struct */
1257         jpeg = devm_kzalloc(&pdev->dev, sizeof(struct s5p_jpeg), GFP_KERNEL);
1258         if (!jpeg)
1259                 return -ENOMEM;
1260
1261         mutex_init(&jpeg->lock);
1262         spin_lock_init(&jpeg->slock);
1263         jpeg->dev = &pdev->dev;
1264
1265         /* memory-mapped registers */
1266         res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1267
1268         jpeg->regs = devm_ioremap_resource(&pdev->dev, res);
1269         if (IS_ERR(jpeg->regs))
1270                 return PTR_ERR(jpeg->regs);
1271
1272         /* interrupt service routine registration */
1273         jpeg->irq = ret = platform_get_irq(pdev, 0);
1274         if (ret < 0) {
1275                 dev_err(&pdev->dev, "cannot find IRQ\n");
1276                 return ret;
1277         }
1278
1279         ret = devm_request_irq(&pdev->dev, jpeg->irq, s5p_jpeg_irq, 0,
1280                         dev_name(&pdev->dev), jpeg);
1281         if (ret) {
1282                 dev_err(&pdev->dev, "cannot claim IRQ %d\n", jpeg->irq);
1283                 return ret;
1284         }
1285
1286         /* clocks */
1287         jpeg->clk = clk_get(&pdev->dev, "jpeg");
1288         if (IS_ERR(jpeg->clk)) {
1289                 dev_err(&pdev->dev, "cannot get clock\n");
1290                 ret = PTR_ERR(jpeg->clk);
1291                 return ret;
1292         }
1293         dev_dbg(&pdev->dev, "clock source %p\n", jpeg->clk);
1294
1295         /* v4l2 device */
1296         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
1297         if (ret) {
1298                 dev_err(&pdev->dev, "Failed to register v4l2 device\n");
1299                 goto clk_get_rollback;
1300         }
1301
1302         /* mem2mem device */
1303         jpeg->m2m_dev = v4l2_m2m_init(&s5p_jpeg_m2m_ops);
1304         if (IS_ERR(jpeg->m2m_dev)) {
1305                 v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
1306                 ret = PTR_ERR(jpeg->m2m_dev);
1307                 goto device_register_rollback;
1308         }
1309
1310         jpeg->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
1311         if (IS_ERR(jpeg->alloc_ctx)) {
1312                 v4l2_err(&jpeg->v4l2_dev, "Failed to init memory allocator\n");
1313                 ret = PTR_ERR(jpeg->alloc_ctx);
1314                 goto m2m_init_rollback;
1315         }
1316
1317         /* JPEG encoder /dev/videoX node */
1318         jpeg->vfd_encoder = video_device_alloc();
1319         if (!jpeg->vfd_encoder) {
1320                 v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
1321                 ret = -ENOMEM;
1322                 goto vb2_allocator_rollback;
1323         }
1324         snprintf(jpeg->vfd_encoder->name, sizeof(jpeg->vfd_encoder->name),
1325                                 "%s-enc", S5P_JPEG_M2M_NAME);
1326         jpeg->vfd_encoder->fops         = &s5p_jpeg_fops;
1327         jpeg->vfd_encoder->ioctl_ops    = &s5p_jpeg_ioctl_ops;
1328         jpeg->vfd_encoder->minor        = -1;
1329         jpeg->vfd_encoder->release      = video_device_release;
1330         jpeg->vfd_encoder->lock         = &jpeg->lock;
1331         jpeg->vfd_encoder->v4l2_dev     = &jpeg->v4l2_dev;
1332         jpeg->vfd_encoder->vfl_dir      = VFL_DIR_M2M;
1333
1334         ret = video_register_device(jpeg->vfd_encoder, VFL_TYPE_GRABBER, -1);
1335         if (ret) {
1336                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1337                 goto enc_vdev_alloc_rollback;
1338         }
1339
1340         video_set_drvdata(jpeg->vfd_encoder, jpeg);
1341         v4l2_info(&jpeg->v4l2_dev,
1342                   "encoder device registered as /dev/video%d\n",
1343                   jpeg->vfd_encoder->num);
1344
1345         /* JPEG decoder /dev/videoX node */
1346         jpeg->vfd_decoder = video_device_alloc();
1347         if (!jpeg->vfd_decoder) {
1348                 v4l2_err(&jpeg->v4l2_dev, "Failed to allocate video device\n");
1349                 ret = -ENOMEM;
1350                 goto enc_vdev_register_rollback;
1351         }
1352         snprintf(jpeg->vfd_decoder->name, sizeof(jpeg->vfd_decoder->name),
1353                                 "%s-dec", S5P_JPEG_M2M_NAME);
1354         jpeg->vfd_decoder->fops         = &s5p_jpeg_fops;
1355         jpeg->vfd_decoder->ioctl_ops    = &s5p_jpeg_ioctl_ops;
1356         jpeg->vfd_decoder->minor        = -1;
1357         jpeg->vfd_decoder->release      = video_device_release;
1358         jpeg->vfd_decoder->lock         = &jpeg->lock;
1359         jpeg->vfd_decoder->v4l2_dev     = &jpeg->v4l2_dev;
1360         jpeg->vfd_decoder->vfl_dir      = VFL_DIR_M2M;
1361
1362         ret = video_register_device(jpeg->vfd_decoder, VFL_TYPE_GRABBER, -1);
1363         if (ret) {
1364                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1365                 goto dec_vdev_alloc_rollback;
1366         }
1367
1368         video_set_drvdata(jpeg->vfd_decoder, jpeg);
1369         v4l2_info(&jpeg->v4l2_dev,
1370                   "decoder device registered as /dev/video%d\n",
1371                   jpeg->vfd_decoder->num);
1372
1373         /* final statements & power management */
1374         platform_set_drvdata(pdev, jpeg);
1375
1376         pm_runtime_enable(&pdev->dev);
1377
1378         v4l2_info(&jpeg->v4l2_dev, "Samsung S5P JPEG codec\n");
1379
1380         return 0;
1381
1382 dec_vdev_alloc_rollback:
1383         video_device_release(jpeg->vfd_decoder);
1384
1385 enc_vdev_register_rollback:
1386         video_unregister_device(jpeg->vfd_encoder);
1387
1388 enc_vdev_alloc_rollback:
1389         video_device_release(jpeg->vfd_encoder);
1390
1391 vb2_allocator_rollback:
1392         vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
1393
1394 m2m_init_rollback:
1395         v4l2_m2m_release(jpeg->m2m_dev);
1396
1397 device_register_rollback:
1398         v4l2_device_unregister(&jpeg->v4l2_dev);
1399
1400 clk_get_rollback:
1401         clk_put(jpeg->clk);
1402
1403         return ret;
1404 }
1405
1406 static int s5p_jpeg_remove(struct platform_device *pdev)
1407 {
1408         struct s5p_jpeg *jpeg = platform_get_drvdata(pdev);
1409
1410         pm_runtime_disable(jpeg->dev);
1411
1412         video_unregister_device(jpeg->vfd_decoder);
1413         video_device_release(jpeg->vfd_decoder);
1414         video_unregister_device(jpeg->vfd_encoder);
1415         video_device_release(jpeg->vfd_encoder);
1416         vb2_dma_contig_cleanup_ctx(jpeg->alloc_ctx);
1417         v4l2_m2m_release(jpeg->m2m_dev);
1418         v4l2_device_unregister(&jpeg->v4l2_dev);
1419
1420         if (!pm_runtime_status_suspended(&pdev->dev))
1421                 clk_disable_unprepare(jpeg->clk);
1422
1423         clk_put(jpeg->clk);
1424
1425         return 0;
1426 }
1427
1428 static int s5p_jpeg_runtime_suspend(struct device *dev)
1429 {
1430         struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
1431
1432         clk_disable_unprepare(jpeg->clk);
1433
1434         return 0;
1435 }
1436
1437 static int s5p_jpeg_runtime_resume(struct device *dev)
1438 {
1439         struct s5p_jpeg *jpeg = dev_get_drvdata(dev);
1440         unsigned long flags;
1441         int ret;
1442
1443         ret = clk_prepare_enable(jpeg->clk);
1444         if (ret < 0)
1445                 return ret;
1446
1447         spin_lock_irqsave(&jpeg->slock, flags);
1448
1449         /*
1450          * JPEG IP allows storing two Huffman tables for each component
1451          * We fill table 0 for each component
1452          */
1453         s5p_jpeg_set_hdctbl(jpeg->regs);
1454         s5p_jpeg_set_hdctblg(jpeg->regs);
1455         s5p_jpeg_set_hactbl(jpeg->regs);
1456         s5p_jpeg_set_hactblg(jpeg->regs);
1457
1458         spin_unlock_irqrestore(&jpeg->slock, flags);
1459
1460         return 0;
1461 }
1462
1463 static int s5p_jpeg_suspend(struct device *dev)
1464 {
1465         if (pm_runtime_suspended(dev))
1466                 return 0;
1467
1468         return s5p_jpeg_runtime_suspend(dev);
1469 }
1470
1471 static int s5p_jpeg_resume(struct device *dev)
1472 {
1473         if (pm_runtime_suspended(dev))
1474                 return 0;
1475
1476         return s5p_jpeg_runtime_resume(dev);
1477 }
1478
1479 static const struct dev_pm_ops s5p_jpeg_pm_ops = {
1480         SET_SYSTEM_SLEEP_PM_OPS(s5p_jpeg_suspend, s5p_jpeg_resume)
1481         SET_RUNTIME_PM_OPS(s5p_jpeg_runtime_suspend, s5p_jpeg_runtime_resume, NULL)
1482 };
1483
1484 #ifdef CONFIG_OF
1485 static const struct of_device_id s5p_jpeg_of_match[] = {
1486         { .compatible = "samsung,s5pv210-jpeg" },
1487         { .compatible = "samsung,exynos4210-jpeg" },
1488         { /* sentinel */ },
1489 };
1490 MODULE_DEVICE_TABLE(of, s5p_jpeg_of_match);
1491 #endif
1492
1493 static struct platform_driver s5p_jpeg_driver = {
1494         .probe = s5p_jpeg_probe,
1495         .remove = s5p_jpeg_remove,
1496         .driver = {
1497                 .of_match_table = of_match_ptr(s5p_jpeg_of_match),
1498                 .owner = THIS_MODULE,
1499                 .name = S5P_JPEG_M2M_NAME,
1500                 .pm = &s5p_jpeg_pm_ops,
1501         },
1502 };
1503
1504 module_platform_driver(s5p_jpeg_driver);
1505
1506 MODULE_AUTHOR("Andrzej Pietrasiewicz <andrzej.p@samsung.com>");
1507 MODULE_DESCRIPTION("Samsung JPEG codec driver");
1508 MODULE_LICENSE("GPL");