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