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

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