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

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