Pileus Git - ~andy/linux/blob - drivers/media/platform/s5p-jpeg/jpeg-core.c

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