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 void mtk_jpeg_enc_device_run(void *priv)
 940 {
 941         struct mtk_jpeg_ctx *ctx = priv;
 942         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 943         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 944         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
 945         unsigned long flags;
 946         int ret;
 947
 948         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
 949         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
 950
 951         ret = pm_runtime_resume_and_get(jpeg->dev);
 952         if (ret < 0)
 953                 goto enc_end;
 954
 955         schedule_delayed_work(&jpeg->job_timeout_work,
 956                         msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
 957
 958         spin_lock_irqsave(&jpeg->hw_lock, flags);
 959
 960         /*
 961          * Resetting the hardware every frame is to ensure that all the
 962          * registers are cleared. This is a hardware requirement.
 963          */
 964         mtk_jpeg_enc_reset(jpeg->reg_base);
 965
 966         mtk_jpeg_set_enc_src(ctx, jpeg->reg_base, &src_buf->vb2_buf);
 967         mtk_jpeg_set_enc_dst(ctx, jpeg->reg_base, &dst_buf->vb2_buf);
 968         mtk_jpeg_set_enc_params(ctx, jpeg->reg_base);
 969         mtk_jpeg_enc_start(jpeg->reg_base);
 970         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
 971         return;
 972
 973 enc_end:
 974         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
 975         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
 976         v4l2_m2m_buf_done(src_buf, buf_state);
 977         v4l2_m2m_buf_done(dst_buf, buf_state);
 978         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
 979 }
 980
 981 static void mtk_jpeg_multicore_enc_device_run(void *priv)
 982 {
 983         struct mtk_jpeg_ctx *ctx = priv;
 984         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 985
 986         queue_work(jpeg->workqueue, &ctx->jpeg_work);
 987 }
 988
 989 static void mtk_jpeg_multicore_dec_device_run(void *priv)
 990 {
 991         struct mtk_jpeg_ctx *ctx = priv;
 992         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
 993
 994         queue_work(jpeg->workqueue, &ctx->jpeg_work);
 995 }
 996
 997 static void mtk_jpeg_dec_device_run(void *priv)
 998 {
 999         struct mtk_jpeg_ctx *ctx = priv;
1000         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1001         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1002         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1003         unsigned long flags;
1004         struct mtk_jpeg_src_buf *jpeg_src_buf;
1005         struct mtk_jpeg_bs bs;
1006         struct mtk_jpeg_fb fb;
1007         int ret;
1008
1009         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1010         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1011         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1012
1013         if (mtk_jpeg_check_resolution_change(ctx, &jpeg_src_buf->dec_param)) {
1014                 mtk_jpeg_queue_src_chg_event(ctx);
1015                 ctx->state = MTK_JPEG_SOURCE_CHANGE;
1016                 v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1017                 return;
1018         }
1019
1020         ret = pm_runtime_resume_and_get(jpeg->dev);
1021         if (ret < 0)
1022                 goto dec_end;
1023
1024         mtk_jpeg_set_dec_src(ctx, &src_buf->vb2_buf, &bs);
1025         if (mtk_jpeg_set_dec_dst(ctx, &jpeg_src_buf->dec_param, &dst_buf->vb2_buf, &fb))
1026                 goto dec_end;
1027
1028         schedule_delayed_work(&jpeg->job_timeout_work,
1029                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1030
1031         spin_lock_irqsave(&jpeg->hw_lock, flags);
1032         mtk_jpeg_dec_reset(jpeg->reg_base);
1033         mtk_jpeg_dec_set_config(jpeg->reg_base,
1034                                 &jpeg_src_buf->dec_param,
1035                                 jpeg_src_buf->bs_size,
1036                                 &bs,
1037                                 &fb);
1038         mtk_jpeg_dec_start(jpeg->reg_base);
1039         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1040         return;
1041
1042 dec_end:
1043         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1044         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1045         v4l2_m2m_buf_done(src_buf, buf_state);
1046         v4l2_m2m_buf_done(dst_buf, buf_state);
1047         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1048 }
1049
1050 static int mtk_jpeg_dec_job_ready(void *priv)
1051 {
1052         struct mtk_jpeg_ctx *ctx = priv;
1053
1054         return (ctx->state == MTK_JPEG_RUNNING) ? 1 : 0;
1055 }
1056
1057 static const struct v4l2_m2m_ops mtk_jpeg_enc_m2m_ops = {
1058         .device_run = mtk_jpeg_enc_device_run,
1059 };
1060
1061 static const struct v4l2_m2m_ops mtk_jpeg_multicore_enc_m2m_ops = {
1062         .device_run = mtk_jpeg_multicore_enc_device_run,
1063 };
1064
1065 static const struct v4l2_m2m_ops mtk_jpeg_multicore_dec_m2m_ops = {
1066         .device_run = mtk_jpeg_multicore_dec_device_run,
1067 };
1068
1069 static const struct v4l2_m2m_ops mtk_jpeg_dec_m2m_ops = {
1070         .device_run = mtk_jpeg_dec_device_run,
1071         .job_ready  = mtk_jpeg_dec_job_ready,
1072 };
1073
1074 static int mtk_jpeg_queue_init(void *priv, struct vb2_queue *src_vq,
1075                                struct vb2_queue *dst_vq)
1076 {
1077         struct mtk_jpeg_ctx *ctx = priv;
1078         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1079         int ret;
1080
1081         src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1082         src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
1083         src_vq->drv_priv = ctx;
1084         src_vq->buf_struct_size = sizeof(struct mtk_jpeg_src_buf);
1085         src_vq->ops = jpeg->variant->qops;
1086         src_vq->mem_ops = &vb2_dma_contig_memops;
1087         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1088         src_vq->lock = &ctx->jpeg->lock;
1089         src_vq->dev = ctx->jpeg->dev;
1090         ret = vb2_queue_init(src_vq);
1091         if (ret)
1092                 return ret;
1093
1094         dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1095         dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
1096         dst_vq->drv_priv = ctx;
1097         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1098         dst_vq->ops = jpeg->variant->qops;
1099         dst_vq->mem_ops = &vb2_dma_contig_memops;
1100         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1101         dst_vq->lock = &ctx->jpeg->lock;
1102         dst_vq->dev = ctx->jpeg->dev;
1103         ret = vb2_queue_init(dst_vq);
1104
1105         return ret;
1106 }
1107
1108 static void mtk_jpeg_clk_on(struct mtk_jpeg_dev *jpeg)
1109 {
1110         int ret;
1111
1112         ret = clk_bulk_prepare_enable(jpeg->variant->num_clks,
1113                                       jpeg->variant->clks);
1114         if (ret)
1115                 dev_err(jpeg->dev, "Failed to open jpeg clk: %d\n", ret);
1116 }
1117
1118 static void mtk_jpeg_clk_off(struct mtk_jpeg_dev *jpeg)
1119 {
1120         clk_bulk_disable_unprepare(jpeg->variant->num_clks,
1121                                    jpeg->variant->clks);
1122 }
1123
1124 static void mtk_jpeg_set_default_params(struct mtk_jpeg_ctx *ctx)
1125 {
1126         struct mtk_jpeg_q_data *q = &ctx->out_q;
1127         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1128
1129         ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
1130         q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
1131         q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
1132         q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
1133         q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
1134
1135         q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
1136                                       jpeg->variant->num_formats,
1137                                       jpeg->variant->out_q_default_fourcc,
1138                                       MTK_JPEG_FMT_FLAG_OUTPUT);
1139         q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
1140         q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
1141         mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
1142
1143         q = &ctx->cap_q;
1144         q->fmt = mtk_jpeg_find_format(jpeg->variant->formats,
1145                                       jpeg->variant->num_formats,
1146                                       jpeg->variant->cap_q_default_fourcc,
1147                                       MTK_JPEG_FMT_FLAG_CAPTURE);
1148         q->pix_mp.colorspace = V4L2_COLORSPACE_SRGB;
1149         q->pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_601;
1150         q->pix_mp.quantization = V4L2_QUANTIZATION_FULL_RANGE;
1151         q->pix_mp.xfer_func = V4L2_XFER_FUNC_SRGB;
1152         q->pix_mp.width = MTK_JPEG_MIN_WIDTH;
1153         q->pix_mp.height = MTK_JPEG_MIN_HEIGHT;
1154
1155         mtk_jpeg_try_fmt_mplane(&q->pix_mp, q->fmt);
1156 }
1157
1158 static int mtk_jpeg_open(struct file *file)
1159 {
1160         struct mtk_jpeg_dev *jpeg = video_drvdata(file);
1161         struct video_device *vfd = video_devdata(file);
1162         struct mtk_jpeg_ctx *ctx;
1163         int ret = 0;
1164
1165         ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
1166         if (!ctx)
1167                 return -ENOMEM;
1168
1169         if (mutex_lock_interruptible(&jpeg->lock)) {
1170                 ret = -ERESTARTSYS;
1171                 goto free;
1172         }
1173
1174         INIT_WORK(&ctx->jpeg_work, jpeg->variant->jpeg_worker);
1175         INIT_LIST_HEAD(&ctx->dst_done_queue);
1176         spin_lock_init(&ctx->done_queue_lock);
1177         v4l2_fh_init(&ctx->fh, vfd);
1178         file->private_data = &ctx->fh;
1179         v4l2_fh_add(&ctx->fh);
1180
1181         ctx->jpeg = jpeg;
1182         ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(jpeg->m2m_dev, ctx,
1183                                             mtk_jpeg_queue_init);
1184         if (IS_ERR(ctx->fh.m2m_ctx)) {
1185                 ret = PTR_ERR(ctx->fh.m2m_ctx);
1186                 goto error;
1187         }
1188
1189         if (jpeg->variant->cap_q_default_fourcc == V4L2_PIX_FMT_JPEG) {
1190                 ret = mtk_jpeg_enc_ctrls_setup(ctx);
1191                 if (ret) {
1192                         v4l2_err(&jpeg->v4l2_dev, "Failed to setup jpeg enc controls\n");
1193                         goto error;
1194                 }
1195         } else {
1196                 v4l2_ctrl_handler_init(&ctx->ctrl_hdl, 0);
1197         }
1198
1199         mtk_jpeg_set_default_params(ctx);
1200         mutex_unlock(&jpeg->lock);
1201         return 0;
1202
1203 error:
1204         v4l2_fh_del(&ctx->fh);
1205         v4l2_fh_exit(&ctx->fh);
1206         mutex_unlock(&jpeg->lock);
1207 free:
1208         kfree(ctx);
1209         return ret;
1210 }
1211
1212 static int mtk_jpeg_release(struct file *file)
1213 {
1214         struct mtk_jpeg_dev *jpeg = video_drvdata(file);
1215         struct mtk_jpeg_ctx *ctx = mtk_jpeg_fh_to_ctx(file->private_data);
1216
1217         mutex_lock(&jpeg->lock);
1218         v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
1219         v4l2_ctrl_handler_free(&ctx->ctrl_hdl);
1220         v4l2_fh_del(&ctx->fh);
1221         v4l2_fh_exit(&ctx->fh);
1222         kfree(ctx);
1223         mutex_unlock(&jpeg->lock);
1224         return 0;
1225 }
1226
1227 static const struct v4l2_file_operations mtk_jpeg_fops = {
1228         .owner          = THIS_MODULE,
1229         .open           = mtk_jpeg_open,
1230         .release        = mtk_jpeg_release,
1231         .poll           = v4l2_m2m_fop_poll,
1232         .unlocked_ioctl = video_ioctl2,
1233         .mmap           = v4l2_m2m_fop_mmap,
1234 };
1235
1236 static void mtk_jpeg_job_timeout_work(struct work_struct *work)
1237 {
1238         struct mtk_jpeg_dev *jpeg = container_of(work, struct mtk_jpeg_dev,
1239                                                  job_timeout_work.work);
1240         struct mtk_jpeg_ctx *ctx;
1241         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1242
1243         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1244         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1245         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1246
1247         jpeg->variant->hw_reset(jpeg->reg_base);
1248
1249         pm_runtime_put(jpeg->dev);
1250
1251         v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
1252         v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
1253         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1254 }
1255
1256 static int mtk_jpeg_single_core_init(struct platform_device *pdev,
1257                                      struct mtk_jpeg_dev *jpeg_dev)
1258 {
1259         struct mtk_jpeg_dev *jpeg = jpeg_dev;
1260         int jpeg_irq, ret;
1261
1262         INIT_DELAYED_WORK(&jpeg->job_timeout_work,
1263                           mtk_jpeg_job_timeout_work);
1264
1265         jpeg->reg_base = devm_platform_ioremap_resource(pdev, 0);
1266         if (IS_ERR(jpeg->reg_base)) {
1267                 ret = PTR_ERR(jpeg->reg_base);
1268                 return ret;
1269         }
1270
1271         jpeg_irq = platform_get_irq(pdev, 0);
1272         if (jpeg_irq < 0)
1273                 return jpeg_irq;
1274
1275         ret = devm_request_irq(&pdev->dev,
1276                                jpeg_irq,
1277                                jpeg->variant->irq_handler,
1278                                0,
1279                                pdev->name, jpeg);
1280         if (ret) {
1281                 dev_err(&pdev->dev, "Failed to request jpeg_irq %d (%d)\n",
1282                         jpeg_irq, ret);
1283                 return ret;
1284         }
1285
1286         ret = devm_clk_bulk_get(jpeg->dev,
1287                                 jpeg->variant->num_clks,
1288                                 jpeg->variant->clks);
1289         if (ret) {
1290                 dev_err(&pdev->dev, "Failed to init clk\n");
1291                 return ret;
1292         }
1293
1294         return 0;
1295 }
1296
1297 static int mtk_jpeg_probe(struct platform_device *pdev)
1298 {
1299         struct mtk_jpeg_dev *jpeg;
1300         struct device_node *child;
1301         int num_child = 0;
1302         int ret;
1303
1304         jpeg = devm_kzalloc(&pdev->dev, sizeof(*jpeg), GFP_KERNEL);
1305         if (!jpeg)
1306                 return -ENOMEM;
1307
1308         mutex_init(&jpeg->lock);
1309         spin_lock_init(&jpeg->hw_lock);
1310         jpeg->dev = &pdev->dev;
1311         jpeg->variant = of_device_get_match_data(jpeg->dev);
1312
1313         platform_set_drvdata(pdev, jpeg);
1314
1315         ret = devm_of_platform_populate(&pdev->dev);
1316         if (ret) {
1317                 v4l2_err(&jpeg->v4l2_dev, "Master of platform populate failed.");
1318                 return -EINVAL;
1319         }
1320
1321         if (!jpeg->variant->multi_core) {
1322                 ret = mtk_jpeg_single_core_init(pdev, jpeg);
1323                 if (ret) {
1324                         v4l2_err(&jpeg->v4l2_dev, "mtk_jpeg_single_core_init failed.");
1325                         return -EINVAL;
1326                 }
1327         } else {
1328                 init_waitqueue_head(&jpeg->hw_wq);
1329
1330                 for_each_child_of_node(pdev->dev.of_node, child)
1331                         num_child++;
1332
1333                 atomic_set(&jpeg->hw_rdy, num_child);
1334                 atomic_set(&jpeg->hw_index, 0);
1335
1336                 jpeg->workqueue = alloc_ordered_workqueue(MTK_JPEG_NAME,
1337                                                           WQ_MEM_RECLAIM
1338                                                           | WQ_FREEZABLE);
1339                 if (!jpeg->workqueue)
1340                         return -EINVAL;
1341         }
1342
1343         ret = v4l2_device_register(&pdev->dev, &jpeg->v4l2_dev);
1344         if (ret) {
1345                 dev_err(&pdev->dev, "Failed to register v4l2 device\n");
1346                 return -EINVAL;
1347         }
1348
1349         jpeg->m2m_dev = v4l2_m2m_init(jpeg->variant->m2m_ops);
1350
1351         if (IS_ERR(jpeg->m2m_dev)) {
1352                 v4l2_err(&jpeg->v4l2_dev, "Failed to init mem2mem device\n");
1353                 ret = PTR_ERR(jpeg->m2m_dev);
1354                 goto err_m2m_init;
1355         }
1356
1357         jpeg->vdev = video_device_alloc();
1358         if (!jpeg->vdev) {
1359                 ret = -ENOMEM;
1360                 goto err_vfd_jpeg_alloc;
1361         }
1362         snprintf(jpeg->vdev->name, sizeof(jpeg->vdev->name),
1363                  "%s", jpeg->variant->dev_name);
1364         jpeg->vdev->fops = &mtk_jpeg_fops;
1365         jpeg->vdev->ioctl_ops = jpeg->variant->ioctl_ops;
1366         jpeg->vdev->minor = -1;
1367         jpeg->vdev->release = video_device_release;
1368         jpeg->vdev->lock = &jpeg->lock;
1369         jpeg->vdev->v4l2_dev = &jpeg->v4l2_dev;
1370         jpeg->vdev->vfl_dir = VFL_DIR_M2M;
1371         jpeg->vdev->device_caps = V4L2_CAP_STREAMING |
1372                                   V4L2_CAP_VIDEO_M2M_MPLANE;
1373
1374         ret = video_register_device(jpeg->vdev, VFL_TYPE_VIDEO, -1);
1375         if (ret) {
1376                 v4l2_err(&jpeg->v4l2_dev, "Failed to register video device\n");
1377                 goto err_vfd_jpeg_register;
1378         }
1379
1380         video_set_drvdata(jpeg->vdev, jpeg);
1381         v4l2_info(&jpeg->v4l2_dev,
1382                   "%s device registered as /dev/video%d (%d,%d)\n",
1383                   jpeg->variant->dev_name, jpeg->vdev->num,
1384                   VIDEO_MAJOR, jpeg->vdev->minor);
1385
1386         pm_runtime_enable(&pdev->dev);
1387
1388         return 0;
1389
1390 err_vfd_jpeg_register:
1391         video_device_release(jpeg->vdev);
1392
1393 err_vfd_jpeg_alloc:
1394         v4l2_m2m_release(jpeg->m2m_dev);
1395
1396 err_m2m_init:
1397         v4l2_device_unregister(&jpeg->v4l2_dev);
1398
1399         return ret;
1400 }
1401
1402 static void mtk_jpeg_remove(struct platform_device *pdev)
1403 {
1404         struct mtk_jpeg_dev *jpeg = platform_get_drvdata(pdev);
1405
1406         pm_runtime_disable(&pdev->dev);
1407         video_unregister_device(jpeg->vdev);
1408         v4l2_m2m_release(jpeg->m2m_dev);
1409         v4l2_device_unregister(&jpeg->v4l2_dev);
1410 }
1411
1412 static __maybe_unused int mtk_jpeg_pm_suspend(struct device *dev)
1413 {
1414         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1415
1416         mtk_jpeg_clk_off(jpeg);
1417
1418         return 0;
1419 }
1420
1421 static __maybe_unused int mtk_jpeg_pm_resume(struct device *dev)
1422 {
1423         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1424
1425         mtk_jpeg_clk_on(jpeg);
1426
1427         return 0;
1428 }
1429
1430 static __maybe_unused int mtk_jpeg_suspend(struct device *dev)
1431 {
1432         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1433
1434         v4l2_m2m_suspend(jpeg->m2m_dev);
1435         return pm_runtime_force_suspend(dev);
1436 }
1437
1438 static __maybe_unused int mtk_jpeg_resume(struct device *dev)
1439 {
1440         struct mtk_jpeg_dev *jpeg = dev_get_drvdata(dev);
1441         int ret;
1442
1443         ret = pm_runtime_force_resume(dev);
1444         if (ret < 0)
1445                 return ret;
1446
1447         v4l2_m2m_resume(jpeg->m2m_dev);
1448         return ret;
1449 }
1450
1451 static const struct dev_pm_ops mtk_jpeg_pm_ops = {
1452         SET_SYSTEM_SLEEP_PM_OPS(mtk_jpeg_suspend, mtk_jpeg_resume)
1453         SET_RUNTIME_PM_OPS(mtk_jpeg_pm_suspend, mtk_jpeg_pm_resume, NULL)
1454 };
1455
1456 static int mtk_jpegenc_get_hw(struct mtk_jpeg_ctx *ctx)
1457 {
1458         struct mtk_jpegenc_comp_dev *comp_jpeg;
1459         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1460         unsigned long flags;
1461         int hw_id = -1;
1462         int i;
1463
1464         spin_lock_irqsave(&jpeg->hw_lock, flags);
1465         for (i = 0; i < MTK_JPEGENC_HW_MAX; i++) {
1466                 comp_jpeg = jpeg->enc_hw_dev[i];
1467                 if (comp_jpeg->hw_state == MTK_JPEG_HW_IDLE) {
1468                         hw_id = i;
1469                         comp_jpeg->hw_state = MTK_JPEG_HW_BUSY;
1470                         break;
1471                 }
1472         }
1473         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1474
1475         return hw_id;
1476 }
1477
1478 static int mtk_jpegenc_set_hw_param(struct mtk_jpeg_ctx *ctx,
1479                                     int hw_id,
1480                                     struct vb2_v4l2_buffer *src_buf,
1481                                     struct vb2_v4l2_buffer *dst_buf)
1482 {
1483         struct mtk_jpegenc_comp_dev *jpeg = ctx->jpeg->enc_hw_dev[hw_id];
1484
1485         jpeg->hw_param.curr_ctx = ctx;
1486         jpeg->hw_param.src_buffer = src_buf;
1487         jpeg->hw_param.dst_buffer = dst_buf;
1488
1489         return 0;
1490 }
1491
1492 static int mtk_jpegenc_put_hw(struct mtk_jpeg_dev *jpeg, int hw_id)
1493 {
1494         unsigned long flags;
1495
1496         spin_lock_irqsave(&jpeg->hw_lock, flags);
1497         jpeg->enc_hw_dev[hw_id]->hw_state = MTK_JPEG_HW_IDLE;
1498         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1499
1500         return 0;
1501 }
1502
1503 static int mtk_jpegdec_get_hw(struct mtk_jpeg_ctx *ctx)
1504 {
1505         struct mtk_jpegdec_comp_dev *comp_jpeg;
1506         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1507         unsigned long flags;
1508         int hw_id = -1;
1509         int i;
1510
1511         spin_lock_irqsave(&jpeg->hw_lock, flags);
1512         for (i = 0; i < MTK_JPEGDEC_HW_MAX; i++) {
1513                 comp_jpeg = jpeg->dec_hw_dev[i];
1514                 if (comp_jpeg->hw_state == MTK_JPEG_HW_IDLE) {
1515                         hw_id = i;
1516                         comp_jpeg->hw_state = MTK_JPEG_HW_BUSY;
1517                         break;
1518                 }
1519         }
1520         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1521
1522         return hw_id;
1523 }
1524
1525 static int mtk_jpegdec_put_hw(struct mtk_jpeg_dev *jpeg, int hw_id)
1526 {
1527         unsigned long flags;
1528
1529         spin_lock_irqsave(&jpeg->hw_lock, flags);
1530         jpeg->dec_hw_dev[hw_id]->hw_state =
1531                 MTK_JPEG_HW_IDLE;
1532         spin_unlock_irqrestore(&jpeg->hw_lock, flags);
1533
1534         return 0;
1535 }
1536
1537 static int mtk_jpegdec_set_hw_param(struct mtk_jpeg_ctx *ctx,
1538                                     int hw_id,
1539                                     struct vb2_v4l2_buffer *src_buf,
1540                                     struct vb2_v4l2_buffer *dst_buf)
1541 {
1542         struct mtk_jpegdec_comp_dev *jpeg =
1543                 ctx->jpeg->dec_hw_dev[hw_id];
1544
1545         jpeg->hw_param.curr_ctx = ctx;
1546         jpeg->hw_param.src_buffer = src_buf;
1547         jpeg->hw_param.dst_buffer = dst_buf;
1548
1549         return 0;
1550 }
1551
1552 static irqreturn_t mtk_jpeg_enc_done(struct mtk_jpeg_dev *jpeg)
1553 {
1554         struct mtk_jpeg_ctx *ctx;
1555         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1556         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1557         u32 result_size;
1558
1559         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1560         if (!ctx) {
1561                 v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
1562                 return IRQ_HANDLED;
1563         }
1564
1565         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1566         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1567
1568         result_size = mtk_jpeg_enc_get_file_size(jpeg->reg_base);
1569         vb2_set_plane_payload(&dst_buf->vb2_buf, 0, result_size);
1570
1571         buf_state = VB2_BUF_STATE_DONE;
1572
1573         v4l2_m2m_buf_done(src_buf, buf_state);
1574         v4l2_m2m_buf_done(dst_buf, buf_state);
1575         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1576         pm_runtime_put(ctx->jpeg->dev);
1577         return IRQ_HANDLED;
1578 }
1579
1580 static void mtk_jpegenc_worker(struct work_struct *work)
1581 {
1582         struct mtk_jpegenc_comp_dev *comp_jpeg[MTK_JPEGENC_HW_MAX];
1583         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1584         struct mtk_jpeg_src_buf *jpeg_dst_buf;
1585         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1586         int ret, i, hw_id = 0;
1587         unsigned long flags;
1588
1589         struct mtk_jpeg_ctx *ctx = container_of(work,
1590                 struct mtk_jpeg_ctx,
1591                 jpeg_work);
1592         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1593
1594         for (i = 0; i < MTK_JPEGENC_HW_MAX; i++)
1595                 comp_jpeg[i] = jpeg->enc_hw_dev[i];
1596         i = 0;
1597
1598 retry_select:
1599         hw_id = mtk_jpegenc_get_hw(ctx);
1600         if (hw_id < 0) {
1601                 ret = wait_event_interruptible(jpeg->hw_wq,
1602                                                atomic_read(&jpeg->hw_rdy) > 0);
1603                 if (ret != 0 || (i++ > MTK_JPEG_MAX_RETRY_TIME)) {
1604                         dev_err(jpeg->dev, "%s : %d, all HW are busy\n",
1605                                 __func__, __LINE__);
1606                         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1607                         return;
1608                 }
1609
1610                 goto retry_select;
1611         }
1612
1613         atomic_dec(&jpeg->hw_rdy);
1614         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1615         if (!src_buf)
1616                 goto getbuf_fail;
1617
1618         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1619         if (!dst_buf)
1620                 goto getbuf_fail;
1621
1622         v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
1623
1624         mtk_jpegenc_set_hw_param(ctx, hw_id, src_buf, dst_buf);
1625         ret = pm_runtime_get_sync(comp_jpeg[hw_id]->dev);
1626         if (ret < 0) {
1627                 dev_err(jpeg->dev, "%s : %d, pm_runtime_get_sync fail !!!\n",
1628                         __func__, __LINE__);
1629                 goto enc_end;
1630         }
1631
1632         ret = clk_prepare_enable(comp_jpeg[hw_id]->venc_clk.clks->clk);
1633         if (ret) {
1634                 dev_err(jpeg->dev, "%s : %d, jpegenc clk_prepare_enable fail\n",
1635                         __func__, __LINE__);
1636                 goto enc_end;
1637         }
1638
1639         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1640         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1641
1642         schedule_delayed_work(&comp_jpeg[hw_id]->job_timeout_work,
1643                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1644
1645         spin_lock_irqsave(&comp_jpeg[hw_id]->hw_lock, flags);
1646         jpeg_dst_buf = mtk_jpeg_vb2_to_srcbuf(&dst_buf->vb2_buf);
1647         jpeg_dst_buf->curr_ctx = ctx;
1648         jpeg_dst_buf->frame_num = ctx->total_frame_num;
1649         ctx->total_frame_num++;
1650         mtk_jpeg_enc_reset(comp_jpeg[hw_id]->reg_base);
1651         mtk_jpeg_set_enc_dst(ctx,
1652                              comp_jpeg[hw_id]->reg_base,
1653                              &dst_buf->vb2_buf);
1654         mtk_jpeg_set_enc_src(ctx,
1655                              comp_jpeg[hw_id]->reg_base,
1656                              &src_buf->vb2_buf);
1657         mtk_jpeg_set_enc_params(ctx, comp_jpeg[hw_id]->reg_base);
1658         mtk_jpeg_enc_start(comp_jpeg[hw_id]->reg_base);
1659         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1660         spin_unlock_irqrestore(&comp_jpeg[hw_id]->hw_lock, flags);
1661
1662         return;
1663
1664 enc_end:
1665         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1666         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1667         v4l2_m2m_buf_done(src_buf, buf_state);
1668         v4l2_m2m_buf_done(dst_buf, buf_state);
1669 getbuf_fail:
1670         atomic_inc(&jpeg->hw_rdy);
1671         mtk_jpegenc_put_hw(jpeg, hw_id);
1672         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1673 }
1674
1675 static void mtk_jpegdec_worker(struct work_struct *work)
1676 {
1677         struct mtk_jpeg_ctx *ctx = container_of(work, struct mtk_jpeg_ctx,
1678                 jpeg_work);
1679         struct mtk_jpegdec_comp_dev *comp_jpeg[MTK_JPEGDEC_HW_MAX];
1680         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1681         struct mtk_jpeg_src_buf *jpeg_src_buf, *jpeg_dst_buf;
1682         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1683         struct mtk_jpeg_dev *jpeg = ctx->jpeg;
1684         int ret, i, hw_id = 0;
1685         struct mtk_jpeg_bs bs;
1686         struct mtk_jpeg_fb fb;
1687         unsigned long flags;
1688
1689         for (i = 0; i < MTK_JPEGDEC_HW_MAX; i++)
1690                 comp_jpeg[i] = jpeg->dec_hw_dev[i];
1691         i = 0;
1692
1693 retry_select:
1694         hw_id = mtk_jpegdec_get_hw(ctx);
1695         if (hw_id < 0) {
1696                 ret = wait_event_interruptible_timeout(jpeg->hw_wq,
1697                                                        atomic_read(&jpeg->hw_rdy) > 0,
1698                                                        MTK_JPEG_HW_TIMEOUT_MSEC);
1699                 if (ret != 0 || (i++ > MTK_JPEG_MAX_RETRY_TIME)) {
1700                         dev_err(jpeg->dev, "%s : %d, all HW are busy\n",
1701                                 __func__, __LINE__);
1702                         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1703                         return;
1704                 }
1705
1706                 goto retry_select;
1707         }
1708
1709         atomic_dec(&jpeg->hw_rdy);
1710         src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
1711         if (!src_buf)
1712                 goto getbuf_fail;
1713
1714         dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
1715         if (!dst_buf)
1716                 goto getbuf_fail;
1717
1718         v4l2_m2m_buf_copy_metadata(src_buf, dst_buf, true);
1719         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1720         jpeg_dst_buf = mtk_jpeg_vb2_to_srcbuf(&dst_buf->vb2_buf);
1721
1722         if (mtk_jpeg_check_resolution_change(ctx,
1723                                              &jpeg_src_buf->dec_param)) {
1724                 mtk_jpeg_queue_src_chg_event(ctx);
1725                 ctx->state = MTK_JPEG_SOURCE_CHANGE;
1726                 goto getbuf_fail;
1727         }
1728
1729         jpeg_src_buf->curr_ctx = ctx;
1730         jpeg_src_buf->frame_num = ctx->total_frame_num;
1731         jpeg_dst_buf->curr_ctx = ctx;
1732         jpeg_dst_buf->frame_num = ctx->total_frame_num;
1733
1734         mtk_jpegdec_set_hw_param(ctx, hw_id, src_buf, dst_buf);
1735         ret = pm_runtime_get_sync(comp_jpeg[hw_id]->dev);
1736         if (ret < 0) {
1737                 dev_err(jpeg->dev, "%s : %d, pm_runtime_get_sync fail !!!\n",
1738                         __func__, __LINE__);
1739                 goto dec_end;
1740         }
1741
1742         ret = clk_prepare_enable(comp_jpeg[hw_id]->jdec_clk.clks->clk);
1743         if (ret) {
1744                 dev_err(jpeg->dev, "%s : %d, jpegdec clk_prepare_enable fail\n",
1745                         __func__, __LINE__);
1746                 goto clk_end;
1747         }
1748
1749         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1750         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1751
1752         mtk_jpeg_set_dec_src(ctx, &src_buf->vb2_buf, &bs);
1753         if (mtk_jpeg_set_dec_dst(ctx,
1754                                  &jpeg_src_buf->dec_param,
1755                                  &dst_buf->vb2_buf, &fb)) {
1756                 dev_err(jpeg->dev, "%s : %d, mtk_jpeg_set_dec_dst fail\n",
1757                         __func__, __LINE__);
1758                 goto setdst_end;
1759         }
1760
1761         schedule_delayed_work(&comp_jpeg[hw_id]->job_timeout_work,
1762                               msecs_to_jiffies(MTK_JPEG_HW_TIMEOUT_MSEC));
1763
1764         spin_lock_irqsave(&comp_jpeg[hw_id]->hw_lock, flags);
1765         ctx->total_frame_num++;
1766         mtk_jpeg_dec_reset(comp_jpeg[hw_id]->reg_base);
1767         mtk_jpeg_dec_set_config(comp_jpeg[hw_id]->reg_base,
1768                                 &jpeg_src_buf->dec_param,
1769                                 jpeg_src_buf->bs_size,
1770                                 &bs,
1771                                 &fb);
1772         mtk_jpeg_dec_start(comp_jpeg[hw_id]->reg_base);
1773         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1774         spin_unlock_irqrestore(&comp_jpeg[hw_id]->hw_lock, flags);
1775
1776         return;
1777
1778 setdst_end:
1779         clk_disable_unprepare(comp_jpeg[hw_id]->jdec_clk.clks->clk);
1780 clk_end:
1781         pm_runtime_put(comp_jpeg[hw_id]->dev);
1782 dec_end:
1783         v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1784         v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1785         v4l2_m2m_buf_done(src_buf, buf_state);
1786         v4l2_m2m_buf_done(dst_buf, buf_state);
1787 getbuf_fail:
1788         atomic_inc(&jpeg->hw_rdy);
1789         mtk_jpegdec_put_hw(jpeg, hw_id);
1790         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1791 }
1792
1793 static irqreturn_t mtk_jpeg_enc_irq(int irq, void *priv)
1794 {
1795         struct mtk_jpeg_dev *jpeg = priv;
1796         u32 irq_status;
1797         irqreturn_t ret = IRQ_NONE;
1798
1799         cancel_delayed_work(&jpeg->job_timeout_work);
1800
1801         irq_status = readl(jpeg->reg_base + JPEG_ENC_INT_STS) &
1802                      JPEG_ENC_INT_STATUS_MASK_ALLIRQ;
1803         if (irq_status)
1804                 writel(0, jpeg->reg_base + JPEG_ENC_INT_STS);
1805
1806         if (!(irq_status & JPEG_ENC_INT_STATUS_DONE))
1807                 return ret;
1808
1809         ret = mtk_jpeg_enc_done(jpeg);
1810         return ret;
1811 }
1812
1813 static irqreturn_t mtk_jpeg_dec_irq(int irq, void *priv)
1814 {
1815         struct mtk_jpeg_dev *jpeg = priv;
1816         struct mtk_jpeg_ctx *ctx;
1817         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1818         struct mtk_jpeg_src_buf *jpeg_src_buf;
1819         enum vb2_buffer_state buf_state = VB2_BUF_STATE_ERROR;
1820         u32     dec_irq_ret;
1821         u32 dec_ret;
1822         int i;
1823
1824         cancel_delayed_work(&jpeg->job_timeout_work);
1825
1826         dec_ret = mtk_jpeg_dec_get_int_status(jpeg->reg_base);
1827         dec_irq_ret = mtk_jpeg_dec_enum_result(dec_ret);
1828         ctx = v4l2_m2m_get_curr_priv(jpeg->m2m_dev);
1829         if (!ctx) {
1830                 v4l2_err(&jpeg->v4l2_dev, "Context is NULL\n");
1831                 return IRQ_HANDLED;
1832         }
1833
1834         src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
1835         dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
1836         jpeg_src_buf = mtk_jpeg_vb2_to_srcbuf(&src_buf->vb2_buf);
1837
1838         if (dec_irq_ret >= MTK_JPEG_DEC_RESULT_UNDERFLOW)
1839                 mtk_jpeg_dec_reset(jpeg->reg_base);
1840
1841         if (dec_irq_ret != MTK_JPEG_DEC_RESULT_EOF_DONE) {
1842                 dev_err(jpeg->dev, "decode failed\n");
1843                 goto dec_end;
1844         }
1845
1846         for (i = 0; i < dst_buf->vb2_buf.num_planes; i++)
1847                 vb2_set_plane_payload(&dst_buf->vb2_buf, i,
1848                                       jpeg_src_buf->dec_param.comp_size[i]);
1849
1850         buf_state = VB2_BUF_STATE_DONE;
1851
1852 dec_end:
1853         v4l2_m2m_buf_done(src_buf, buf_state);
1854         v4l2_m2m_buf_done(dst_buf, buf_state);
1855         v4l2_m2m_job_finish(jpeg->m2m_dev, ctx->fh.m2m_ctx);
1856         pm_runtime_put(ctx->jpeg->dev);
1857         return IRQ_HANDLED;
1858 }
1859
1860 static struct clk_bulk_data mtk_jpeg_clocks[] = {
1861         { .id = "jpgenc" },
1862 };
1863
1864 static struct clk_bulk_data mt8173_jpeg_dec_clocks[] = {
1865         { .id = "jpgdec-smi" },
1866         { .id = "jpgdec" },
1867 };
1868
1869 static const struct mtk_jpeg_variant mt8173_jpeg_drvdata = {
1870         .clks = mt8173_jpeg_dec_clocks,
1871         .num_clks = ARRAY_SIZE(mt8173_jpeg_dec_clocks),
1872         .formats = mtk_jpeg_dec_formats,
1873         .num_formats = MTK_JPEG_DEC_NUM_FORMATS,
1874         .qops = &mtk_jpeg_dec_qops,
1875         .irq_handler = mtk_jpeg_dec_irq,
1876         .hw_reset = mtk_jpeg_dec_reset,
1877         .m2m_ops = &mtk_jpeg_dec_m2m_ops,
1878         .dev_name = "mtk-jpeg-dec",
1879         .ioctl_ops = &mtk_jpeg_dec_ioctl_ops,
1880         .out_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1881         .cap_q_default_fourcc = V4L2_PIX_FMT_YUV420M,
1882 };
1883
1884 static const struct mtk_jpeg_variant mtk_jpeg_drvdata = {
1885         .clks = mtk_jpeg_clocks,
1886         .num_clks = ARRAY_SIZE(mtk_jpeg_clocks),
1887         .formats = mtk_jpeg_enc_formats,
1888         .num_formats = MTK_JPEG_ENC_NUM_FORMATS,
1889         .qops = &mtk_jpeg_enc_qops,
1890         .irq_handler = mtk_jpeg_enc_irq,
1891         .hw_reset = mtk_jpeg_enc_reset,
1892         .m2m_ops = &mtk_jpeg_enc_m2m_ops,
1893         .dev_name = "mtk-jpeg-enc",
1894         .ioctl_ops = &mtk_jpeg_enc_ioctl_ops,
1895         .out_q_default_fourcc = V4L2_PIX_FMT_YUYV,
1896         .cap_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1897         .multi_core = false,
1898 };
1899
1900 static struct mtk_jpeg_variant mtk8195_jpegenc_drvdata = {
1901         .formats = mtk_jpeg_enc_formats,
1902         .num_formats = MTK_JPEG_ENC_NUM_FORMATS,
1903         .qops = &mtk_jpeg_enc_qops,
1904         .m2m_ops = &mtk_jpeg_multicore_enc_m2m_ops,
1905         .dev_name = "mtk-jpeg-enc",
1906         .ioctl_ops = &mtk_jpeg_enc_ioctl_ops,
1907         .out_q_default_fourcc = V4L2_PIX_FMT_YUYV,
1908         .cap_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1909         .multi_core = true,
1910         .jpeg_worker = mtk_jpegenc_worker,
1911 };
1912
1913 static const struct mtk_jpeg_variant mtk8195_jpegdec_drvdata = {
1914         .formats = mtk_jpeg_dec_formats,
1915         .num_formats = MTK_JPEG_DEC_NUM_FORMATS,
1916         .qops = &mtk_jpeg_dec_qops,
1917         .m2m_ops = &mtk_jpeg_multicore_dec_m2m_ops,
1918         .dev_name = "mtk-jpeg-dec",
1919         .ioctl_ops = &mtk_jpeg_dec_ioctl_ops,
1920         .out_q_default_fourcc = V4L2_PIX_FMT_JPEG,
1921         .cap_q_default_fourcc = V4L2_PIX_FMT_YUV420M,
1922         .multi_core = true,
1923         .jpeg_worker = mtk_jpegdec_worker,
1924 };
1925
1926 static const struct of_device_id mtk_jpeg_match[] = {
1927         {
1928                 .compatible = "mediatek,mt8173-jpgdec",
1929                 .data = &mt8173_jpeg_drvdata,
1930         },
1931         {
1932                 .compatible = "mediatek,mt2701-jpgdec",
1933                 .data = &mt8173_jpeg_drvdata,
1934         },
1935         {
1936                 .compatible = "mediatek,mtk-jpgenc",
1937                 .data = &mtk_jpeg_drvdata,
1938         },
1939         {
1940                 .compatible = "mediatek,mt8195-jpgenc",
1941                 .data = &mtk8195_jpegenc_drvdata,
1942         },
1943         {
1944                 .compatible = "mediatek,mt8195-jpgdec",
1945                 .data = &mtk8195_jpegdec_drvdata,
1946         },
1947         {},
1948 };
1949
1950 MODULE_DEVICE_TABLE(of, mtk_jpeg_match);
1951
1952 static struct platform_driver mtk_jpeg_driver = {
1953         .probe = mtk_jpeg_probe,
1954         .remove_new = mtk_jpeg_remove,
1955         .driver = {
1956                 .name           = MTK_JPEG_NAME,
1957                 .of_match_table = mtk_jpeg_match,
1958                 .pm             = &mtk_jpeg_pm_ops,
1959         },
1960 };
1961
1962 module_platform_driver(mtk_jpeg_driver);
1963
1964 MODULE_DESCRIPTION("MediaTek JPEG codec driver");
1965 MODULE_LICENSE("GPL v2");