drivers/media/platform/mediatek/jpeg/mtk_jpeg_core.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2016 MediaTek Inc.
   4  * Author: Ming Hsiu Tsai <minghsiu.tsai@mediatek.com>
   5  *         Rick Chang <rick.chang@mediatek.com>
   6  *         Xia Jiang <xia.jiang@mediatek.com>
   7  */
   8
   9 #include <linux/clk.h>
  10 #include <linux/err.h>
  11 #include <linux/interrupt.h>
  12 #include <linux/io.h>
  13 #include <linux/kernel.h>
  14 #include <linux/module.h>
  15 #include <linux/of_platform.h>
  16 #include <linux/platform_device.h>
  17 #include <linux/pm_runtime.h>
  18 #include <linux/slab.h>
  19 #include <linux/spinlock.h>
  20 #include <media/v4l2-event.h>
  21 #include <media/v4l2-mem2mem.h>
  22 #include <media/v4l2-ioctl.h>
  23 #include <media/videobuf2-core.h>
  24 #include <media/videobuf2-dma-contig.h>
  25
  26 #include "mtk_jpeg_enc_hw.h"
  27 #include "mtk_jpeg_dec_hw.h"
  28 #include "mtk_jpeg_core.h"
  29 #include "mtk_jpeg_dec_parse.h"
  30
  31 static struct mtk_jpeg_fmt mtk_jpeg_enc_formats[] = {
  32         {
  33                 .fourcc         = V4L2_PIX_FMT_JPEG,
  34                 .colplanes      = 1,
  35                 .flags          = MTK_JPEG_FMT_FLAG_CAPTURE,
  36         },
  37         {
  38                 .fourcc         = V4L2_PIX_FMT_NV12M,
  39                 .hw_format      = JPEG_ENC_YUV_FORMAT_NV12,
  40                 .h_sample       = {4, 4},
  41                 .v_sample       = {4, 2},
  42                 .colplanes      = 2,
  43                 .h_align        = 4,
  44                 .v_align        = 4,
  45                 .flags          = MTK_JPEG_FMT_FLAG_OUTPUT,
  46         },
  47         {
  48                 .fourcc         = V4L2_PIX_FMT_NV21M,
  49                 .hw_format      = JEPG_ENC_YUV_FORMAT_NV21,
  50                 .h_sample       = {4, 4},
  51                 .v_sample       = {4, 2},
  52                 .colplanes      = 2,
  53                 .h_align        = 4,
  54                 .v_align        = 4,
  55                 .flags          = MTK_JPEG_FMT_FLAG_OUTPUT,
  56         },
  57         {
  58                 .fourcc         = V4L2_PIX_FMT_YUYV,
  59                 .hw_format      = JPEG_ENC_YUV_FORMAT_YUYV,
  60                 .h_sample       = {8},
  61                 .v_sample       = {4},
  62                 .colplanes      = 1,
  63                 .h_align        = 5,
  64                 .v_align        = 3,
  65                 .flags          = MTK_JPEG_FMT_FLAG_OUTPUT,
  66         },
  67         {
  68                 .fourcc         = V4L2_PIX_FMT_YVYU,
  69                 .hw_format      = JPEG_ENC_YUV_FORMAT_YVYU,
  70                 .h_sample       = {8},
  71                 .v_sample       = {4},
  72                 .colplanes      = 1,
  73                 .h_align        = 5,
  74                 .v_align        = 3,
  75                 .flags          = MTK_JPEG_FMT_FLAG_OUTPUT,
  76         },
  77 };
  78
  79 static struct mtk_jpeg_fmt mtk_jpeg_dec_formats[] = {
  80         {
  81                 .fourcc         = V4L2_PIX_FMT_JPEG,
  82                 .colplanes      = 1,
  83                 .flags          = MTK_JPEG_FMT_FLAG_OUTPUT,
  84         },
  85         {
  86                 .fourcc         = V4L2_PIX_FMT_YUV420M,
  87                 .h_sample       = {4, 2, 2},
  88                 .v_sample       = {4, 2, 2},
  89                 .colplanes      = 3,
  90                 .h_align        = 5,
  91                 .v_align        = 4,
  92                 .flags          = MTK_JPEG_FMT_FLAG_CAPTURE,
  93         },
  94         {
  95                 .fourcc         = V4L2_PIX_FMT_YUV422M,
  96                 .h_sample       = {4, 2, 2},
  97                 .v_sample       = {4, 4, 4},
  98                 .colplanes      = 3,
  99                 .h_align        = 5,
 100                 .v_align        = 3,
 101                 .flags          = MTK_JPEG_FMT_FLAG_CAPTURE,
 102         },
 103 };
 104
 105 #define MTK_JPEG_ENC_NUM_FORMATS ARRAY_SIZE(mtk_jpeg_enc_formats)
 106 #define MTK_JPEG_DEC_NUM_FORMATS ARRAY_SIZE(mtk_jpeg_dec_formats)
 107 #define MTK_JPEG_MAX_RETRY_TIME 5000
 108
 109 enum {
 110         MTK_JPEG_BUF_FLAGS_INIT                 = 0,
 111         MTK_JPEG_BUF_FLAGS_LAST_FRAME           = 1,
 112 };
 113
 114 static int debug;
 115 module_param(debug, int, 0644);
 116
 117 static inline struct mtk_jpeg_ctx *ctrl_to_ctx(struct v4l2_ctrl *ctrl)
 118 {
 119         return container_of(ctrl->handler, struct mtk_jpeg_ctx, ctrl_hdl);
 120 }
 121
 122 static inline struct mtk_jpeg_ctx *mtk_jpeg_fh_to_ctx(struct v4l2_fh *fh)
 123 {
 124         return container_of(fh, struct mtk_jpeg_ctx, fh);
 125 }
 126
 127 static inline struct mtk_jpeg_src_buf *mtk_jpeg_vb2_to_srcbuf(
 128                                                         struct vb2_buffer *vb)
 129 {
 130         return container_of(to_vb2_v4l2_buffer(vb), struct mtk_jpeg_src_buf, b);
 131 }
 132
 133 static int mtk_jpeg_querycap(struct file *file, void *priv,
 134                              struct v4l2_capability *cap)
 135 {
 136         struct mtk_jpeg_dev *jpeg = video_drvdata(file);
 137
 138         strscpy(cap->driver, jpeg->variant->dev_name, sizeof(cap->driver));
 139         strscpy(cap->card, jpeg->variant->dev_name, sizeof(cap->card));
 140
 141         return 0;
 142 }
 143
 144 static int vidioc_jpeg_enc_s_ctrl(struct v4l2_ctrl *ctrl)
 145 {
 146         struct mtk_jpeg_ctx *ctx = ctrl_to_ctx(ctrl);
 147
 148         switch (ctrl->id) {
 149         case V4L2_CID_JPEG_RESTART_INTERVAL:
 150                 ctx->restart_interval = ctrl->val;
 151                 break;
 152         case V4L2_CID_JPEG_COMPRESSION_QUALITY:
 153                 ctx->enc_quality = ctrl->val;
 154                 break;
 155         case V4L2_CID_JPEG_ACTIVE_MARKER:
 156                 ctx->enable_exif = ctrl->val & V4L2_JPEG_ACTIVE_MARKER_APP1;
 157                 break;
 158         }
 159
 160         return 0;
 161 }
 162
 163 static const struct v4l2_ctrl_ops mtk_jpeg_enc_ctrl_ops = {
 164         .s_ctrl = vidioc_jpeg_enc_s_ctrl,
 165 };
 166
 167 static int mtk_jpeg_enc_ctrls_setup(struct mtk_jpeg_ctx *ctx)
 168 {
 169         const struct v4l2_ctrl_ops *ops = &mtk_jpeg_enc_ctrl_ops;
 170         struct v4l2_ctrl_handler *handler = &ctx->ctrl_hdl;
 171
 172         v4l2_ctrl_handler_init(handler, 3);
 173
 174         v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_RESTART_INTERVAL, 0, 100,
 175                           1, 0);
 176         v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_COMPRESSION_QUALITY, 48,
 177                           100, 1, 90);
 178         v4l2_ctrl_new_std(handler, ops, V4L2_CID_JPEG_ACTIVE_MARKER, 0,
 179                           V4L2_JPEG_ACTIVE_MARKER_APP1, 0, 0);
 180
 181         if (handler->error) {
 182                 v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
 183                 return handler->error;
 184         }
 185
 186         v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
 187
 188         return 0;
 189 }
 190
 191 static int mtk_jpeg_enum_fmt(struct mtk_jpeg_fmt *mtk_jpeg_formats, int n,
 192                              struct v4l2_fmtdesc *f, u32 type)
 193 {
 194         int i, num = 0;
 195
 196         for (i = 0; i < n; ++i) {
 197                 if (mtk_jpeg_formats[i].flags & type) {
 198                         if (num == f->index)
 199                                 break;
 200                         ++num;
 201                 }
 202         }
 203
 204         if (i >= n)
 205                 return -EINVAL;
 206
 207         f->pixelformat = mtk_jpeg_formats[i].fourcc;
 208
 209         return 0;
 210 }
 211
 212 static int mtk_jpeg_enum_fmt_vid_cap(struct file *file, void *priv,
 213                                      struct v4l2_fmtdesc *f)
 214 {
 215         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 216         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 217
 218         return mtk_jpeg_enum_fmt(jpeg->variant->formats,
 219                                  jpeg->variant->num_formats, f,
 220                                  MTK_JPEG_FMT_FLAG_CAPTURE);
 221 }
 222
 223 static int mtk_jpeg_enum_fmt_vid_out(struct file *file, void *priv,
 224                                      struct v4l2_fmtdesc *f)
 225 {
 226         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 227         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 228
 229         return mtk_jpeg_enum_fmt(jpeg->variant->formats,
 230                                  jpeg->variant->num_formats, f,
 231                                  MTK_JPEG_FMT_FLAG_OUTPUT);
 232 }
 233
 234 static struct mtk_jpeg_q_data *mtk_jpeg_get_q_data(struct mtk_jpeg_ctx *ctx,
 235                                                    enum v4l2_buf_type type)
 236 {
 237         if (V4L2_TYPE_IS_OUTPUT(type))
 238                 return &ctx->out_q;
 239         return &ctx->cap_q;
 240 }
 241
 242 static struct mtk_jpeg_fmt *
 243 mtk_jpeg_find_format(struct mtk_jpeg_fmt *mtk_jpeg_formats, int num_formats,
 244                      u32 pixelformat, unsigned int fmt_type)
 245 {
 246         unsigned int k;
 247         struct mtk_jpeg_fmt *fmt;
 248
 249         for (k = 0; k < num_formats; k++) {
 250                 fmt = &mtk_jpeg_formats[k];
 251
 252                 if (fmt->fourcc == pixelformat && fmt->flags & fmt_type)
 253                         return fmt;
 254         }
 255
 256         return NULL;
 257 }
 258
 259 static int mtk_jpeg_try_fmt_mplane(struct v4l2_pix_format_mplane *pix_mp,
 260                                    struct mtk_jpeg_fmt *fmt)
 261 {
 262         int i;
 263
 264         pix_mp->field = V4L2_FIELD_NONE;
 265
 266         pix_mp->num_planes = fmt->colplanes;
 267         pix_mp->pixelformat = fmt->fourcc;
 268
 269         if (fmt->fourcc == V4L2_PIX_FMT_JPEG) {
 270                 struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[0];
 271
 272                 pix_mp->height = clamp(pix_mp->height, MTK_JPEG_MIN_HEIGHT,
 273                                        MTK_JPEG_MAX_HEIGHT);
 274                 pix_mp->width = clamp(pix_mp->width, MTK_JPEG_MIN_WIDTH,
 275                                       MTK_JPEG_MAX_WIDTH);
 276
 277                 pfmt->bytesperline = 0;
 278                 /* Source size must be aligned to 128 */
 279                 pfmt->sizeimage = round_up(pfmt->sizeimage, 128);
 280                 if (pfmt->sizeimage == 0)
 281                         pfmt->sizeimage = MTK_JPEG_DEFAULT_SIZEIMAGE;
 282                 return 0;
 283         }
 284
 285         /* other fourcc */
 286         pix_mp->height = clamp(round_up(pix_mp->height, fmt->v_align),
 287                                MTK_JPEG_MIN_HEIGHT, MTK_JPEG_MAX_HEIGHT);
 288         pix_mp->width = clamp(round_up(pix_mp->width, fmt->h_align),
 289                               MTK_JPEG_MIN_WIDTH, MTK_JPEG_MAX_WIDTH);
 290
 291         for (i = 0; i < fmt->colplanes; i++) {
 292                 struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
 293                 u32 stride = pix_mp->width * fmt->h_sample[i] / 4;
 294                 u32 h = pix_mp->height * fmt->v_sample[i] / 4;
 295
 296                 pfmt->bytesperline = stride;
 297                 pfmt->sizeimage = stride * h;
 298         }
 299         return 0;
 300 }
 301
 302 static int mtk_jpeg_g_fmt_vid_mplane(struct file *file, void *priv,
 303                                      struct v4l2_format *f)
 304 {
 305         struct vb2_queue *vq;
 306         struct mtk_jpeg_q_data *q_data = NULL;
 307         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 308         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 309         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 310         int i;
 311
 312         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 313         if (!vq)
 314                 return -EINVAL;
 315
 316         q_data = mtk_jpeg_get_q_data(ctx, f->type);
 317
 318         pix_mp->width = q_data->pix_mp.width;
 319         pix_mp->height = q_data->pix_mp.height;
 320         pix_mp->field = V4L2_FIELD_NONE;
 321         pix_mp->pixelformat = q_data->fmt->fourcc;
 322         pix_mp->num_planes = q_data->fmt->colplanes;
 323         pix_mp->colorspace = q_data->pix_mp.colorspace;
 324         pix_mp->ycbcr_enc = q_data->pix_mp.ycbcr_enc;
 325         pix_mp->xfer_func = q_data->pix_mp.xfer_func;
 326         pix_mp->quantization = q_data->pix_mp.quantization;
 327
 328         v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) g_fmt:%c%c%c%c wxh:%ux%u\n",
 329                  f->type,
 330                  (pix_mp->pixelformat & 0xff),
 331                  (pix_mp->pixelformat >>  8 & 0xff),
 332                  (pix_mp->pixelformat >> 16 & 0xff),
 333                  (pix_mp->pixelformat >> 24 & 0xff),
 334                  pix_mp->width, pix_mp->height);
 335
 336         for (i = 0; i < pix_mp->num_planes; i++) {
 337                 struct v4l2_plane_pix_format *pfmt = &pix_mp->plane_fmt[i];
 338
 339                 pfmt->bytesperline = q_data->pix_mp.plane_fmt[i].bytesperline;
 340                 pfmt->sizeimage = q_data->pix_mp.plane_fmt[i].sizeimage;
 341
 342                 v4l2_dbg(1, debug, &jpeg->v4l2_dev,
 343                          "plane[%d] bpl=%u, size=%u\n",
 344                          i,
 345                          pfmt->bytesperline,
 346                          pfmt->sizeimage);
 347         }
 348         return 0;
 349 }
 350
 351 static int mtk_jpeg_try_fmt_vid_cap_mplane(struct file *file, void *priv,
 352                                            struct v4l2_format *f)
 353 {
 354         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 355         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 356         struct mtk_jpeg_fmt *fmt;
 357
 358         fmt = mtk_jpeg_find_format(jpeg->variant->formats,
 359                                    jpeg->variant->num_formats,
 360                                    f->fmt.pix_mp.pixelformat,
 361                                    MTK_JPEG_FMT_FLAG_CAPTURE);
 362         if (!fmt)
 363                 fmt = ctx->cap_q.fmt;
 364
 365         v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
 366                  f->type,
 367                  (fmt->fourcc & 0xff),
 368                  (fmt->fourcc >>  8 & 0xff),
 369                  (fmt->fourcc >> 16 & 0xff),
 370                  (fmt->fourcc >> 24 & 0xff));
 371
 372         if (ctx->state != MTK_JPEG_INIT) {
 373                 mtk_jpeg_g_fmt_vid_mplane(file, priv, f);
 374                 return 0;
 375         }
 376
 377         return mtk_jpeg_try_fmt_mplane(&f->fmt.pix_mp, fmt);
 378 }
 379
 380 static int mtk_jpeg_try_fmt_vid_out_mplane(struct file *file, void *priv,
 381                                            struct v4l2_format *f)
 382 {
 383         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 384         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 385         struct mtk_jpeg_fmt *fmt;
 386
 387         fmt = mtk_jpeg_find_format(jpeg->variant->formats,
 388                                    jpeg->variant->num_formats,
 389                                    f->fmt.pix_mp.pixelformat,
 390                                    MTK_JPEG_FMT_FLAG_OUTPUT);
 391         if (!fmt)
 392                 fmt = ctx->out_q.fmt;
 393
 394         v4l2_dbg(2, debug, &ctx->jpeg->v4l2_dev, "(%d) try_fmt:%c%c%c%c\n",
 395                  f->type,
 396                  (fmt->fourcc & 0xff),
 397                  (fmt->fourcc >>  8 & 0xff),
 398                  (fmt->fourcc >> 16 & 0xff),
 399                  (fmt->fourcc >> 24 & 0xff));
 400
 401         if (ctx->state != MTK_JPEG_INIT) {
 402                 mtk_jpeg_g_fmt_vid_mplane(file, priv, f);
 403                 return 0;
 404         }
 405
 406         return mtk_jpeg_try_fmt_mplane(&f->fmt.pix_mp, fmt);
 407 }
 408
 409 static int mtk_jpeg_s_fmt_mplane(struct mtk_jpeg_ctx *ctx,
 410                                  struct v4l2_format *f, unsigned int fmt_type)
 411 {
 412         struct vb2_queue *vq;
 413         struct mtk_jpeg_q_data *q_data = NULL;
 414         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 415         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 416         int i;
 417
 418         vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
 419         if (!vq)
 420                 return -EINVAL;
 421
 422         q_data = mtk_jpeg_get_q_data(ctx, f->type);
 423
 424         if (vb2_is_busy(vq)) {
 425                 v4l2_err(&jpeg->v4l2_dev, "queue busy\n");
 426                 return -EBUSY;
 427         }
 428
 429         q_data->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
 430                                            jpeg->variant->num_formats,
 431                                            pix_mp->pixelformat, fmt_type);
 432         q_data->pix_mp.width = pix_mp->width;
 433         q_data->pix_mp.height = pix_mp->height;
 434         q_data->enc_crop_rect.width = pix_mp->width;
 435         q_data->enc_crop_rect.height = pix_mp->height;
 436         q_data->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
 437         q_data->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
 438         q_data->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
 439         q_data->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
 440
 441         v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) s_fmt:%c%c%c%c wxh:%ux%u\n",
 442                  f->type,
 443                  (q_data->fmt->fourcc & 0xff),
 444                  (q_data->fmt->fourcc >>  8 & 0xff),
 445                  (q_data->fmt->fourcc >> 16 & 0xff),
 446                  (q_data->fmt->fourcc >> 24 & 0xff),
 447                  q_data->pix_mp.width, q_data->pix_mp.height);
 448
 449         for (i = 0; i < q_data->fmt->colplanes; i++) {
 450                 q_data->pix_mp.plane_fmt[i].bytesperline =
 451                                         pix_mp->plane_fmt[i].bytesperline;
 452                 q_data->pix_mp.plane_fmt[i].sizeimage =
 453                                         pix_mp->plane_fmt[i].sizeimage;
 454
 455                 v4l2_dbg(1, debug, &jpeg->v4l2_dev,
 456                          "plane[%d] bpl=%u, size=%u\n",
 457                          i, q_data->pix_mp.plane_fmt[i].bytesperline,
 458                          q_data->pix_mp.plane_fmt[i].sizeimage);
 459         }
 460
 461         return 0;
 462 }
 463
 464 static int mtk_jpeg_s_fmt_vid_out_mplane(struct file *file, void *priv,
 465                                          struct v4l2_format *f)
 466 {
 467         int ret;
 468
 469         ret = mtk_jpeg_try_fmt_vid_out_mplane(file, priv, f);
 470         if (ret)
 471                 return ret;
 472
 473         return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f,
 474                                      MTK_JPEG_FMT_FLAG_OUTPUT);
 475 }
 476
 477 static int mtk_jpeg_s_fmt_vid_cap_mplane(struct file *file, void *priv,
 478                                          struct v4l2_format *f)
 479 {
 480         int ret;
 481
 482         ret = mtk_jpeg_try_fmt_vid_cap_mplane(file, priv, f);
 483         if (ret)
 484                 return ret;
 485
 486         return mtk_jpeg_s_fmt_mplane(mtk_jpeg_fh_to_ctx(priv), f,
 487                                      MTK_JPEG_FMT_FLAG_CAPTURE);
 488 }
 489
 490 static void mtk_jpeg_queue_src_chg_event(struct mtk_jpeg_ctx *ctx)
 491 {
 492         static const struct v4l2_event ev_src_ch = {
 493                 .type = V4L2_EVENT_SOURCE_CHANGE,
 494                 .u.src_change.changes =
 495                 V4L2_EVENT_SRC_CH_RESOLUTION,
 496         };
 497
 498         v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
 499 }
 500
 501 static int mtk_jpeg_subscribe_event(struct v4l2_fh *fh,
 502                                     const struct v4l2_event_subscription *sub)
 503 {
 504         switch (sub->type) {
 505         case V4L2_EVENT_SOURCE_CHANGE:
 506                 return v4l2_src_change_event_subscribe(fh, sub);
 507         }
 508
 509         return v4l2_ctrl_subscribe_event(fh, sub);
 510 }
 511
 512 static int mtk_jpeg_enc_g_selection(struct file *file, void *priv,
 513                                     struct v4l2_selection *s)
 514 {
 515         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 516
 517         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 518                 return -EINVAL;
 519
 520         switch (s->target) {
 521         case V4L2_SEL_TGT_CROP:
 522                 s->r = ctx->out_q.enc_crop_rect;
 523                 break;
 524         case V4L2_SEL_TGT_CROP_BOUNDS:
 525         case V4L2_SEL_TGT_CROP_DEFAULT:
 526                 s->r.width = ctx->out_q.pix_mp.width;
 527                 s->r.height = ctx->out_q.pix_mp.height;
 528                 s->r.left = 0;
 529                 s->r.top = 0;
 530                 break;
 531         default:
 532                 return -EINVAL;
 533         }
 534         return 0;
 535 }
 536
 537 static int mtk_jpeg_dec_g_selection(struct file *file, void *priv,
 538                                     struct v4l2_selection *s)
 539 {
 540         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 541
 542         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 543                 return -EINVAL;
 544
 545         switch (s->target) {
 546         case V4L2_SEL_TGT_COMPOSE:
 547         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 548                 s->r.width = ctx->out_q.pix_mp.width;
 549                 s->r.height = ctx->out_q.pix_mp.height;
 550                 s->r.left = 0;
 551                 s->r.top = 0;
 552                 break;
 553         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 554         case V4L2_SEL_TGT_COMPOSE_PADDED:
 555                 s->r.width = ctx->cap_q.pix_mp.width;
 556                 s->r.height = ctx->cap_q.pix_mp.height;
 557                 s->r.left = 0;
 558                 s->r.top = 0;
 559                 break;
 560         default:
 561                 return -EINVAL;
 562         }
 563         return 0;
 564 }
 565
 566 static int mtk_jpeg_enc_s_selection(struct file *file, void *priv,
 567                                     struct v4l2_selection *s)
 568 {
 569         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 570
 571         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 572                 return -EINVAL;
 573
 574         switch (s->target) {
 575         case V4L2_SEL_TGT_CROP:
 576                 s->r.left = 0;
 577                 s->r.top = 0;
 578                 s->r.width = min(s->r.width, ctx->out_q.pix_mp.width);
 579                 s->r.height = min(s->r.height, ctx->out_q.pix_mp.height);
 580                 ctx->out_q.enc_crop_rect = s->r;
 581                 break;
 582         default:
 583                 return -EINVAL;
 584         }
 585
 586         return 0;
 587 }
 588
 589 static int mtk_jpeg_qbuf(struct file *file, void *priv, struct v4l2_buffer *buf)
 590 {
 591         struct v4l2_fh *fh = file->private_data;
 592         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(priv);
 593         struct vb2_queue *vq;
 594         struct vb2_buffer *vb;
 595         struct mtk_jpeg_src_buf *jpeg_src_buf;
 596
 597         if (buf->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 598                 goto end;
 599
 600         vq = v4l2_m2m_get_vq(fh->m2m_ctx, buf->type);
 601         if (buf->index >= vq->num_buffers) {
 602                 dev_err(ctx->jpeg->dev, "buffer index out of range\n");
 603                 return -EINVAL;
 604         }
 605
 606         vb = vq->bufs[buf->index];
 607         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
 608         jpeg_src_buf->bs_size = buf->m.planes[0].bytesused;
 609
 610 end:
 611         return v4l2_m2m_qbuf(file, fh->m2m_ctx, buf);
 612 }
 613
 614 static const struct v4l2_ioctl_ops mtk_jpeg_enc_ioctl_ops = {
 615         .vidioc_querycap                = mtk_jpeg_querycap,
 616         .vidioc_enum_fmt_vid_cap        = mtk_jpeg_enum_fmt_vid_cap,
 617         .vidioc_enum_fmt_vid_out        = mtk_jpeg_enum_fmt_vid_out,
 618         .vidioc_try_fmt_vid_cap_mplane  = mtk_jpeg_try_fmt_vid_cap_mplane,
 619         .vidioc_try_fmt_vid_out_mplane  = mtk_jpeg_try_fmt_vid_out_mplane,
 620         .vidioc_g_fmt_vid_cap_mplane    = mtk_jpeg_g_fmt_vid_mplane,
 621         .vidioc_g_fmt_vid_out_mplane    = mtk_jpeg_g_fmt_vid_mplane,
 622         .vidioc_s_fmt_vid_cap_mplane    = mtk_jpeg_s_fmt_vid_cap_mplane,
 623         .vidioc_s_fmt_vid_out_mplane    = mtk_jpeg_s_fmt_vid_out_mplane,
 624         .vidioc_qbuf                    = v4l2_m2m_ioctl_qbuf,
 625         .vidioc_subscribe_event         = mtk_jpeg_subscribe_event,
 626         .vidioc_g_selection             = mtk_jpeg_enc_g_selection,
 627         .vidioc_s_selection             = mtk_jpeg_enc_s_selection,
 628
 629         .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
 630         .vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
 631         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 632         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 633         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 634         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 635         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 636         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 637
 638         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 639
 640         .vidioc_encoder_cmd             = v4l2_m2m_ioctl_encoder_cmd,
 641         .vidioc_try_encoder_cmd         = v4l2_m2m_ioctl_try_encoder_cmd,
 642 };
 643
 644 static const struct v4l2_ioctl_ops mtk_jpeg_dec_ioctl_ops = {
 645         .vidioc_querycap                = mtk_jpeg_querycap,
 646         .vidioc_enum_fmt_vid_cap        = mtk_jpeg_enum_fmt_vid_cap,
 647         .vidioc_enum_fmt_vid_out        = mtk_jpeg_enum_fmt_vid_out,
 648         .vidioc_try_fmt_vid_cap_mplane  = mtk_jpeg_try_fmt_vid_cap_mplane,
 649         .vidioc_try_fmt_vid_out_mplane  = mtk_jpeg_try_fmt_vid_out_mplane,
 650         .vidioc_g_fmt_vid_cap_mplane    = mtk_jpeg_g_fmt_vid_mplane,
 651         .vidioc_g_fmt_vid_out_mplane    = mtk_jpeg_g_fmt_vid_mplane,
 652         .vidioc_s_fmt_vid_cap_mplane    = mtk_jpeg_s_fmt_vid_cap_mplane,
 653         .vidioc_s_fmt_vid_out_mplane    = mtk_jpeg_s_fmt_vid_out_mplane,
 654         .vidioc_qbuf                    = mtk_jpeg_qbuf,
 655         .vidioc_subscribe_event         = mtk_jpeg_subscribe_event,
 656         .vidioc_g_selection             = mtk_jpeg_dec_g_selection,
 657
 658         .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
 659         .vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
 660         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 661         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 662         .vidioc_dqbuf                   = v4l2_m2m_ioctl_dqbuf,
 663         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 664         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 665         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 666
 667         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 668
 669         .vidioc_decoder_cmd = v4l2_m2m_ioctl_decoder_cmd,
 670         .vidioc_try_decoder_cmd = v4l2_m2m_ioctl_try_decoder_cmd,
 671 };
 672
 673 static int mtk_jpeg_queue_setup(struct vb2_queue *q,
 674                                 unsigned int *num_buffers,
 675                                 unsigned int *num_planes,
 676                                 unsigned int sizes[],
 677                                 struct device *alloc_ctxs[])
 678 {
 679         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
 680         struct mtk_jpeg_q_data *q_data = NULL;
 681         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 682         int i;
 683
 684         v4l2_dbg(1, debug, &jpeg->v4l2_dev, "(%d) buf_req count=%u\n",
 685                  q->type, *num_buffers);
 686
 687         q_data = mtk_jpeg_get_q_data(ctx, q->type);
 688         if (!q_data)
 689                 return -EINVAL;
 690
 691         if (*num_planes) {
 692                 for (i = 0; i < *num_planes; i++)
 693                         if (sizes[i] < q_data->pix_mp.plane_fmt[i].sizeimage)
 694                                 return -EINVAL;
 695                 return 0;
 696         }
 697
 698         *num_planes = q_data->fmt->colplanes;
 699         for (i = 0; i < q_data->fmt->colplanes; i++) {
 700                 sizes[i] =  q_data->pix_mp.plane_fmt[i].sizeimage;
 701                 v4l2_dbg(1, debug, &jpeg->v4l2_dev, "sizeimage[%d]=%u\n",
 702                          i, sizes[i]);
 703         }
 704
 705         return 0;
 706 }
 707
 708 static int mtk_jpeg_buf_prepare(struct vb2_buffer *vb)
 709 {
 710         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 711         struct mtk_jpeg_q_data *q_data = NULL;
 712         struct v4l2_plane_pix_format plane_fmt = {};
 713         int i;
 714
 715         q_data = mtk_jpeg_get_q_data(ctx, vb->vb2_queue->type);
 716         if (!q_data)
 717                 return -EINVAL;
 718
 719         for (i = 0; i < q_data->fmt->colplanes; i++) {
 720                 plane_fmt = q_data->pix_mp.plane_fmt[i];
 721                 if (ctx->enable_exif &&
 722                     q_data->fmt->fourcc == V4L2_PIX_FMT_JPEG)
 723                         vb2_set_plane_payload(vb, i, plane_fmt.sizeimage +
 724                                               MTK_JPEG_MAX_EXIF_SIZE);
 725                 else
 726                         vb2_set_plane_payload(vb, i,  plane_fmt.sizeimage);
 727         }
 728
 729         return 0;
 730 }
 731
 732 static bool mtk_jpeg_check_resolution_change(struct mtk_jpeg_ctx *ctx,
 733                                              struct mtk_jpeg_dec_param *param)
 734 {
 735         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 736         struct mtk_jpeg_q_data *q_data;
 737
 738         q_data = &ctx->out_q;
 739         if (q_data->pix_mp.width != param->pic_w ||
 740             q_data->pix_mp.height != param->pic_h) {
 741                 v4l2_dbg(1, debug, &jpeg->v4l2_dev, "Picture size change\n");
 742                 return true;
 743         }
 744
 745         q_data = &ctx->cap_q;
 746         if (q_data->fmt !=
 747             mtk_jpeg_find_format(jpeg->variant->formats,
 748                                  jpeg->variant->num_formats, param->dst_fourcc,
 749                                  MTK_JPEG_FMT_FLAG_CAPTURE)) {
 750                 v4l2_dbg(1, debug, &jpeg->v4l2_dev, "format change\n");
 751                 return true;
 752         }
 753         return false;
 754 }
 755
 756 static void mtk_jpeg_set_queue_data(struct mtk_jpeg_ctx *ctx,
 757                                     struct mtk_jpeg_dec_param *param)
 758 {
 759         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 760         struct mtk_jpeg_q_data *q_data;
 761         int i;
 762
 763         q_data = &ctx->out_q;
 764         q_data->pix_mp.width = param->pic_w;
 765         q_data->pix_mp.height = param->pic_h;
 766
 767         q_data = &ctx->cap_q;
 768         q_data->pix_mp.width = param->dec_w;
 769         q_data->pix_mp.height = param->dec_h;
 770         q_data->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
 771                                            jpeg->variant->num_formats,
 772                                            param->dst_fourcc,
 773                                            MTK_JPEG_FMT_FLAG_CAPTURE);
 774
 775         for (i = 0; i < q_data->fmt->colplanes; i++) {
 776                 q_data->pix_mp.plane_fmt[i].bytesperline = param->mem_stride[i];
 777                 q_data->pix_mp.plane_fmt[i].sizeimage = param->comp_size[i];
 778         }
 779
 780         v4l2_dbg(1, debug, &jpeg->v4l2_dev,
 781                  "set_parse cap:%c%c%c%c pic(%u, %u), buf(%u, %u)\n",
 782                  (param->dst_fourcc & 0xff),
 783                  (param->dst_fourcc >>  8 & 0xff),
 784                  (param->dst_fourcc >> 16 & 0xff),
 785                  (param->dst_fourcc >> 24 & 0xff),
 786                  param->pic_w, param->pic_h,
 787                  param->dec_w, param->dec_h);
 788 }
 789
 790 static void mtk_jpeg_enc_buf_queue(struct vb2_buffer *vb)
 791 {
 792         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 793         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 794
 795         v4l2_dbg(2, debug, &jpeg->v4l2_dev, "(%d) buf_q id=%d, vb=%p\n",
 796                  vb->vb2_queue->type, vb->index, vb);
 797
 798         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
 799 }
 800
 801 static void mtk_jpeg_dec_buf_queue(struct vb2_buffer *vb)
 802 {
 803         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 804         struct mtk_jpeg_dec_param *param;
 805         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 806         struct mtk_jpeg_src_buf *jpeg_src_buf;
 807         bool header_valid;
 808
 809         v4l2_dbg(2, debug, &jpeg->v4l2_dev, "(%d) buf_q id=%d, vb=%p\n",
 810                  vb->vb2_queue->type, vb->index, vb);
 811
 812         if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 813                 goto end;
 814
 815         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(vb);
 816         param = &jpeg_src_buf->dec_param;
 817         memset(param, 0, sizeof(*param));
 818
 819         header_valid = mtk_jpeg_parse(param, (u8 *)vb2_plane_vaddr(vb, 0),
 820                                       vb2_get_plane_payload(vb, 0));
 821         if (!header_valid) {
 822                 v4l2_err(&jpeg->v4l2_dev, "Header invalid.\n");
 823                 vb2_buffer_done(vb, VB2_BUF_STATE_ERROR);
 824                 return;
 825         }
 826
 827         if (ctx->state == MTK_JPEG_INIT) {
 828                 struct vb2_queue *dst_vq = v4l2_m2m_get_vq(
 829                         ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
 830
 831                 mtk_jpeg_queue_src_chg_event(ctx);
 832                 mtk_jpeg_set_queue_data(ctx, param);
 833                 ctx->state = vb2_is_streaming(dst_vq) ?
 834                                 MTK_JPEG_SOURCE_CHANGE : MTK_JPEG_RUNNING;
 835         }
 836 end:
 837         v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, to_vb2_v4l2_buffer(vb));
 838 }
 839
 840 static struct vb2_v4l2_buffer *mtk_jpeg_buf_remove(struct mtk_jpeg_ctx *ctx,
 841                                  enum v4l2_buf_type type)
 842 {
 843         if (V4L2_TYPE_IS_OUTPUT(type))
 844                 return v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 845         else
 846                 return v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 847 }
 848
 849 static void mtk_jpeg_enc_stop_streaming(struct vb2_queue *q)
 850 {
 851         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
 852         struct vb2_v4l2_buffer *vb;
 853
 854         while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
 855                 v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
 856 }
 857
 858 static void mtk_jpeg_dec_stop_streaming(struct vb2_queue *q)
 859 {
 860         struct mtk_jpeg_ctx *ctx = vb2_get_drv_priv(q);
 861         struct vb2_v4l2_buffer *vb;
 862
 863         /*
 864          * STREAMOFF is an acknowledgment for source change event.
 865          * Before STREAMOFF, we still have to return the old resolution and
 866          * subsampling. Update capture queue when the stream is off.
 867          */
 868         if (ctx->state == MTK_JPEG_SOURCE_CHANGE &&
 869             V4L2_TYPE_IS_CAPTURE(q->type)) {
 870                 struct mtk_jpeg_src_buf *src_buf;
 871
 872                 vb = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 873                 src_buf = mtk_jpeg_vb2_to_srcbuf(&vb->vb2_buf);
 874                 mtk_jpeg_set_queue_data(ctx, &src_buf->dec_param);
 875                 ctx->state = MTK_JPEG_RUNNING;
 876         } else if (V4L2_TYPE_IS_OUTPUT(q->type)) {
 877                 ctx->state = MTK_JPEG_INIT;
 878         }
 879
 880         while ((vb = mtk_jpeg_buf_remove(ctx, q->type)))
 881                 v4l2_m2m_buf_done(vb, VB2_BUF_STATE_ERROR);
 882 }
 883
 884 static const struct vb2_ops mtk_jpeg_dec_qops = {
 885         .queue_setup        = mtk_jpeg_queue_setup,
 886         .buf_prepare        = mtk_jpeg_buf_prepare,
 887         .buf_queue          = mtk_jpeg_dec_buf_queue,
 888         .wait_prepare       = vb2_ops_wait_prepare,
 889         .wait_finish        = vb2_ops_wait_finish,
 890         .stop_streaming     = mtk_jpeg_dec_stop_streaming,
 891 };
 892
 893 static const struct vb2_ops mtk_jpeg_enc_qops = {
 894         .queue_setup        = mtk_jpeg_queue_setup,
 895         .buf_prepare        = mtk_jpeg_buf_prepare,
 896         .buf_queue          = mtk_jpeg_enc_buf_queue,
 897         .wait_prepare       = vb2_ops_wait_prepare,
 898         .wait_finish        = vb2_ops_wait_finish,
 899         .stop_streaming     = mtk_jpeg_enc_stop_streaming,
 900 };
 901
 902 static void mtk_jpeg_set_dec_src(struct mtk_jpeg_ctx *ctx,
 903                                  struct vb2_buffer *src_buf,
 904                                  struct mtk_jpeg_bs *bs)
 905 {
 906         bs->str_addr = vb2_dma_contig_plane_dma_addr(src_buf, 0);
 907         bs->end_addr = bs->str_addr +
 908                        round_up(vb2_get_plane_payload(src_buf, 0), 16);
 909         bs->size = round_up(vb2_plane_size(src_buf, 0), 128);
 910 }
 911
 912 static int mtk_jpeg_set_dec_dst(struct mtk_jpeg_ctx *ctx,
 913                                 struct mtk_jpeg_dec_param *param,
 914                                 struct vb2_buffer *dst_buf,
 915                                 struct mtk_jpeg_fb *fb)
 916 {
 917         int i;
 918
 919         if (param->comp_num != dst_buf->num_planes) {
 920                 dev_err(ctx->jpeg->dev, "plane number mismatch (%u != %u)\n",
 921                         param->comp_num, dst_buf->num_planes);
 922                 return -EINVAL;
 923         }
 924
 925         for (i = 0; i < dst_buf->num_planes; i++) {
 926                 if (vb2_plane_size(dst_buf, i) < param->comp_size[i]) {
 927                         dev_err(ctx->jpeg->dev,
 928                                 "buffer size is underflow (%lu < %u)\n",
 929                                 vb2_plane_size(dst_buf, 0),
 930                                 param->comp_size[i]);
 931                         return -EINVAL;
 932                 }
 933                 fb->plane_addr[i] = vb2_dma_contig_plane_dma_addr(dst_buf, i);
 934         }
 935
 936         return 0;
 937 }
 938
 939 static int mtk_jpegenc_get_hw(struct mtk_jpeg_ctx *ctx)
 940 {
 941         struct mtk_jpegenc_comp_dev *comp_jpeg;
 942         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 943         unsigned long flags;
 944         int hw_id = -1;
 945         int i;
 946
 947         spin_lock_irqsave(&jpeg->hw_lock, flags);
 948         for (i = 0; i < MTK_JPEGENC_HW_MAX; i++) {
 949                 comp_jpeg = jpeg->enc_hw_dev[i];
 950                 if (comp_jpeg->hw_state == MTK_JPEG_HW_IDLE) {
 951                         hw_id = i;
 952                         comp_jpeg->hw_state = MTK_JPEG_HW_BUSY;
 953                         break;
 954                 }
 955         }
 956         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
 957
 958         return hw_id;
 959 }
 960
 961 static int mtk_jpegenc_set_hw_param(struct mtk_jpeg_ctx *ctx,
 962                                     int hw_id,
 963                                     struct vb2_v4l2_buffer *src_buf,
 964                                     struct vb2_v4l2_buffer *dst_buf)
 965 {
 966         struct mtk_jpegenc_comp_dev *jpeg = ctx->jpeg->enc_hw_dev[hw_id];
 967
 968         jpeg->hw_param.curr_ctx = ctx;
 969         jpeg->hw_param.src_buffer = src_buf;
 970         jpeg->hw_param.dst_buffer = dst_buf;
 971
 972         return 0;
 973 }
 974
 975 static int mtk_jpegenc_put_hw(struct mtk_jpeg_dev *jpeg, int hw_id)
 976 {
 977         unsigned long flags;
 978
 979         spin_lock_irqsave(&jpeg->hw_lock, flags);
 980         jpeg->enc_hw_dev[hw_id]->hw_state = MTK_JPEG_HW_IDLE;
 981         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
 982
 983         return 0;
 984 }
 985
 986 static void mtk_jpegenc_worker(struct work_struct *work)
 987 {
 988         struct mtk_jpegenc_comp_dev *comp_jpeg[MTK_JPEGENC_HW_MAX];
 989         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
 990         struct mtk_jpeg_src_buf *jpeg_dst_buf;
 991         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 992         int ret, i, hw_id = 0;
 993         unsigned long flags;
 994
 995         struct mtk_jpeg_ctx *ctx = container_of(work,
 996                 struct mtk_jpeg_ctx,
 997                 jpeg_work);
 998         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 999
1000         for (i = 0; i < MTK_JPEGENC_HW_MAX; i++)
1001                 comp_jpeg[i] = jpeg->enc_hw_dev[i];
1002         i = 0;
1003
1004 retry_select:
1005         hw_id = mtk_jpegenc_get_hw(ctx);
1006         if (hw_id < 0) {
1007                 ret = wait_event_interruptible(jpeg->hw_wq,
1008                                                atomic_read(&jpeg->hw_rdy) > 0);
1009                 if (ret != 0 || (i++ > MTK_JPEG_MAX_RETRY_TIME)) {
1010                         dev_err(jpeg->dev, "%s : %d, all HW are busy\n",
1011                                 __func__, __LINE__);
1012                         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1013                         return;
1014                 }
1015
1016                 goto retry_select;
1017         }
1018
1019         atomic_dec(&jpeg->hw_rdy);
1020         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1021         if (!src_buf)
1022                 goto getbuf_fail;
1023
1024         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1025         if (!dst_buf)
1026                 goto getbuf_fail;
1027
1028         v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
1029
1030         mtk_jpegenc_set_hw_param(ctx, hw_id, src_buf, dst_buf);
1031         ret = pm_runtime_get_sync(comp_jpeg[hw_id]->dev);
1032         if (ret < 0) {
1033                 dev_err(jpeg->dev, "%s : %d, pm_runtime_get_sync fail !!!\n",
1034                         __func__, __LINE__);
1035                 goto enc_end;
1036         }
1037
1038         ret = clk_prepare_enable(comp_jpeg[hw_id]->venc_clk.clks->clk);
1039         if (ret) {
1040                 dev_err(jpeg->dev, "%s : %d, jpegenc clk_prepare_enable fail\n",
1041                         __func__, __LINE__);
1042                 goto enc_end;
1043         }
1044
1045         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1046         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1047
1048         schedule_delayed_work(&comp_jpeg[hw_id]->job_timeout_work,
1049                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1050
1051         spin_lock_irqsave(&comp_jpeg[hw_id]->hw_lock, flags);
1052         jpeg_dst_buf = mtk_jpeg_vb2_to_srcbuf(&dst_buf->vb2_buf);
1053         jpeg_dst_buf->curr_ctx = ctx;
1054         jpeg_dst_buf->frame_num = ctx->total_frame_num;
1055         ctx->total_frame_num++;
1056         mtk_jpeg_enc_reset(comp_jpeg[hw_id]->reg_base);
1057         mtk_jpeg_set_enc_dst(ctx,
1058                              comp_jpeg[hw_id]->reg_base,
1059                              &dst_buf->vb2_buf);
1060         mtk_jpeg_set_enc_src(ctx,
1061                              comp_jpeg[hw_id]->reg_base,
1062                              &src_buf->vb2_buf);
1063         mtk_jpeg_set_enc_params(ctx, comp_jpeg[hw_id]->reg_base);
1064         mtk_jpeg_enc_start(comp_jpeg[hw_id]->reg_base);
1065         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1066         spin_unlock_irqrestore(&comp_jpeg[hw_id]->hw_lock, flags);
1067
1068         return;
1069
1070 enc_end:
1071         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1072         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1073         v4l2_m2m_buf_done(src_buf, buf_state);
1074         v4l2_m2m_buf_done(dst_buf, buf_state);
1075 getbuf_fail:
1076         atomic_inc(&jpeg->hw_rdy);
1077         mtk_jpegenc_put_hw(jpeg, hw_id);
1078         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1079 }
1080
1081 static void mtk_jpeg_enc_device_run(void *priv)
1082 {
1083         struct mtk_jpeg_ctx *ctx = priv;
1084         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1085         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1086         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1087         unsigned long flags;
1088         int ret;
1089
1090         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1091         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1092
1093         ret = pm_runtime_resume_and_get(jpeg->dev);
1094         if (ret < 0)
1095                 goto enc_end;
1096
1097         schedule_delayed_work(&jpeg->job_timeout_work,
1098                         msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1099
1100         spin_lock_irqsave(&jpeg->hw_lock, flags);
1101
1102         /*
1103          * Resetting the hardware every frame is to ensure that all the
1104          * registers are cleared. This is a hardware requirement.
1105          */
1106         mtk_jpeg_enc_reset(jpeg->reg_base);
1107
1108         mtk_jpeg_set_enc_src(ctx, jpeg->reg_base, &src_buf->vb2_buf);
1109         mtk_jpeg_set_enc_dst(ctx, jpeg->reg_base, &dst_buf->vb2_buf);
1110         mtk_jpeg_set_enc_params(ctx, jpeg->reg_base);
1111         mtk_jpeg_enc_start(jpeg->reg_base);
1112         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1113         return;
1114
1115 enc_end:
1116         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1117         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1118         v4l2_m2m_buf_done(src_buf, buf_state);
1119         v4l2_m2m_buf_done(dst_buf, buf_state);
1120         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1121 }
1122
1123 static void mtk_jpeg_multicore_enc_device_run(void *priv)
1124 {
1125         struct mtk_jpeg_ctx *ctx = priv;
1126         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1127
1128         queue_work(jpeg->workqueue, &ctx->jpeg_work);
1129 }
1130
1131 static int mtk_jpegdec_get_hw(struct mtk_jpeg_ctx *ctx)
1132 {
1133         struct mtk_jpegdec_comp_dev *comp_jpeg;
1134         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1135         unsigned long flags;
1136         int hw_id = -1;
1137         int i;
1138
1139         spin_lock_irqsave(&jpeg->hw_lock, flags);
1140         for (i = 0; i < MTK_JPEGDEC_HW_MAX; i++) {
1141                 comp_jpeg = jpeg->dec_hw_dev[i];
1142                 if (comp_jpeg->hw_state == MTK_JPEG_HW_IDLE) {
1143                         hw_id = i;
1144                         comp_jpeg->hw_state = MTK_JPEG_HW_BUSY;
1145                         break;
1146                 }
1147         }
1148         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1149
1150         return hw_id;
1151 }
1152
1153 static int mtk_jpegdec_put_hw(struct mtk_jpeg_dev *jpeg, int hw_id)
1154 {
1155         unsigned long flags;
1156
1157         spin_lock_irqsave(&jpeg->hw_lock, flags);
1158         jpeg->dec_hw_dev[hw_id]->hw_state =
1159                 MTK_JPEG_HW_IDLE;
1160         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1161
1162         return 0;
1163 }
1164
1165 static int mtk_jpegdec_set_hw_param(struct mtk_jpeg_ctx *ctx,
1166                                     int hw_id,
1167                                     struct vb2_v4l2_buffer *src_buf,
1168                                     struct vb2_v4l2_buffer *dst_buf)
1169 {
1170         struct mtk_jpegdec_comp_dev *jpeg =
1171                 ctx->jpeg->dec_hw_dev[hw_id];
1172
1173         jpeg->hw_param.curr_ctx = ctx;
1174         jpeg->hw_param.src_buffer = src_buf;
1175         jpeg->hw_param.dst_buffer = dst_buf;
1176
1177         return 0;
1178 }
1179
1180 static void mtk_jpegdec_worker(struct work_struct *work)
1181 {
1182         struct mtk_jpeg_ctx *ctx = container_of(work, struct mtk_jpeg_ctx,
1183                 jpeg_work);
1184         struct mtk_jpegdec_comp_dev *comp_jpeg[MTK_JPEGDEC_HW_MAX];
1185         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1186         struct mtk_jpeg_src_buf *jpeg_src_buf, *jpeg_dst_buf;
1187         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1188         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1189         int ret, i, hw_id = 0;
1190         struct mtk_jpeg_bs bs;
1191         struct mtk_jpeg_fb fb;
1192         unsigned long flags;
1193
1194         for (i = 0; i < MTK_JPEGDEC_HW_MAX; i++)
1195                 comp_jpeg[i] = jpeg->dec_hw_dev[i];
1196         i = 0;
1197
1198 retry_select:
1199         hw_id = mtk_jpegdec_get_hw(ctx);
1200         if (hw_id < 0) {
1201                 ret = wait_event_interruptible_timeout(jpeg->hw_wq,
1202                                                        atomic_read(&jpeg->hw_rdy) > 0,
1203                                                        MTK_JPEG_HW_TIMEOUT_MSEC);
1204                 if (ret != 0 || (i++ > MTK_JPEG_MAX_RETRY_TIME)) {
1205                         dev_err(jpeg->dev, "%s : %d, all HW are busy\n",
1206                                 __func__, __LINE__);
1207                         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1208                         return;
1209                 }
1210
1211                 goto retry_select;
1212         }
1213
1214         atomic_dec(&jpeg->hw_rdy);
1215         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1216         if (!src_buf)
1217                 goto getbuf_fail;
1218
1219         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1220         if (!dst_buf)
1221                 goto getbuf_fail;
1222
1223         v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
1224         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1225         jpeg_dst_buf = mtk_jpeg_vb2_to_srcbuf(&dst_buf->vb2_buf);
1226
1227         if (mtk_jpeg_check_resolution_change(ctx,
1228                                              &jpeg_src_buf->dec_param)) {
1229                 mtk_jpeg_queue_src_chg_event(ctx);
1230                 ctx->state = MTK_JPEG_SOURCE_CHANGE;
1231                 goto getbuf_fail;
1232         }
1233
1234         jpeg_src_buf->curr_ctx = ctx;
1235         jpeg_src_buf->frame_num = ctx->total_frame_num;
1236         jpeg_dst_buf->curr_ctx = ctx;
1237         jpeg_dst_buf->frame_num = ctx->total_frame_num;
1238
1239         mtk_jpegdec_set_hw_param(ctx, hw_id, src_buf, dst_buf);
1240         ret = pm_runtime_get_sync(comp_jpeg[hw_id]->dev);
1241         if (ret < 0) {
1242                 dev_err(jpeg->dev, "%s : %d, pm_runtime_get_sync fail !!!\n",
1243                         __func__, __LINE__);
1244                 goto dec_end;
1245         }
1246
1247         ret = clk_prepare_enable(comp_jpeg[hw_id]->jdec_clk.clks->clk);
1248         if (ret) {
1249                 dev_err(jpeg->dev, "%s : %d, jpegdec clk_prepare_enable fail\n",
1250                         __func__, __LINE__);
1251                 goto clk_end;
1252         }
1253
1254         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1255         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1256
1257         schedule_delayed_work(&comp_jpeg[hw_id]->job_timeout_work,
1258                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1259
1260         mtk_jpeg_set_dec_src(ctx, &src_buf->vb2_buf, &bs);
1261         if (mtk_jpeg_set_dec_dst(ctx,
1262                                  &jpeg_src_buf->dec_param,
1263                                  &dst_buf->vb2_buf, &fb)) {
1264                 dev_err(jpeg->dev, "%s : %d, mtk_jpeg_set_dec_dst fail\n",
1265                         __func__, __LINE__);
1266                 goto setdst_end;
1267         }
1268
1269         spin_lock_irqsave(&comp_jpeg[hw_id]->hw_lock, flags);
1270         ctx->total_frame_num++;
1271         mtk_jpeg_dec_reset(comp_jpeg[hw_id]->reg_base);
1272         mtk_jpeg_dec_set_config(comp_jpeg[hw_id]->reg_base,
1273                                 &jpeg_src_buf->dec_param,
1274                                 jpeg_src_buf->bs_size,
1275                                 &bs,
1276                                 &fb);
1277         mtk_jpeg_dec_start(comp_jpeg[hw_id]->reg_base);
1278         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1279         spin_unlock_irqrestore(&comp_jpeg[hw_id]->hw_lock, flags);
1280
1281         return;
1282
1283 setdst_end:
1284         clk_disable_unprepare(comp_jpeg[hw_id]->jdec_clk.clks->clk);
1285 clk_end:
1286         pm_runtime_put(comp_jpeg[hw_id]->dev);
1287 dec_end:
1288         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1289         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1290         v4l2_m2m_buf_done(src_buf, buf_state);
1291         v4l2_m2m_buf_done(dst_buf, buf_state);
1292 getbuf_fail:
1293         atomic_inc(&jpeg->hw_rdy);
1294         mtk_jpegdec_put_hw(jpeg, hw_id);
1295         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1296 }
1297
1298 static void mtk_jpeg_multicore_dec_device_run(void *priv)
1299 {
1300         struct mtk_jpeg_ctx *ctx = priv;
1301         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1302
1303         queue_work(jpeg->workqueue, &ctx->jpeg_work);
1304 }
1305
1306 static void mtk_jpeg_dec_device_run(void *priv)
1307 {
1308         struct mtk_jpeg_ctx *ctx = priv;
1309         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1310         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1311         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1312         unsigned long flags;
1313         struct mtk_jpeg_src_buf *jpeg_src_buf;
1314         struct mtk_jpeg_bs bs;
1315         struct mtk_jpeg_fb fb;
1316         int ret;
1317
1318         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1319         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1320         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1321
1322         if (mtk_jpeg_check_resolution_change(ctx, &jpeg_src_buf->dec_param)) {
1323                 mtk_jpeg_queue_src_chg_event(ctx);
1324                 ctx->state = MTK_JPEG_SOURCE_CHANGE;
1325                 v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1326                 return;
1327         }
1328
1329         ret = pm_runtime_resume_and_get(jpeg->dev);
1330         if (ret < 0)
1331                 goto dec_end;
1332
1333         schedule_delayed_work(&jpeg->job_timeout_work,
1334                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1335
1336         mtk_jpeg_set_dec_src(ctx, &src_buf->vb2_buf, &bs);
1337         if (mtk_jpeg_set_dec_dst(ctx, &jpeg_src_buf->dec_param, &dst_buf->vb2_buf, &fb))
1338                 goto dec_end;
1339
1340         spin_lock_irqsave(&jpeg->hw_lock, flags);
1341         mtk_jpeg_dec_reset(jpeg->reg_base);
1342         mtk_jpeg_dec_set_config(jpeg->reg_base,
1343                                 &jpeg_src_buf->dec_param,
1344                                 jpeg_src_buf->bs_size,
1345                                 &bs,
1346                                 &fb);
1347         mtk_jpeg_dec_start(jpeg->reg_base);
1348         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1349         return;
1350
1351 dec_end:
1352         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1353         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1354         v4l2_m2m_buf_done(src_buf, buf_state);
1355         v4l2_m2m_buf_done(dst_buf, buf_state);
1356         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1357 }
1358
1359 static int mtk_jpeg_dec_job_ready(void *priv)
1360 {
1361         struct mtk_jpeg_ctx *ctx = priv;
1362
1363         return (ctx->state == MTK_JPEG_RUNNING) ? 1 : 0;
1364 }
1365
1366 static const struct v4l2_m2m_ops mtk_jpeg_enc_m2m_ops = {
1367         .device_run = mtk_jpeg_enc_device_run,
1368 };
1369
1370 static const struct v4l2_m2m_ops mtk_jpeg_multicore_enc_m2m_ops = {
1371         .device_run = mtk_jpeg_multicore_enc_device_run,
1372 };
1373
1374 static const struct v4l2_m2m_ops mtk_jpeg_multicore_dec_m2m_ops = {
1375         .device_run = mtk_jpeg_multicore_dec_device_run,
1376 };
1377
1378 static const struct v4l2_m2m_ops mtk_jpeg_dec_m2m_ops = {
1379         .device_run = mtk_jpeg_dec_device_run,
1380         .job_ready  = mtk_jpeg_dec_job_ready,
1381 };
1382
1383 static int mtk_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
1384                                struct vb2_queue *dst_vq)
1385 {
1386         struct mtk_jpeg_ctx *ctx = priv;
1387         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1388         int ret;
1389
1390         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1391         src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
1392         src_vq->drv_priv = ctx;
1393         src_vq->buf_struct_size = sizeof(struct mtk_jpeg_src_buf);
1394         src_vq->ops = jpeg->variant->qops;
1395         src_vq->mem_ops = &vb2_dma_contig_memops;
1396         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1397         src_vq->lock = &ctx->jpeg->lock;
1398         src_vq->dev = ctx->jpeg->dev;
1399         ret = vb2_queue_init(src_vq);
1400         if (ret)
1401                 return ret;
1402
1403         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1404         dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
1405         dst_vq->drv_priv = ctx;
1406         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1407         dst_vq->ops = jpeg->variant->qops;
1408         dst_vq->mem_ops = &vb2_dma_contig_memops;
1409         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1410         dst_vq->lock = &ctx->jpeg->lock;
1411         dst_vq->dev = ctx->jpeg->dev;
1412         ret = vb2_queue_init(dst_vq);
1413
1414         return ret;
1415 }
1416
1417 static void mtk_jpeg_clk_on(struct mtk_jpeg_dev *jpeg)
1418 {
1419         int ret;
1420
1421         ret = clk_bulk_prepare_enable(jpeg->variant->num_clks,
1422                                       jpeg->variant->clks);
1423         if (ret)
1424                 dev_err(jpeg->dev, "Failed to open jpeg clk: %d\n", ret);
1425 }
1426
1427 static void mtk_jpeg_clk_off(struct mtk_jpeg_dev *jpeg)
1428 {
1429         clk_bulk_disable_unprepare(jpeg->variant->num_clks,
1430                                    jpeg->variant->clks);
1431 }
1432
1433 static irqreturn_t mtk_jpeg_enc_done(struct mtk_jpeg_dev *jpeg)
1434 {
1435         struct mtk_jpeg_ctx *ctx;
1436         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1437         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1438         u32 result_size;
1439
1440         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1441         if (!ctx) {
1442                 v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
1443                 return IRQ_HANDLED;
1444         }
1445
1446         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1447         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1448
1449         result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base);
1450         vb2_set_plane_payload(&dst_buf->vb2_buf, 0, result_size);
1451
1452         buf_state = VB2_BUF_STATE_DONE;
1453
1454         v4l2_m2m_buf_done(src_buf, buf_state);
1455         v4l2_m2m_buf_done(dst_buf, buf_state);
1456         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1457         pm_runtime_put(ctx->jpeg->dev);
1458         return IRQ_HANDLED;
1459 }
1460
1461 static irqreturn_t mtk_jpeg_enc_irq(int irq, void *priv)
1462 {
1463         struct mtk_jpeg_dev *jpeg = priv;
1464         u32 irq_status;
1465         irqreturn_t ret = IRQ_NONE;
1466
1467         cancel_delayed_work(&jpeg->job_timeout_work);
1468
1469         irq_status = readl(jpeg->reg_base + JPEG_ENC_INT_STS) &
1470                      JPEG_ENC_INT_STATUS_MASK_ALLIRQ;
1471         if (irq_status)
1472                 writel(0, jpeg->reg_base + JPEG_ENC_INT_STS);
1473
1474         if (!(irq_status & JPEG_ENC_INT_STATUS_DONE))
1475                 return ret;
1476
1477         ret = mtk_jpeg_enc_done(jpeg);
1478         return ret;
1479 }
1480
1481 static irqreturn_t mtk_jpeg_dec_irq(int irq, void *priv)
1482 {
1483         struct mtk_jpeg_dev *jpeg = priv;
1484         struct mtk_jpeg_ctx *ctx;
1485         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1486         struct mtk_jpeg_src_buf *jpeg_src_buf;
1487         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1488         u32     dec_irq_ret;
1489         u32 dec_ret;
1490         int i;
1491
1492         cancel_delayed_work(&jpeg->job_timeout_work);
1493
1494         dec_ret = mtk_jpeg_dec_get_int_status(jpeg->reg_base);
1495         dec_irq_ret = mtk_jpeg_dec_enum_result(dec_ret);
1496         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1497         if (!ctx) {
1498                 v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
1499                 return IRQ_HANDLED;
1500         }
1501
1502         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1503         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1504         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1505
1506         if (dec_irq_ret >= MTK_JPEG_DEC_RESULT_UNDERFLOW)
1507                 mtk_jpeg_dec_reset(jpeg->reg_base);
1508
1509         if (dec_irq_ret != MTK_JPEG_DEC_RESULT_EOF_DONE) {
1510                 dev_err(jpeg->dev, "decode failed\n");
1511                 goto dec_end;
1512         }
1513
1514         for (i = 0; i < dst_buf->vb2_buf.num_planes; i++)
1515                 vb2_set_plane_payload(&dst_buf->vb2_buf, i,
1516                                       jpeg_src_buf->dec_param.comp_size[i]);
1517
1518         buf_state = VB2_BUF_STATE_DONE;
1519
1520 dec_end:
1521         v4l2_m2m_buf_done(src_buf, buf_state);
1522         v4l2_m2m_buf_done(dst_buf, buf_state);
1523         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1524         pm_runtime_put(ctx->jpeg->dev);
1525         return IRQ_HANDLED;
1526 }
1527
1528 static void mtk_jpeg_set_default_params(struct mtk_jpeg_ctx *ctx)
1529 {
1530         struct mtk_jpeg_q_data *q = &ctx->out_q;
1531         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1532
1533         ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
1534         q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
1535         q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
1536         q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
1537         q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
1538
1539         q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
1540                                       jpeg->variant->num_formats,
1541                                       jpeg->variant->out_q_default_fourcc,
1542                                       MTK_JPEG_FMT_FLAG_OUTPUT);
1543         q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
1544         q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
1545         mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
1546
1547         q = &ctx->cap_q;
1548         q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
1549                                       jpeg->variant->num_formats,
1550                                       jpeg->variant->cap_q_default_fourcc,
1551                                       MTK_JPEG_FMT_FLAG_CAPTURE);
1552         q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
1553         q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
1554         q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
1555         q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
1556         q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
1557         q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
1558
1559         mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
1560 }
1561
1562 static int mtk_jpeg_open(struct file *file)
1563 {
1564         struct mtk_jpeg_dev *jpeg = video_drvdata(file);
1565         struct video_device *vfd = video_devdata(file);
1566         struct mtk_jpeg_ctx *ctx;
1567         int ret = 0;
1568
1569         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1570         if (!ctx)
1571                 return -ENOMEM;
1572
1573         if (mutex_lock_interruptible(&jpeg->lock)) {
1574                 ret = -ERESTARTSYS;
1575                 goto free;
1576         }
1577
1578         INIT_WORK(&ctx->jpeg_work, jpeg->variant->jpeg_worker);
1579         INIT_LIST_HEAD(&ctx->dst_done_queue);
1580         spin_lock_init(&ctx->done_queue_lock);
1581         v4l2_fh_init(&ctx->fh, vfd);
1582         file->private_data = &ctx->fh;
1583         v4l2_fh_add(&ctx->fh);
1584
1585         ctx->jpeg = jpeg;
1586         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx,
1587                                             mtk_jpeg_queue_init);
1588         if (IS_ERR(ctx->fh.m2m_ctx)) {
1589                 ret = PTR_ERR(ctx->fh.m2m_ctx);
1590                 goto error;
1591         }
1592
1593         if (jpeg->variant->cap_q_default_fourcc == V4L2_PIX_FMT_JPEG) {
1594                 ret = mtk_jpeg_enc_ctrls_setup(ctx);
1595                 if (ret) {
1596                         v4l2_err(&jpeg->v4l2_dev, "Failed to setup jpeg enc controls\n");
1597                         goto error;
1598                 }
1599         } else {
1600                 v4l2_ctrl_handler_init(&ctx->ctrl_hdl, 0);
1601         }
1602
1603         mtk_jpeg_set_default_params(ctx);
1604         mutex_unlock(&jpeg->lock);
1605         return 0;
1606
1607 error:
1608         v4l2_fh_del(&ctx->fh);
1609         v4l2_fh_exit(&ctx->fh);
1610         mutex_unlock(&jpeg->lock);
1611 free:
1612         kfree(ctx);
1613         return ret;
1614 }
1615
1616 static int mtk_jpeg_release(struct file *file)
1617 {
1618         struct mtk_jpeg_dev *jpeg = video_drvdata(file);
1619         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(file->private_data);
1620
1621         mutex_lock(&jpeg->lock);
1622         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1623         v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
1624         v4l2_fh_del(&ctx->fh);
1625         v4l2_fh_exit(&ctx->fh);
1626         kfree(ctx);
1627         mutex_unlock(&jpeg->lock);
1628         return 0;
1629 }
1630
1631 static const struct v4l2_file_operations mtk_jpeg_fops = {
1632         .owner          = THIS_MODULE,
1633         .open           = mtk_jpeg_open,
1634         .release        = mtk_jpeg_release,
1635         .poll           = v4l2_m2m_fop_poll,
1636         .unlocked_ioctl = video_ioctl2,
1637         .mmap           = v4l2_m2m_fop_mmap,
1638 };
1639
1640 static struct clk_bulk_data mt8173_jpeg_dec_clocks[] = {
1641         { .id = "jpgdec-smi" },
1642         { .id = "jpgdec" },
1643 };
1644
1645 static struct clk_bulk_data mtk_jpeg_clocks[] = {
1646         { .id = "jpgenc" },
1647 };
1648
1649 static void mtk_jpeg_job_timeout_work(struct work_struct *work)
1650 {
1651         struct mtk_jpeg_dev *jpeg = container_of(work, struct mtk_jpeg_dev,
1652                                                  job_timeout_work.work);
1653         struct mtk_jpeg_ctx *ctx;
1654         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1655
1656         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1657         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1658         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1659
1660         jpeg->variant->hw_reset(jpeg->reg_base);
1661
1662         pm_runtime_put(jpeg->dev);
1663
1664         v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
1665         v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
1666         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1667 }
1668
1669 static int mtk_jpeg_single_core_init(struct platform_device *pdev,
1670                                      struct mtk_jpeg_dev *jpeg_dev)
1671 {
1672         struct mtk_jpeg_dev *jpeg = jpeg_dev;
1673         int jpeg_irq, ret;
1674
1675         INIT_DELAYED_WORK(&jpeg->job_timeout_work,
1676                           mtk_jpeg_job_timeout_work);
1677
1678         jpeg->reg_base = devm_platform_ioremap_resource(pdev, 0);
1679         if (IS_ERR(jpeg->reg_base)) {
1680                 ret = PTR_ERR(jpeg->reg_base);
1681                 return ret;
1682         }
1683
1684         jpeg_irq = platform_get_irq(pdev, 0);
1685         if (jpeg_irq < 0)
1686                 return jpeg_irq;
1687
1688         ret = devm_request_irq(&pdev->dev,
1689                                jpeg_irq,
1690                                jpeg->variant->irq_handler,
1691                                0,
1692                                pdev->name, jpeg);
1693         if (ret) {
1694                 dev_err(&pdev->dev, "Failed to request jpeg_irq %d (%d)\n",
1695                         jpeg_irq, ret);
1696                 return ret;
1697         }
1698
1699         ret = devm_clk_bulk_get(jpeg->dev,
1700                                 jpeg->variant->num_clks,
1701                                 jpeg->variant->clks);
1702         if (ret) {
1703                 dev_err(&pdev->dev, "Failed to init clk\n");
1704                 return ret;
1705         }
1706
1707         return 0;
1708 }
1709
1710 static int mtk_jpeg_probe(struct platform_device *pdev)
1711 {
1712         struct mtk_jpeg_dev *jpeg;
1713         struct device_node *child;
1714         int num_child = 0;
1715         int ret;
1716
1717         jpeg = devm_kzalloc(&pdev->dev, sizeof(*jpeg), GFP_KERNEL);
1718         if (!jpeg)
1719                 return -ENOMEM;
1720
1721         mutex_init(&jpeg->lock);
1722         spin_lock_init(&jpeg->hw_lock);
1723         jpeg->dev = &pdev->dev;
1724         jpeg->variant = of_device_get_match_data(jpeg->dev);
1725
1726         ret = devm_of_platform_populate(&pdev->dev);
1727         if (ret) {
1728                 v4l2_err(&jpeg->v4l2_dev, "Master of platform populate failed.");
1729                 return -EINVAL;
1730         }
1731
1732         if (!jpeg->variant->multi_core) {
1733                 ret = mtk_jpeg_single_core_init(pdev, jpeg);
1734                 if (ret) {
1735                         v4l2_err(&jpeg->v4l2_dev, "mtk_jpeg_single_core_init failed.");
1736                         return -EINVAL;
1737                 }
1738         } else {
1739                 init_waitqueue_head(&jpeg->hw_wq);
1740
1741                 for_each_child_of_node(pdev->dev.of_node, child)
1742                         num_child++;
1743
1744                 atomic_set(&jpeg->hw_rdy, num_child);
1745                 atomic_set(&jpeg->hw_index, 0);
1746
1747                 jpeg->workqueue = alloc_ordered_workqueue(MTK_JPEG_NAME,
1748                                                           WQ_MEM_RECLAIM
1749                                                           | WQ_FREEZABLE);
1750                 if (!jpeg->workqueue)
1751                         return -EINVAL;
1752         }
1753
1754         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
1755         if (ret) {
1756                 dev_err(&pdev->dev, "Failed to register v4l2 device\n");
1757                 return -EINVAL;
1758         }
1759
1760         jpeg->m2m_dev = v4l2_m2m_init(jpeg->variant->m2m_ops);
1761
1762         if (IS_ERR(jpeg->m2m_dev)) {
1763                 v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
1764                 ret = PTR_ERR(jpeg->m2m_dev);
1765                 goto err_m2m_init;
1766         }
1767
1768         jpeg->vdev = video_device_alloc();
1769         if (!jpeg->vdev) {
1770                 ret = -ENOMEM;
1771                 goto err_vfd_jpeg_alloc;
1772         }
1773         snprintf(jpeg->vdev->name, sizeof(jpeg->vdev->name),
1774                  "%s", jpeg->variant->dev_name);
1775         jpeg->vdev->fops = &mtk_jpeg_fops;
1776         jpeg->vdev->ioctl_ops = jpeg->variant->ioctl_ops;
1777         jpeg->vdev->minor = -1;
1778         jpeg->vdev->release = video_device_release;
1779         jpeg->vdev->lock = &jpeg->lock;
1780         jpeg->vdev->v4l2_dev = &jpeg->v4l2_dev;
1781         jpeg->vdev->vfl_dir = VFL_DIR_M2M;
1782         jpeg->vdev->device_caps = V4L2_CAP_STREAMING |
1783                                   V4L2_CAP_VIDEO_M2M_MPLANE;
1784
1785         ret = video_register_device(jpeg->vdev, VFL_TYPE_VIDEO, -1);
1786         if (ret) {
1787                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1788                 goto err_vfd_jpeg_register;
1789         }
1790
1791         video_set_drvdata(jpeg->vdev, jpeg);
1792         v4l2_info(&jpeg->v4l2_dev,
1793                   "%s device registered as /dev/video%d (%d,%d)\n",
1794                   jpeg->variant->dev_name, jpeg->vdev->num,
1795                   VIDEO_MAJOR, jpeg->vdev->minor);
1796
1797         platform_set_drvdata(pdev, jpeg);
1798
1799         pm_runtime_enable(&pdev->dev);
1800
1801         return 0;
1802
1803 err_vfd_jpeg_register:
1804         video_device_release(jpeg->vdev);
1805
1806 err_vfd_jpeg_alloc:
1807         v4l2_m2m_release(jpeg->m2m_dev);
1808
1809 err_m2m_init:
1810         v4l2_device_unregister(&jpeg->v4l2_dev);
1811
1812         return ret;
1813 }
1814
1815 static void mtk_jpeg_remove(struct platform_device *pdev)
1816 {
1817         struct mtk_jpeg_dev *jpeg = platform_get_drvdata(pdev);
1818
1819         pm_runtime_disable(&pdev->dev);
1820         video_unregister_device(jpeg->vdev);
1821         v4l2_m2m_release(jpeg->m2m_dev);
1822         v4l2_device_unregister(&jpeg->v4l2_dev);
1823 }
1824
1825 static __maybe_unused int mtk_jpeg_pm_suspend(struct device *dev)
1826 {
1827         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1828
1829         mtk_jpeg_clk_off(jpeg);
1830
1831         return 0;
1832 }
1833
1834 static __maybe_unused int mtk_jpeg_pm_resume(struct device *dev)
1835 {
1836         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1837
1838         mtk_jpeg_clk_on(jpeg);
1839
1840         return 0;
1841 }
1842
1843 static __maybe_unused int mtk_jpeg_suspend(struct device *dev)
1844 {
1845         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1846
1847         v4l2_m2m_suspend(jpeg->m2m_dev);
1848         return pm_runtime_force_suspend(dev);
1849 }
1850
1851 static __maybe_unused int mtk_jpeg_resume(struct device *dev)
1852 {
1853         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1854         int ret;
1855
1856         ret = pm_runtime_force_resume(dev);
1857         if (ret < 0)
1858                 return ret;
1859
1860         v4l2_m2m_resume(jpeg->m2m_dev);
1861         return ret;
1862 }
1863
1864 static const struct dev_pm_ops mtk_jpeg_pm_ops = {
1865         SET_SYSTEM_SLEEP_PM_OPS(mtk_jpeg_suspend, mtk_jpeg_resume)
1866         SET_RUNTIME_PM_OPS(mtk_jpeg_pm_suspend, mtk_jpeg_pm_resume, NULL)
1867 };
1868
1869 #if defined(CONFIG_OF)
1870 static const struct mtk_jpeg_variant mt8173_jpeg_drvdata = {
1871         .clks = mt8173_jpeg_dec_clocks,
1872         .num_clks = ARRAY_SIZE(mt8173_jpeg_dec_clocks),
1873         .formats = mtk_jpeg_dec_formats,
1874         .num_formats = MTK_JPEG_DEC_NUM_FORMATS,
1875         .qops = &mtk_jpeg_dec_qops,
1876         .irq_handler = mtk_jpeg_dec_irq,
1877         .hw_reset = mtk_jpeg_dec_reset,
1878         .m2m_ops = &mtk_jpeg_dec_m2m_ops,
1879         .dev_name = "mtk-jpeg-dec",
1880         .ioctl_ops = &mtk_jpeg_dec_ioctl_ops,
1881         .out_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1882         .cap_q_default_fourcc = V4L2_PIX_FMT_YUV420M,
1883 };
1884
1885 static const struct mtk_jpeg_variant mtk_jpeg_drvdata = {
1886         .clks = mtk_jpeg_clocks,
1887         .num_clks = ARRAY_SIZE(mtk_jpeg_clocks),
1888         .formats = mtk_jpeg_enc_formats,
1889         .num_formats = MTK_JPEG_ENC_NUM_FORMATS,
1890         .qops = &mtk_jpeg_enc_qops,
1891         .irq_handler = mtk_jpeg_enc_irq,
1892         .hw_reset = mtk_jpeg_enc_reset,
1893         .m2m_ops = &mtk_jpeg_enc_m2m_ops,
1894         .dev_name = "mtk-jpeg-enc",
1895         .ioctl_ops = &mtk_jpeg_enc_ioctl_ops,
1896         .out_q_default_fourcc = V4L2_PIX_FMT_YUYV,
1897         .cap_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1898         .multi_core = false,
1899 };
1900
1901 static struct mtk_jpeg_variant mtk8195_jpegenc_drvdata = {
1902         .formats = mtk_jpeg_enc_formats,
1903         .num_formats = MTK_JPEG_ENC_NUM_FORMATS,
1904         .qops = &mtk_jpeg_enc_qops,
1905         .m2m_ops = &mtk_jpeg_multicore_enc_m2m_ops,
1906         .dev_name = "mtk-jpeg-enc",
1907         .ioctl_ops = &mtk_jpeg_enc_ioctl_ops,
1908         .out_q_default_fourcc = V4L2_PIX_FMT_YUYV,
1909         .cap_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1910         .multi_core = true,
1911         .jpeg_worker = mtk_jpegenc_worker,
1912 };
1913
1914 static const struct mtk_jpeg_variant mtk8195_jpegdec_drvdata = {
1915         .formats = mtk_jpeg_dec_formats,
1916         .num_formats = MTK_JPEG_DEC_NUM_FORMATS,
1917         .qops = &mtk_jpeg_dec_qops,
1918         .m2m_ops = &mtk_jpeg_multicore_dec_m2m_ops,
1919         .dev_name = "mtk-jpeg-dec",
1920         .ioctl_ops = &mtk_jpeg_dec_ioctl_ops,
1921         .out_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1922         .cap_q_default_fourcc = V4L2_PIX_FMT_YUV420M,
1923         .multi_core = true,
1924         .jpeg_worker = mtk_jpegdec_worker,
1925 };
1926
1927 static const struct of_device_id mtk_jpeg_match[] = {
1928         {
1929                 .compatible = "mediatek,mt8173-jpgdec",
1930                 .data = &mt8173_jpeg_drvdata,
1931         },
1932         {
1933                 .compatible = "mediatek,mt2701-jpgdec",
1934                 .data = &mt8173_jpeg_drvdata,
1935         },
1936         {
1937                 .compatible = "mediatek,mtk-jpgenc",
1938                 .data = &mtk_jpeg_drvdata,
1939         },
1940         {
1941                 .compatible = "mediatek,mt8195-jpgenc",
1942                 .data = &mtk8195_jpegenc_drvdata,
1943         },
1944         {
1945                 .compatible = "mediatek,mt8195-jpgdec",
1946                 .data = &mtk8195_jpegdec_drvdata,
1947         },
1948         {},
1949 };
1950
1951 MODULE_DEVICE_TABLE(of, mtk_jpeg_match);
1952 #endif
1953
1954 static struct platform_driver mtk_jpeg_driver = {
1955         .probe = mtk_jpeg_probe,
1956         .remove_new = mtk_jpeg_remove,
1957         .driver = {
1958                 .name           = MTK_JPEG_NAME,
1959                 .of_match_table = of_match_ptr(mtk_jpeg_match),
1960                 .pm             = &mtk_jpeg_pm_ops,
1961         },
1962 };
1963
1964 module_platform_driver(mtk_jpeg_driver);
1965
1966 MODULE_DESCRIPTION("MediaTek JPEG codec driver");
1967 MODULE_LICENSE("GPL v2");