drivers/media/platform/mtk-vcodec/mtk_vcodec_dec.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Copyright (c) 2016 MediaTek Inc.
   4  * Author: PC Chen <[email protected]>
   5  *         Tiffany Lin <[email protected]>
   6  */
   7
   8 #include <media/v4l2-event.h>
   9 #include <media/v4l2-mem2mem.h>
  10 #include <media/videobuf2-dma-contig.h>
  11
  12 #include "mtk_vcodec_drv.h"
  13 #include "mtk_vcodec_dec.h"
  14 #include "mtk_vcodec_intr.h"
  15 #include "mtk_vcodec_util.h"
  16 #include "vdec_drv_if.h"
  17 #include "mtk_vcodec_dec_pm.h"
  18
  19 #define OUT_FMT_IDX     0
  20 #define CAP_FMT_IDX     3
  21
  22 #define MTK_VDEC_MIN_W  64U
  23 #define MTK_VDEC_MIN_H  64U
  24 #define DFT_CFG_WIDTH   MTK_VDEC_MIN_W
  25 #define DFT_CFG_HEIGHT  MTK_VDEC_MIN_H
  26
  27 static const struct mtk_video_fmt mtk_video_formats[] = {
  28         {
  29                 .fourcc = V4L2_PIX_FMT_H264,
  30                 .type = MTK_FMT_DEC,
  31                 .num_planes = 1,
  32                 .flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
  33         },
  34         {
  35                 .fourcc = V4L2_PIX_FMT_VP8,
  36                 .type = MTK_FMT_DEC,
  37                 .num_planes = 1,
  38                 .flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
  39         },
  40         {
  41                 .fourcc = V4L2_PIX_FMT_VP9,
  42                 .type = MTK_FMT_DEC,
  43                 .num_planes = 1,
  44                 .flags = V4L2_FMT_FLAG_DYN_RESOLUTION,
  45         },
  46         {
  47                 .fourcc = V4L2_PIX_FMT_MT21C,
  48                 .type = MTK_FMT_FRAME,
  49                 .num_planes = 2,
  50         },
  51 };
  52
  53 static const struct mtk_codec_framesizes mtk_vdec_framesizes[] = {
  54         {
  55                 .fourcc = V4L2_PIX_FMT_H264,
  56                 .stepwise = {  MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
  57                                 MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
  58         },
  59         {
  60                 .fourcc = V4L2_PIX_FMT_VP8,
  61                 .stepwise = {  MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
  62                                 MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
  63         },
  64         {
  65                 .fourcc = V4L2_PIX_FMT_VP9,
  66                 .stepwise = {  MTK_VDEC_MIN_W, MTK_VDEC_MAX_W, 16,
  67                                 MTK_VDEC_MIN_H, MTK_VDEC_MAX_H, 16 },
  68         },
  69 };
  70
  71 #define NUM_SUPPORTED_FRAMESIZE ARRAY_SIZE(mtk_vdec_framesizes)
  72 #define NUM_FORMATS ARRAY_SIZE(mtk_video_formats)
  73
  74 static const struct mtk_video_fmt *mtk_vdec_find_format(struct v4l2_format *f)
  75 {
  76         const struct mtk_video_fmt *fmt;
  77         unsigned int k;
  78
  79         for (k = 0; k < NUM_FORMATS; k++) {
  80                 fmt = &mtk_video_formats[k];
  81                 if (fmt->fourcc == f->fmt.pix_mp.pixelformat)
  82                         return fmt;
  83         }
  84
  85         return NULL;
  86 }
  87
  88 static struct mtk_q_data *mtk_vdec_get_q_data(struct mtk_vcodec_ctx *ctx,
  89                                               enum v4l2_buf_type type)
  90 {
  91         if (V4L2_TYPE_IS_OUTPUT(type))
  92                 return &ctx->q_data[MTK_Q_DATA_SRC];
  93
  94         return &ctx->q_data[MTK_Q_DATA_DST];
  95 }
  96
  97 /*
  98  * This function tries to clean all display buffers, the buffers will return
  99  * in display order.
 100  * Note the buffers returned from codec driver may still be in driver's
 101  * reference list.
 102  */
 103 static struct vb2_buffer *get_display_buffer(struct mtk_vcodec_ctx *ctx)
 104 {
 105         struct vdec_fb *disp_frame_buffer = NULL;
 106         struct mtk_video_dec_buf *dstbuf;
 107         struct vb2_v4l2_buffer *vb;
 108
 109         mtk_v4l2_debug(3, "[%d]", ctx->id);
 110         if (vdec_if_get_param(ctx,
 111                         GET_PARAM_DISP_FRAME_BUFFER,
 112                         &disp_frame_buffer)) {
 113                 mtk_v4l2_err("[%d]Cannot get param : GET_PARAM_DISP_FRAME_BUFFER",
 114                         ctx->id);
 115                 return NULL;
 116         }
 117
 118         if (disp_frame_buffer == NULL) {
 119                 mtk_v4l2_debug(3, "No display frame buffer");
 120                 return NULL;
 121         }
 122
 123         dstbuf = container_of(disp_frame_buffer, struct mtk_video_dec_buf,
 124                                 frame_buffer);
 125         vb = &dstbuf->m2m_buf.vb;
 126         mutex_lock(&ctx->lock);
 127         if (dstbuf->used) {
 128                 vb2_set_plane_payload(&vb->vb2_buf, 0,
 129                                       ctx->picinfo.fb_sz[0]);
 130                 if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
 131                         vb2_set_plane_payload(&vb->vb2_buf, 1,
 132                                               ctx->picinfo.fb_sz[1]);
 133
 134                 mtk_v4l2_debug(2,
 135                                 "[%d]status=%x queue id=%d to done_list %d",
 136                                 ctx->id, disp_frame_buffer->status,
 137                                 vb->vb2_buf.index,
 138                                 dstbuf->queued_in_vb2);
 139
 140                 v4l2_m2m_buf_done(vb, VB2_BUF_STATE_DONE);
 141                 ctx->decoded_frame_cnt++;
 142         }
 143         mutex_unlock(&ctx->lock);
 144         return &vb->vb2_buf;
 145 }
 146
 147 /*
 148  * This function tries to clean all capture buffers that are not used as
 149  * reference buffers by codec driver any more
 150  * In this case, we need re-queue buffer to vb2 buffer if user space
 151  * already returns this buffer to v4l2 or this buffer is just the output of
 152  * previous sps/pps/resolution change decode, or do nothing if user
 153  * space still owns this buffer
 154  */
 155 static struct vb2_buffer *get_free_buffer(struct mtk_vcodec_ctx *ctx)
 156 {
 157         struct mtk_video_dec_buf *dstbuf;
 158         struct vdec_fb *free_frame_buffer = NULL;
 159         struct vb2_v4l2_buffer *vb;
 160
 161         if (vdec_if_get_param(ctx,
 162                                 GET_PARAM_FREE_FRAME_BUFFER,
 163                                 &free_frame_buffer)) {
 164                 mtk_v4l2_err("[%d] Error!! Cannot get param", ctx->id);
 165                 return NULL;
 166         }
 167         if (free_frame_buffer == NULL) {
 168                 mtk_v4l2_debug(3, " No free frame buffer");
 169                 return NULL;
 170         }
 171
 172         mtk_v4l2_debug(3, "[%d] tmp_frame_addr = 0x%p",
 173                         ctx->id, free_frame_buffer);
 174
 175         dstbuf = container_of(free_frame_buffer, struct mtk_video_dec_buf,
 176                                 frame_buffer);
 177         vb = &dstbuf->m2m_buf.vb;
 178
 179         mutex_lock(&ctx->lock);
 180         if (dstbuf->used) {
 181                 if ((dstbuf->queued_in_vb2) &&
 182                     (dstbuf->queued_in_v4l2) &&
 183                     (free_frame_buffer->status == FB_ST_FREE)) {
 184                         /*
 185                          * After decode sps/pps or non-display buffer, we don't
 186                          * need to return capture buffer to user space, but
 187                          * just re-queue this capture buffer to vb2 queue.
 188                          * This reduce overheads that dq/q unused capture
 189                          * buffer. In this case, queued_in_vb2 = true.
 190                          */
 191                         mtk_v4l2_debug(2,
 192                                 "[%d]status=%x queue id=%d to rdy_queue %d",
 193                                 ctx->id, free_frame_buffer->status,
 194                                 vb->vb2_buf.index,
 195                                 dstbuf->queued_in_vb2);
 196                         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
 197                 } else if (!dstbuf->queued_in_vb2 && dstbuf->queued_in_v4l2) {
 198                         /*
 199                          * If buffer in v4l2 driver but not in vb2 queue yet,
 200                          * and we get this buffer from free_list, it means
 201                          * that codec driver do not use this buffer as
 202                          * reference buffer anymore. We should q buffer to vb2
 203                          * queue, so later work thread could get this buffer
 204                          * for decode. In this case, queued_in_vb2 = false
 205                          * means this buffer is not from previous decode
 206                          * output.
 207                          */
 208                         mtk_v4l2_debug(2,
 209                                         "[%d]status=%x queue id=%d to rdy_queue",
 210                                         ctx->id, free_frame_buffer->status,
 211                                         vb->vb2_buf.index);
 212                         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb);
 213                         dstbuf->queued_in_vb2 = true;
 214                 } else {
 215                         /*
 216                          * Codec driver do not need to reference this capture
 217                          * buffer and this buffer is not in v4l2 driver.
 218                          * Then we don't need to do any thing, just add log when
 219                          * we need to debug buffer flow.
 220                          * When this buffer q from user space, it could
 221                          * directly q to vb2 buffer
 222                          */
 223                         mtk_v4l2_debug(3, "[%d]status=%x err queue id=%d %d %d",
 224                                         ctx->id, free_frame_buffer->status,
 225                                         vb->vb2_buf.index,
 226                                         dstbuf->queued_in_vb2,
 227                                         dstbuf->queued_in_v4l2);
 228                 }
 229                 dstbuf->used = false;
 230         }
 231         mutex_unlock(&ctx->lock);
 232         return &vb->vb2_buf;
 233 }
 234
 235 static void clean_display_buffer(struct mtk_vcodec_ctx *ctx)
 236 {
 237         struct vb2_buffer *framptr;
 238
 239         do {
 240                 framptr = get_display_buffer(ctx);
 241         } while (framptr);
 242 }
 243
 244 static void clean_free_buffer(struct mtk_vcodec_ctx *ctx)
 245 {
 246         struct vb2_buffer *framptr;
 247
 248         do {
 249                 framptr = get_free_buffer(ctx);
 250         } while (framptr);
 251 }
 252
 253 static void mtk_vdec_queue_res_chg_event(struct mtk_vcodec_ctx *ctx)
 254 {
 255         static const struct v4l2_event ev_src_ch = {
 256                 .type = V4L2_EVENT_SOURCE_CHANGE,
 257                 .u.src_change.changes =
 258                 V4L2_EVENT_SRC_CH_RESOLUTION,
 259         };
 260
 261         mtk_v4l2_debug(1, "[%d]", ctx->id);
 262         v4l2_event_queue_fh(&ctx->fh, &ev_src_ch);
 263 }
 264
 265 static void mtk_vdec_flush_decoder(struct mtk_vcodec_ctx *ctx)
 266 {
 267         bool res_chg;
 268         int ret = 0;
 269
 270         ret = vdec_if_decode(ctx, NULL, NULL, &res_chg);
 271         if (ret)
 272                 mtk_v4l2_err("DecodeFinal failed, ret=%d", ret);
 273
 274         clean_display_buffer(ctx);
 275         clean_free_buffer(ctx);
 276 }
 277
 278 static void mtk_vdec_update_fmt(struct mtk_vcodec_ctx *ctx,
 279                                 unsigned int pixelformat)
 280 {
 281         const struct mtk_video_fmt *fmt;
 282         struct mtk_q_data *dst_q_data;
 283         unsigned int k;
 284
 285         dst_q_data = &ctx->q_data[MTK_Q_DATA_DST];
 286         for (k = 0; k < NUM_FORMATS; k++) {
 287                 fmt = &mtk_video_formats[k];
 288                 if (fmt->fourcc == pixelformat) {
 289                         mtk_v4l2_debug(1, "Update cap fourcc(%d -> %d)",
 290                                 dst_q_data->fmt->fourcc, pixelformat);
 291                         dst_q_data->fmt = fmt;
 292                         return;
 293                 }
 294         }
 295
 296         mtk_v4l2_err("Cannot get fourcc(%d), using init value", pixelformat);
 297 }
 298
 299 static int mtk_vdec_pic_info_update(struct mtk_vcodec_ctx *ctx)
 300 {
 301         unsigned int dpbsize = 0;
 302         int ret;
 303
 304         if (vdec_if_get_param(ctx,
 305                                 GET_PARAM_PIC_INFO,
 306                                 &ctx->last_decoded_picinfo)) {
 307                 mtk_v4l2_err("[%d]Error!! Cannot get param : GET_PARAM_PICTURE_INFO ERR",
 308                                 ctx->id);
 309                 return -EINVAL;
 310         }
 311
 312         if (ctx->last_decoded_picinfo.pic_w == 0 ||
 313                 ctx->last_decoded_picinfo.pic_h == 0 ||
 314                 ctx->last_decoded_picinfo.buf_w == 0 ||
 315                 ctx->last_decoded_picinfo.buf_h == 0) {
 316                 mtk_v4l2_err("Cannot get correct pic info");
 317                 return -EINVAL;
 318         }
 319
 320         if (ctx->last_decoded_picinfo.cap_fourcc != ctx->picinfo.cap_fourcc &&
 321                 ctx->picinfo.cap_fourcc != 0)
 322                 mtk_vdec_update_fmt(ctx, ctx->picinfo.cap_fourcc);
 323
 324         if ((ctx->last_decoded_picinfo.pic_w == ctx->picinfo.pic_w) ||
 325             (ctx->last_decoded_picinfo.pic_h == ctx->picinfo.pic_h))
 326                 return 0;
 327
 328         mtk_v4l2_debug(1,
 329                         "[%d]-> new(%d,%d), old(%d,%d), real(%d,%d)",
 330                         ctx->id, ctx->last_decoded_picinfo.pic_w,
 331                         ctx->last_decoded_picinfo.pic_h,
 332                         ctx->picinfo.pic_w, ctx->picinfo.pic_h,
 333                         ctx->last_decoded_picinfo.buf_w,
 334                         ctx->last_decoded_picinfo.buf_h);
 335
 336         ret = vdec_if_get_param(ctx, GET_PARAM_DPB_SIZE, &dpbsize);
 337         if (dpbsize == 0)
 338                 mtk_v4l2_err("Incorrect dpb size, ret=%d", ret);
 339
 340         ctx->dpb_size = dpbsize;
 341
 342         return ret;
 343 }
 344
 345 static void mtk_vdec_worker(struct work_struct *work)
 346 {
 347         struct mtk_vcodec_ctx *ctx = container_of(work, struct mtk_vcodec_ctx,
 348                                 decode_work);
 349         struct mtk_vcodec_dev *dev = ctx->dev;
 350         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 351         struct mtk_vcodec_mem buf;
 352         struct vdec_fb *pfb;
 353         bool res_chg = false;
 354         int ret;
 355         struct mtk_video_dec_buf *dst_buf_info, *src_buf_info;
 356
 357         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 358         if (src_buf == NULL) {
 359                 v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
 360                 mtk_v4l2_debug(1, "[%d] src_buf empty!!", ctx->id);
 361                 return;
 362         }
 363
 364         dst_buf = v4l2_m2m_next_dst_buf(ctx->m2m_ctx);
 365         if (dst_buf == NULL) {
 366                 v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
 367                 mtk_v4l2_debug(1, "[%d] dst_buf empty!!", ctx->id);
 368                 return;
 369         }
 370
 371         src_buf_info = container_of(src_buf, struct mtk_video_dec_buf,
 372                                     m2m_buf.vb);
 373         dst_buf_info = container_of(dst_buf, struct mtk_video_dec_buf,
 374                                     m2m_buf.vb);
 375
 376         pfb = &dst_buf_info->frame_buffer;
 377         pfb->base_y.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
 378         pfb->base_y.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
 379         pfb->base_y.size = ctx->picinfo.fb_sz[0];
 380
 381         pfb->base_c.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 1);
 382         pfb->base_c.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 1);
 383         pfb->base_c.size = ctx->picinfo.fb_sz[1];
 384         pfb->status = 0;
 385         mtk_v4l2_debug(3, "===>[%d] vdec_if_decode() ===>", ctx->id);
 386
 387         mtk_v4l2_debug(3,
 388                         "id=%d Framebuf  pfb=%p VA=%p Y_DMA=%pad C_DMA=%pad Size=%zx",
 389                         dst_buf->vb2_buf.index, pfb,
 390                         pfb->base_y.va, &pfb->base_y.dma_addr,
 391                         &pfb->base_c.dma_addr, pfb->base_y.size);
 392
 393         if (src_buf_info->lastframe) {
 394                 mtk_v4l2_debug(1, "Got empty flush input buffer.");
 395                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
 396
 397                 /* update dst buf status */
 398                 dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 399                 mutex_lock(&ctx->lock);
 400                 dst_buf_info->used = false;
 401                 mutex_unlock(&ctx->lock);
 402
 403                 vdec_if_decode(ctx, NULL, NULL, &res_chg);
 404                 clean_display_buffer(ctx);
 405                 vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
 406                 if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
 407                         vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
 408                 dst_buf->flags |= V4L2_BUF_FLAG_LAST;
 409                 v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
 410                 clean_free_buffer(ctx);
 411                 v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
 412                 return;
 413         }
 414         buf.va = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
 415         buf.dma_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
 416         buf.size = (size_t)src_buf->vb2_buf.planes[0].bytesused;
 417         if (!buf.va) {
 418                 v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
 419                 mtk_v4l2_err("[%d] id=%d src_addr is NULL!!",
 420                                 ctx->id, src_buf->vb2_buf.index);
 421                 return;
 422         }
 423         mtk_v4l2_debug(3, "[%d] Bitstream VA=%p DMA=%pad Size=%zx vb=%p",
 424                         ctx->id, buf.va, &buf.dma_addr, buf.size, src_buf);
 425         dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 426         dst_buf->timecode = src_buf->timecode;
 427         mutex_lock(&ctx->lock);
 428         dst_buf_info->used = true;
 429         mutex_unlock(&ctx->lock);
 430         src_buf_info->used = true;
 431
 432         ret = vdec_if_decode(ctx, &buf, pfb, &res_chg);
 433
 434         if (ret) {
 435                 mtk_v4l2_err(
 436                         " <===[%d], src_buf[%d] sz=0x%zx pts=%llu dst_buf[%d] vdec_if_decode() ret=%d res_chg=%d===>",
 437                         ctx->id,
 438                         src_buf->vb2_buf.index,
 439                         buf.size,
 440                         src_buf->vb2_buf.timestamp,
 441                         dst_buf->vb2_buf.index,
 442                         ret, res_chg);
 443                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
 444                 if (ret == -EIO) {
 445                         mutex_lock(&ctx->lock);
 446                         src_buf_info->error = true;
 447                         mutex_unlock(&ctx->lock);
 448                 }
 449                 v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
 450         } else if (!res_chg) {
 451                 /*
 452                  * we only return src buffer with VB2_BUF_STATE_DONE
 453                  * when decode success without resolution change
 454                  */
 455                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
 456                 v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
 457         }
 458
 459         dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 460         clean_display_buffer(ctx);
 461         clean_free_buffer(ctx);
 462
 463         if (!ret && res_chg) {
 464                 mtk_vdec_pic_info_update(ctx);
 465                 /*
 466                  * On encountering a resolution change in the stream.
 467                  * The driver must first process and decode all
 468                  * remaining buffers from before the resolution change
 469                  * point, so call flush decode here
 470                  */
 471                 mtk_vdec_flush_decoder(ctx);
 472                 /*
 473                  * After all buffers containing decoded frames from
 474                  * before the resolution change point ready to be
 475                  * dequeued on the CAPTURE queue, the driver sends a
 476                  * V4L2_EVENT_SOURCE_CHANGE event for source change
 477                  * type V4L2_EVENT_SRC_CH_RESOLUTION
 478                  */
 479                 mtk_vdec_queue_res_chg_event(ctx);
 480         }
 481         v4l2_m2m_job_finish(dev->m2m_dev_dec, ctx->m2m_ctx);
 482 }
 483
 484 static int vidioc_try_decoder_cmd(struct file *file, void *priv,
 485                                 struct v4l2_decoder_cmd *cmd)
 486 {
 487         switch (cmd->cmd) {
 488         case V4L2_DEC_CMD_STOP:
 489         case V4L2_DEC_CMD_START:
 490                 if (cmd->flags != 0) {
 491                         mtk_v4l2_err("cmd->flags=%u", cmd->flags);
 492                         return -EINVAL;
 493                 }
 494                 break;
 495         default:
 496                 return -EINVAL;
 497         }
 498         return 0;
 499 }
 500
 501
 502 static int vidioc_decoder_cmd(struct file *file, void *priv,
 503                                 struct v4l2_decoder_cmd *cmd)
 504 {
 505         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 506         struct vb2_queue *src_vq, *dst_vq;
 507         int ret;
 508
 509         ret = vidioc_try_decoder_cmd(file, priv, cmd);
 510         if (ret)
 511                 return ret;
 512
 513         mtk_v4l2_debug(1, "decoder cmd=%u", cmd->cmd);
 514         dst_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
 515                                 V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
 516         switch (cmd->cmd) {
 517         case V4L2_DEC_CMD_STOP:
 518                 src_vq = v4l2_m2m_get_vq(ctx->m2m_ctx,
 519                                 V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);
 520                 if (!vb2_is_streaming(src_vq)) {
 521                         mtk_v4l2_debug(1, "Output stream is off. No need to flush.");
 522                         return 0;
 523                 }
 524                 if (!vb2_is_streaming(dst_vq)) {
 525                         mtk_v4l2_debug(1, "Capture stream is off. No need to flush.");
 526                         return 0;
 527                 }
 528                 v4l2_m2m_buf_queue(ctx->m2m_ctx,
 529                                    &ctx->empty_flush_buf->m2m_buf.vb);
 530                 v4l2_m2m_try_schedule(ctx->m2m_ctx);
 531                 break;
 532
 533         case V4L2_DEC_CMD_START:
 534                 vb2_clear_last_buffer_dequeued(dst_vq);
 535                 break;
 536
 537         default:
 538                 return -EINVAL;
 539         }
 540
 541         return 0;
 542 }
 543
 544 void mtk_vdec_unlock(struct mtk_vcodec_ctx *ctx)
 545 {
 546         mutex_unlock(&ctx->dev->dec_mutex);
 547 }
 548
 549 void mtk_vdec_lock(struct mtk_vcodec_ctx *ctx)
 550 {
 551         mutex_lock(&ctx->dev->dec_mutex);
 552 }
 553
 554 void mtk_vcodec_dec_release(struct mtk_vcodec_ctx *ctx)
 555 {
 556         vdec_if_deinit(ctx);
 557         ctx->state = MTK_STATE_FREE;
 558 }
 559
 560 void mtk_vcodec_dec_set_default_params(struct mtk_vcodec_ctx *ctx)
 561 {
 562         struct mtk_q_data *q_data;
 563
 564         ctx->m2m_ctx->q_lock = &ctx->dev->dev_mutex;
 565         ctx->fh.m2m_ctx = ctx->m2m_ctx;
 566         ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
 567         INIT_WORK(&ctx->decode_work, mtk_vdec_worker);
 568         ctx->colorspace = V4L2_COLORSPACE_REC709;
 569         ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 570         ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
 571         ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
 572
 573         q_data = &ctx->q_data[MTK_Q_DATA_SRC];
 574         memset(q_data, 0, sizeof(struct mtk_q_data));
 575         q_data->visible_width = DFT_CFG_WIDTH;
 576         q_data->visible_height = DFT_CFG_HEIGHT;
 577         q_data->fmt = &mtk_video_formats[OUT_FMT_IDX];
 578         q_data->field = V4L2_FIELD_NONE;
 579
 580         q_data->sizeimage[0] = DFT_CFG_WIDTH * DFT_CFG_HEIGHT;
 581         q_data->bytesperline[0] = 0;
 582
 583         q_data = &ctx->q_data[MTK_Q_DATA_DST];
 584         memset(q_data, 0, sizeof(struct mtk_q_data));
 585         q_data->visible_width = DFT_CFG_WIDTH;
 586         q_data->visible_height = DFT_CFG_HEIGHT;
 587         q_data->coded_width = DFT_CFG_WIDTH;
 588         q_data->coded_height = DFT_CFG_HEIGHT;
 589         q_data->fmt = &mtk_video_formats[CAP_FMT_IDX];
 590         q_data->field = V4L2_FIELD_NONE;
 591
 592         v4l_bound_align_image(&q_data->coded_width,
 593                                 MTK_VDEC_MIN_W,
 594                                 MTK_VDEC_MAX_W, 4,
 595                                 &q_data->coded_height,
 596                                 MTK_VDEC_MIN_H,
 597                                 MTK_VDEC_MAX_H, 5, 6);
 598
 599         q_data->sizeimage[0] = q_data->coded_width * q_data->coded_height;
 600         q_data->bytesperline[0] = q_data->coded_width;
 601         q_data->sizeimage[1] = q_data->sizeimage[0] / 2;
 602         q_data->bytesperline[1] = q_data->coded_width;
 603 }
 604
 605 static int vidioc_vdec_qbuf(struct file *file, void *priv,
 606                             struct v4l2_buffer *buf)
 607 {
 608         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 609
 610         if (ctx->state == MTK_STATE_ABORT) {
 611                 mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
 612                                 ctx->id);
 613                 return -EIO;
 614         }
 615
 616         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 617 }
 618
 619 static int vidioc_vdec_dqbuf(struct file *file, void *priv,
 620                              struct v4l2_buffer *buf)
 621 {
 622         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 623
 624         if (ctx->state == MTK_STATE_ABORT) {
 625                 mtk_v4l2_err("[%d] Call on DQBUF after unrecoverable error",
 626                                 ctx->id);
 627                 return -EIO;
 628         }
 629
 630         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 631 }
 632
 633 static int vidioc_vdec_querycap(struct file *file, void *priv,
 634                                 struct v4l2_capability *cap)
 635 {
 636         strscpy(cap->driver, MTK_VCODEC_DEC_NAME, sizeof(cap->driver));
 637         strscpy(cap->bus_info, MTK_PLATFORM_STR, sizeof(cap->bus_info));
 638         strscpy(cap->card, MTK_PLATFORM_STR, sizeof(cap->card));
 639
 640         return 0;
 641 }
 642
 643 static int vidioc_vdec_subscribe_evt(struct v4l2_fh *fh,
 644                                      const struct v4l2_event_subscription *sub)
 645 {
 646         switch (sub->type) {
 647         case V4L2_EVENT_EOS:
 648                 return v4l2_event_subscribe(fh, sub, 2, NULL);
 649         case V4L2_EVENT_SOURCE_CHANGE:
 650                 return v4l2_src_change_event_subscribe(fh, sub);
 651         default:
 652                 return v4l2_ctrl_subscribe_event(fh, sub);
 653         }
 654 }
 655
 656 static int vidioc_try_fmt(struct v4l2_format *f,
 657                           const struct mtk_video_fmt *fmt)
 658 {
 659         struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
 660
 661         pix_fmt_mp->field = V4L2_FIELD_NONE;
 662
 663         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 664                 pix_fmt_mp->num_planes = 1;
 665                 pix_fmt_mp->plane_fmt[0].bytesperline = 0;
 666         } else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 667                 int tmp_w, tmp_h;
 668
 669                 pix_fmt_mp->height = clamp(pix_fmt_mp->height,
 670                                         MTK_VDEC_MIN_H,
 671                                         MTK_VDEC_MAX_H);
 672                 pix_fmt_mp->width = clamp(pix_fmt_mp->width,
 673                                         MTK_VDEC_MIN_W,
 674                                         MTK_VDEC_MAX_W);
 675
 676                 /*
 677                  * Find next closer width align 64, heign align 64, size align
 678                  * 64 rectangle
 679                  * Note: This only get default value, the real HW needed value
 680                  *       only available when ctx in MTK_STATE_HEADER state
 681                  */
 682                 tmp_w = pix_fmt_mp->width;
 683                 tmp_h = pix_fmt_mp->height;
 684                 v4l_bound_align_image(&pix_fmt_mp->width,
 685                                         MTK_VDEC_MIN_W,
 686                                         MTK_VDEC_MAX_W, 6,
 687                                         &pix_fmt_mp->height,
 688                                         MTK_VDEC_MIN_H,
 689                                         MTK_VDEC_MAX_H, 6, 9);
 690
 691                 if (pix_fmt_mp->width < tmp_w &&
 692                         (pix_fmt_mp->width + 64) <= MTK_VDEC_MAX_W)
 693                         pix_fmt_mp->width += 64;
 694                 if (pix_fmt_mp->height < tmp_h &&
 695                         (pix_fmt_mp->height + 64) <= MTK_VDEC_MAX_H)
 696                         pix_fmt_mp->height += 64;
 697
 698                 mtk_v4l2_debug(0,
 699                         "before resize width=%d, height=%d, after resize width=%d, height=%d, sizeimage=%d",
 700                         tmp_w, tmp_h, pix_fmt_mp->width,
 701                         pix_fmt_mp->height,
 702                         pix_fmt_mp->width * pix_fmt_mp->height);
 703
 704                 pix_fmt_mp->num_planes = fmt->num_planes;
 705                 pix_fmt_mp->plane_fmt[0].sizeimage =
 706                                 pix_fmt_mp->width * pix_fmt_mp->height;
 707                 pix_fmt_mp->plane_fmt[0].bytesperline = pix_fmt_mp->width;
 708
 709                 if (pix_fmt_mp->num_planes == 2) {
 710                         pix_fmt_mp->plane_fmt[1].sizeimage =
 711                                 (pix_fmt_mp->width * pix_fmt_mp->height) / 2;
 712                         pix_fmt_mp->plane_fmt[1].bytesperline =
 713                                 pix_fmt_mp->width;
 714                 }
 715         }
 716
 717         pix_fmt_mp->flags = 0;
 718         return 0;
 719 }
 720
 721 static int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
 722                                 struct v4l2_format *f)
 723 {
 724         const struct mtk_video_fmt *fmt;
 725
 726         fmt = mtk_vdec_find_format(f);
 727         if (!fmt) {
 728                 f->fmt.pix.pixelformat = mtk_video_formats[CAP_FMT_IDX].fourcc;
 729                 fmt = mtk_vdec_find_format(f);
 730         }
 731
 732         return vidioc_try_fmt(f, fmt);
 733 }
 734
 735 static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
 736                                 struct v4l2_format *f)
 737 {
 738         struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
 739         const struct mtk_video_fmt *fmt;
 740
 741         fmt = mtk_vdec_find_format(f);
 742         if (!fmt) {
 743                 f->fmt.pix.pixelformat = mtk_video_formats[OUT_FMT_IDX].fourcc;
 744                 fmt = mtk_vdec_find_format(f);
 745         }
 746
 747         if (pix_fmt_mp->plane_fmt[0].sizeimage == 0) {
 748                 mtk_v4l2_err("sizeimage of output format must be given");
 749                 return -EINVAL;
 750         }
 751
 752         return vidioc_try_fmt(f, fmt);
 753 }
 754
 755 static int vidioc_vdec_g_selection(struct file *file, void *priv,
 756                         struct v4l2_selection *s)
 757 {
 758         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 759         struct mtk_q_data *q_data;
 760
 761         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 762                 return -EINVAL;
 763
 764         q_data = &ctx->q_data[MTK_Q_DATA_DST];
 765
 766         switch (s->target) {
 767         case V4L2_SEL_TGT_COMPOSE_DEFAULT:
 768                 s->r.left = 0;
 769                 s->r.top = 0;
 770                 s->r.width = ctx->picinfo.pic_w;
 771                 s->r.height = ctx->picinfo.pic_h;
 772                 break;
 773         case V4L2_SEL_TGT_COMPOSE_BOUNDS:
 774                 s->r.left = 0;
 775                 s->r.top = 0;
 776                 s->r.width = ctx->picinfo.buf_w;
 777                 s->r.height = ctx->picinfo.buf_h;
 778                 break;
 779         case V4L2_SEL_TGT_COMPOSE:
 780                 if (vdec_if_get_param(ctx, GET_PARAM_CROP_INFO, &(s->r))) {
 781                         /* set to default value if header info not ready yet*/
 782                         s->r.left = 0;
 783                         s->r.top = 0;
 784                         s->r.width = q_data->visible_width;
 785                         s->r.height = q_data->visible_height;
 786                 }
 787                 break;
 788         default:
 789                 return -EINVAL;
 790         }
 791
 792         if (ctx->state < MTK_STATE_HEADER) {
 793                 /* set to default value if header info not ready yet*/
 794                 s->r.left = 0;
 795                 s->r.top = 0;
 796                 s->r.width = q_data->visible_width;
 797                 s->r.height = q_data->visible_height;
 798                 return 0;
 799         }
 800
 801         return 0;
 802 }
 803
 804 static int vidioc_vdec_s_selection(struct file *file, void *priv,
 805                                 struct v4l2_selection *s)
 806 {
 807         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 808
 809         if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
 810                 return -EINVAL;
 811
 812         switch (s->target) {
 813         case V4L2_SEL_TGT_COMPOSE:
 814                 s->r.left = 0;
 815                 s->r.top = 0;
 816                 s->r.width = ctx->picinfo.pic_w;
 817                 s->r.height = ctx->picinfo.pic_h;
 818                 break;
 819         default:
 820                 return -EINVAL;
 821         }
 822
 823         return 0;
 824 }
 825
 826 static int vidioc_vdec_s_fmt(struct file *file, void *priv,
 827                              struct v4l2_format *f)
 828 {
 829         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 830         struct v4l2_pix_format_mplane *pix_mp;
 831         struct mtk_q_data *q_data;
 832         int ret = 0;
 833         const struct mtk_video_fmt *fmt;
 834
 835         mtk_v4l2_debug(3, "[%d]", ctx->id);
 836
 837         q_data = mtk_vdec_get_q_data(ctx, f->type);
 838         if (!q_data)
 839                 return -EINVAL;
 840
 841         pix_mp = &f->fmt.pix_mp;
 842         /*
 843          * Setting OUTPUT format after OUTPUT buffers are allocated is invalid
 844          * if using the stateful API.
 845          */
 846         if ((f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) &&
 847             vb2_is_busy(&ctx->m2m_ctx->out_q_ctx.q)) {
 848                 mtk_v4l2_err("out_q_ctx buffers already requested");
 849                 ret = -EBUSY;
 850         }
 851
 852         /*
 853          * Setting CAPTURE format after CAPTURE buffers are allocated is
 854          * invalid.
 855          */
 856         if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
 857             vb2_is_busy(&ctx->m2m_ctx->cap_q_ctx.q)) {
 858                 mtk_v4l2_err("cap_q_ctx buffers already requested");
 859                 ret = -EBUSY;
 860         }
 861
 862         fmt = mtk_vdec_find_format(f);
 863         if (fmt == NULL) {
 864                 if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 865                         f->fmt.pix.pixelformat =
 866                                 mtk_video_formats[OUT_FMT_IDX].fourcc;
 867                         fmt = mtk_vdec_find_format(f);
 868                 } else if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 869                         f->fmt.pix.pixelformat =
 870                                 mtk_video_formats[CAP_FMT_IDX].fourcc;
 871                         fmt = mtk_vdec_find_format(f);
 872                 }
 873         }
 874         if (fmt == NULL)
 875                 return -EINVAL;
 876
 877         q_data->fmt = fmt;
 878         vidioc_try_fmt(f, q_data->fmt);
 879         if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 880                 q_data->sizeimage[0] = pix_mp->plane_fmt[0].sizeimage;
 881                 q_data->coded_width = pix_mp->width;
 882                 q_data->coded_height = pix_mp->height;
 883
 884                 ctx->colorspace = pix_mp->colorspace;
 885                 ctx->ycbcr_enc = pix_mp->ycbcr_enc;
 886                 ctx->quantization = pix_mp->quantization;
 887                 ctx->xfer_func = pix_mp->xfer_func;
 888
 889                 if (ctx->state == MTK_STATE_FREE) {
 890                         ret = vdec_if_init(ctx, q_data->fmt->fourcc);
 891                         if (ret) {
 892                                 mtk_v4l2_err("[%d]: vdec_if_init() fail ret=%d",
 893                                         ctx->id, ret);
 894                                 return -EINVAL;
 895                         }
 896                         ctx->state = MTK_STATE_INIT;
 897                 }
 898         }
 899
 900         return 0;
 901 }
 902
 903 static int vidioc_enum_framesizes(struct file *file, void *priv,
 904                                 struct v4l2_frmsizeenum *fsize)
 905 {
 906         int i = 0;
 907         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 908
 909         if (fsize->index != 0)
 910                 return -EINVAL;
 911
 912         for (i = 0; i < NUM_SUPPORTED_FRAMESIZE; ++i) {
 913                 if (fsize->pixel_format != mtk_vdec_framesizes[i].fourcc)
 914                         continue;
 915
 916                 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 917                 fsize->stepwise = mtk_vdec_framesizes[i].stepwise;
 918                 if (!(ctx->dev->dec_capability &
 919                                 VCODEC_CAPABILITY_4K_DISABLED)) {
 920                         mtk_v4l2_debug(3, "4K is enabled");
 921                         fsize->stepwise.max_width =
 922                                         VCODEC_DEC_4K_CODED_WIDTH;
 923                         fsize->stepwise.max_height =
 924                                         VCODEC_DEC_4K_CODED_HEIGHT;
 925                 }
 926                 mtk_v4l2_debug(1, "%x, %d %d %d %d %d %d",
 927                                 ctx->dev->dec_capability,
 928                                 fsize->stepwise.min_width,
 929                                 fsize->stepwise.max_width,
 930                                 fsize->stepwise.step_width,
 931                                 fsize->stepwise.min_height,
 932                                 fsize->stepwise.max_height,
 933                                 fsize->stepwise.step_height);
 934                 return 0;
 935         }
 936
 937         return -EINVAL;
 938 }
 939
 940 static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool output_queue)
 941 {
 942         const struct mtk_video_fmt *fmt;
 943         int i, j = 0;
 944
 945         for (i = 0; i < NUM_FORMATS; i++) {
 946                 if (output_queue && (mtk_video_formats[i].type != MTK_FMT_DEC))
 947                         continue;
 948                 if (!output_queue &&
 949                         (mtk_video_formats[i].type != MTK_FMT_FRAME))
 950                         continue;
 951
 952                 if (j == f->index)
 953                         break;
 954                 ++j;
 955         }
 956
 957         if (i == NUM_FORMATS)
 958                 return -EINVAL;
 959
 960         fmt = &mtk_video_formats[i];
 961         f->pixelformat = fmt->fourcc;
 962         f->flags = fmt->flags;
 963
 964         return 0;
 965 }
 966
 967 static int vidioc_vdec_enum_fmt_vid_cap(struct file *file, void *priv,
 968                                         struct v4l2_fmtdesc *f)
 969 {
 970         return vidioc_enum_fmt(f, false);
 971 }
 972
 973 static int vidioc_vdec_enum_fmt_vid_out(struct file *file, void *priv,
 974                                         struct v4l2_fmtdesc *f)
 975 {
 976         return vidioc_enum_fmt(f, true);
 977 }
 978
 979 static int vidioc_vdec_g_fmt(struct file *file, void *priv,
 980                              struct v4l2_format *f)
 981 {
 982         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 983         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 984         struct vb2_queue *vq;
 985         struct mtk_q_data *q_data;
 986
 987         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 988         if (!vq) {
 989                 mtk_v4l2_err("no vb2 queue for type=%d", f->type);
 990                 return -EINVAL;
 991         }
 992
 993         q_data = mtk_vdec_get_q_data(ctx, f->type);
 994
 995         pix_mp->field = V4L2_FIELD_NONE;
 996         pix_mp->colorspace = ctx->colorspace;
 997         pix_mp->ycbcr_enc = ctx->ycbcr_enc;
 998         pix_mp->quantization = ctx->quantization;
 999         pix_mp->xfer_func = ctx->xfer_func;
1000
1001         if ((f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) &&
1002             (ctx->state >= MTK_STATE_HEADER)) {
1003                 /* Until STREAMOFF is called on the CAPTURE queue
1004                  * (acknowledging the event), the driver operates as if
1005                  * the resolution hasn't changed yet.
1006                  * So we just return picinfo yet, and update picinfo in
1007                  * stop_streaming hook function
1008                  */
1009                 q_data->sizeimage[0] = ctx->picinfo.fb_sz[0];
1010                 q_data->sizeimage[1] = ctx->picinfo.fb_sz[1];
1011                 q_data->bytesperline[0] = ctx->last_decoded_picinfo.buf_w;
1012                 q_data->bytesperline[1] = ctx->last_decoded_picinfo.buf_w;
1013                 q_data->coded_width = ctx->picinfo.buf_w;
1014                 q_data->coded_height = ctx->picinfo.buf_h;
1015                 ctx->last_decoded_picinfo.cap_fourcc = q_data->fmt->fourcc;
1016
1017                 /*
1018                  * Width and height are set to the dimensions
1019                  * of the movie, the buffer is bigger and
1020                  * further processing stages should crop to this
1021                  * rectangle.
1022                  */
1023                 pix_mp->width = q_data->coded_width;
1024                 pix_mp->height = q_data->coded_height;
1025
1026                 /*
1027                  * Set pixelformat to the format in which mt vcodec
1028                  * outputs the decoded frame
1029                  */
1030                 pix_mp->num_planes = q_data->fmt->num_planes;
1031                 pix_mp->pixelformat = q_data->fmt->fourcc;
1032                 pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
1033                 pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
1034                 pix_mp->plane_fmt[1].bytesperline = q_data->bytesperline[1];
1035                 pix_mp->plane_fmt[1].sizeimage = q_data->sizeimage[1];
1036
1037         } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
1038                 /*
1039                  * This is run on OUTPUT
1040                  * The buffer contains compressed image
1041                  * so width and height have no meaning.
1042                  * Assign value here to pass v4l2-compliance test
1043                  */
1044                 pix_mp->width = q_data->visible_width;
1045                 pix_mp->height = q_data->visible_height;
1046                 pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
1047                 pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
1048                 pix_mp->pixelformat = q_data->fmt->fourcc;
1049                 pix_mp->num_planes = q_data->fmt->num_planes;
1050         } else {
1051                 pix_mp->width = q_data->coded_width;
1052                 pix_mp->height = q_data->coded_height;
1053                 pix_mp->num_planes = q_data->fmt->num_planes;
1054                 pix_mp->pixelformat = q_data->fmt->fourcc;
1055                 pix_mp->plane_fmt[0].bytesperline = q_data->bytesperline[0];
1056                 pix_mp->plane_fmt[0].sizeimage = q_data->sizeimage[0];
1057                 pix_mp->plane_fmt[1].bytesperline = q_data->bytesperline[1];
1058                 pix_mp->plane_fmt[1].sizeimage = q_data->sizeimage[1];
1059
1060                 mtk_v4l2_debug(1, "[%d] type=%d state=%d Format information could not be read, not ready yet!",
1061                                 ctx->id, f->type, ctx->state);
1062         }
1063
1064         return 0;
1065 }
1066
1067 static int vb2ops_vdec_queue_setup(struct vb2_queue *vq,
1068                                 unsigned int *nbuffers,
1069                                 unsigned int *nplanes,
1070                                 unsigned int sizes[],
1071                                 struct device *alloc_devs[])
1072 {
1073         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vq);
1074         struct mtk_q_data *q_data;
1075         unsigned int i;
1076
1077         q_data = mtk_vdec_get_q_data(ctx, vq->type);
1078
1079         if (q_data == NULL) {
1080                 mtk_v4l2_err("vq->type=%d err\n", vq->type);
1081                 return -EINVAL;
1082         }
1083
1084         if (*nplanes) {
1085                 for (i = 0; i < *nplanes; i++) {
1086                         if (sizes[i] < q_data->sizeimage[i])
1087                                 return -EINVAL;
1088                 }
1089         } else {
1090                 if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE)
1091                         *nplanes = 2;
1092                 else
1093                         *nplanes = 1;
1094
1095                 for (i = 0; i < *nplanes; i++)
1096                         sizes[i] = q_data->sizeimage[i];
1097         }
1098
1099         mtk_v4l2_debug(1,
1100                         "[%d]\t type = %d, get %d plane(s), %d buffer(s) of size 0x%x 0x%x ",
1101                         ctx->id, vq->type, *nplanes, *nbuffers,
1102                         sizes[0], sizes[1]);
1103
1104         return 0;
1105 }
1106
1107 static int vb2ops_vdec_buf_prepare(struct vb2_buffer *vb)
1108 {
1109         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1110         struct mtk_q_data *q_data;
1111         int i;
1112
1113         mtk_v4l2_debug(3, "[%d] (%d) id=%d",
1114                         ctx->id, vb->vb2_queue->type, vb->index);
1115
1116         q_data = mtk_vdec_get_q_data(ctx, vb->vb2_queue->type);
1117
1118         for (i = 0; i < q_data->fmt->num_planes; i++) {
1119                 if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
1120                         mtk_v4l2_err("data will not fit into plane %d (%lu < %d)",
1121                                 i, vb2_plane_size(vb, i),
1122                                 q_data->sizeimage[i]);
1123                 }
1124         }
1125
1126         return 0;
1127 }
1128
1129 static void vb2ops_vdec_buf_queue(struct vb2_buffer *vb)
1130 {
1131         struct vb2_v4l2_buffer *src_buf;
1132         struct mtk_vcodec_mem src_mem;
1133         bool res_chg = false;
1134         int ret = 0;
1135         unsigned int dpbsize = 1, i = 0;
1136         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1137         struct vb2_v4l2_buffer *vb2_v4l2 = NULL;
1138         struct mtk_video_dec_buf *buf = NULL;
1139         struct mtk_q_data *dst_q_data;
1140
1141         mtk_v4l2_debug(3, "[%d] (%d) id=%d, vb=%p",
1142                         ctx->id, vb->vb2_queue->type,
1143                         vb->index, vb);
1144         /*
1145          * check if this buffer is ready to be used after decode
1146          */
1147         if (vb->vb2_queue->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
1148                 vb2_v4l2 = to_vb2_v4l2_buffer(vb);
1149                 buf = container_of(vb2_v4l2, struct mtk_video_dec_buf,
1150                                    m2m_buf.vb);
1151                 mutex_lock(&ctx->lock);
1152                 if (!buf->used) {
1153                         v4l2_m2m_buf_queue(ctx->m2m_ctx, vb2_v4l2);
1154                         buf->queued_in_vb2 = true;
1155                         buf->queued_in_v4l2 = true;
1156                 } else {
1157                         buf->queued_in_vb2 = false;
1158                         buf->queued_in_v4l2 = true;
1159                 }
1160                 mutex_unlock(&ctx->lock);
1161                 return;
1162         }
1163
1164         v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
1165
1166         if (ctx->state != MTK_STATE_INIT) {
1167                 mtk_v4l2_debug(3, "[%d] already init driver %d",
1168                                 ctx->id, ctx->state);
1169                 return;
1170         }
1171
1172         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
1173         if (!src_buf) {
1174                 mtk_v4l2_err("No src buffer");
1175                 return;
1176         }
1177         buf = container_of(src_buf, struct mtk_video_dec_buf, m2m_buf.vb);
1178         if (buf->lastframe) {
1179                 /* This shouldn't happen. Just in case. */
1180                 mtk_v4l2_err("Invalid flush buffer.");
1181                 v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
1182                 return;
1183         }
1184
1185         src_mem.va = vb2_plane_vaddr(&src_buf->vb2_buf, 0);
1186         src_mem.dma_addr = vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, 0);
1187         src_mem.size = (size_t)src_buf->vb2_buf.planes[0].bytesused;
1188         mtk_v4l2_debug(2,
1189                         "[%d] buf id=%d va=%p dma=%pad size=%zx",
1190                         ctx->id, src_buf->vb2_buf.index,
1191                         src_mem.va, &src_mem.dma_addr,
1192                         src_mem.size);
1193
1194         ret = vdec_if_decode(ctx, &src_mem, NULL, &res_chg);
1195         if (ret || !res_chg) {
1196                 /*
1197                  * fb == NULL means to parse SPS/PPS header or
1198                  * resolution info in src_mem. Decode can fail
1199                  * if there is no SPS header or picture info
1200                  * in bs
1201                  */
1202
1203                 src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
1204                 if (ret == -EIO) {
1205                         mtk_v4l2_err("[%d] Unrecoverable error in vdec_if_decode.",
1206                                         ctx->id);
1207                         ctx->state = MTK_STATE_ABORT;
1208                         v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
1209                 } else {
1210                         v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
1211                 }
1212                 mtk_v4l2_debug(ret ? 0 : 1,
1213                                "[%d] vdec_if_decode() src_buf=%d, size=%zu, fail=%d, res_chg=%d",
1214                                ctx->id, src_buf->vb2_buf.index,
1215                                src_mem.size, ret, res_chg);
1216                 return;
1217         }
1218
1219         if (vdec_if_get_param(ctx, GET_PARAM_PIC_INFO, &ctx->picinfo)) {
1220                 mtk_v4l2_err("[%d]Error!! Cannot get param : GET_PARAM_PICTURE_INFO ERR",
1221                                 ctx->id);
1222                 return;
1223         }
1224
1225         ctx->last_decoded_picinfo = ctx->picinfo;
1226         dst_q_data = &ctx->q_data[MTK_Q_DATA_DST];
1227         for (i = 0; i < dst_q_data->fmt->num_planes; i++) {
1228                 dst_q_data->sizeimage[i] = ctx->picinfo.fb_sz[i];
1229                 dst_q_data->bytesperline[i] = ctx->picinfo.buf_w;
1230         }
1231
1232         mtk_v4l2_debug(2, "[%d] vdec_if_init() OK wxh=%dx%d pic wxh=%dx%d sz[0]=0x%x sz[1]=0x%x",
1233                         ctx->id,
1234                         ctx->picinfo.buf_w, ctx->picinfo.buf_h,
1235                         ctx->picinfo.pic_w, ctx->picinfo.pic_h,
1236                         dst_q_data->sizeimage[0],
1237                         dst_q_data->sizeimage[1]);
1238
1239         ret = vdec_if_get_param(ctx, GET_PARAM_DPB_SIZE, &dpbsize);
1240         if (dpbsize == 0)
1241                 mtk_v4l2_err("[%d] GET_PARAM_DPB_SIZE fail=%d", ctx->id, ret);
1242
1243         ctx->dpb_size = dpbsize;
1244         ctx->state = MTK_STATE_HEADER;
1245         mtk_v4l2_debug(1, "[%d] dpbsize=%d", ctx->id, ctx->dpb_size);
1246
1247         mtk_vdec_queue_res_chg_event(ctx);
1248 }
1249
1250 static void vb2ops_vdec_buf_finish(struct vb2_buffer *vb)
1251 {
1252         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
1253         struct vb2_v4l2_buffer *vb2_v4l2;
1254         struct mtk_video_dec_buf *buf;
1255         bool buf_error;
1256
1257         vb2_v4l2 = container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
1258         buf = container_of(vb2_v4l2, struct mtk_video_dec_buf, m2m_buf.vb);
1259         mutex_lock(&ctx->lock);
1260         if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1261                 buf->queued_in_v4l2 = false;
1262                 buf->queued_in_vb2 = false;
1263         }
1264         buf_error = buf->error;
1265         mutex_unlock(&ctx->lock);
1266
1267         if (buf_error) {
1268                 mtk_v4l2_err("Unrecoverable error on buffer.");
1269                 ctx->state = MTK_STATE_ABORT;
1270         }
1271 }
1272
1273 static int vb2ops_vdec_buf_init(struct vb2_buffer *vb)
1274 {
1275         struct vb2_v4l2_buffer *vb2_v4l2 = container_of(vb,
1276                                         struct vb2_v4l2_buffer, vb2_buf);
1277         struct mtk_video_dec_buf *buf = container_of(vb2_v4l2,
1278                                         struct mtk_video_dec_buf, m2m_buf.vb);
1279
1280         if (vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
1281                 buf->used = false;
1282                 buf->queued_in_v4l2 = false;
1283         } else {
1284                 buf->lastframe = false;
1285         }
1286
1287         return 0;
1288 }
1289
1290 static int vb2ops_vdec_start_streaming(struct vb2_queue *q, unsigned int count)
1291 {
1292         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
1293
1294         if (ctx->state == MTK_STATE_FLUSH)
1295                 ctx->state = MTK_STATE_HEADER;
1296
1297         return 0;
1298 }
1299
1300 static void vb2ops_vdec_stop_streaming(struct vb2_queue *q)
1301 {
1302         struct vb2_v4l2_buffer *src_buf = NULL, *dst_buf = NULL;
1303         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
1304
1305         mtk_v4l2_debug(3, "[%d] (%d) state=(%x) ctx->decoded_frame_cnt=%d",
1306                         ctx->id, q->type, ctx->state, ctx->decoded_frame_cnt);
1307
1308         if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
1309                 while ((src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx))) {
1310                         struct mtk_video_dec_buf *buf_info = container_of(
1311                                  src_buf, struct mtk_video_dec_buf, m2m_buf.vb);
1312                         if (!buf_info->lastframe)
1313                                 v4l2_m2m_buf_done(src_buf,
1314                                                 VB2_BUF_STATE_ERROR);
1315                 }
1316                 return;
1317         }
1318
1319         if (ctx->state >= MTK_STATE_HEADER) {
1320
1321                 /* Until STREAMOFF is called on the CAPTURE queue
1322                  * (acknowledging the event), the driver operates
1323                  * as if the resolution hasn't changed yet, i.e.
1324                  * VIDIOC_G_FMT< etc. return previous resolution.
1325                  * So we update picinfo here
1326                  */
1327                 ctx->picinfo = ctx->last_decoded_picinfo;
1328
1329                 mtk_v4l2_debug(2,
1330                                 "[%d]-> new(%d,%d), old(%d,%d), real(%d,%d)",
1331                                 ctx->id, ctx->last_decoded_picinfo.pic_w,
1332                                 ctx->last_decoded_picinfo.pic_h,
1333                                 ctx->picinfo.pic_w, ctx->picinfo.pic_h,
1334                                 ctx->last_decoded_picinfo.buf_w,
1335                                 ctx->last_decoded_picinfo.buf_h);
1336
1337                 mtk_vdec_flush_decoder(ctx);
1338         }
1339         ctx->state = MTK_STATE_FLUSH;
1340
1341         while ((dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx))) {
1342                 vb2_set_plane_payload(&dst_buf->vb2_buf, 0, 0);
1343                 if (ctx->q_data[MTK_Q_DATA_DST].fmt->num_planes == 2)
1344                         vb2_set_plane_payload(&dst_buf->vb2_buf, 1, 0);
1345                 v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
1346         }
1347
1348 }
1349
1350 static void m2mops_vdec_device_run(void *priv)
1351 {
1352         struct mtk_vcodec_ctx *ctx = priv;
1353         struct mtk_vcodec_dev *dev = ctx->dev;
1354
1355         queue_work(dev->decode_workqueue, &ctx->decode_work);
1356 }
1357
1358 static int m2mops_vdec_job_ready(void *m2m_priv)
1359 {
1360         struct mtk_vcodec_ctx *ctx = m2m_priv;
1361
1362         mtk_v4l2_debug(3, "[%d]", ctx->id);
1363
1364         if (ctx->state == MTK_STATE_ABORT)
1365                 return 0;
1366
1367         if ((ctx->last_decoded_picinfo.pic_w != ctx->picinfo.pic_w) ||
1368             (ctx->last_decoded_picinfo.pic_h != ctx->picinfo.pic_h))
1369                 return 0;
1370
1371         if (ctx->state != MTK_STATE_HEADER)
1372                 return 0;
1373
1374         return 1;
1375 }
1376
1377 static void m2mops_vdec_job_abort(void *priv)
1378 {
1379         struct mtk_vcodec_ctx *ctx = priv;
1380
1381         ctx->state = MTK_STATE_ABORT;
1382 }
1383
1384 static int mtk_vdec_g_v_ctrl(struct v4l2_ctrl *ctrl)
1385 {
1386         struct mtk_vcodec_ctx *ctx = ctrl_to_ctx(ctrl);
1387         int ret = 0;
1388
1389         switch (ctrl->id) {
1390         case V4L2_CID_MIN_BUFFERS_FOR_CAPTURE:
1391                 if (ctx->state >= MTK_STATE_HEADER) {
1392                         ctrl->val = ctx->dpb_size;
1393                 } else {
1394                         mtk_v4l2_debug(0, "Seqinfo not ready");
1395                         ctrl->val = 0;
1396                 }
1397                 break;
1398         default:
1399                 ret = -EINVAL;
1400         }
1401         return ret;
1402 }
1403
1404 static const struct v4l2_ctrl_ops mtk_vcodec_dec_ctrl_ops = {
1405         .g_volatile_ctrl = mtk_vdec_g_v_ctrl,
1406 };
1407
1408 int mtk_vcodec_dec_ctrls_setup(struct mtk_vcodec_ctx *ctx)
1409 {
1410         struct v4l2_ctrl *ctrl;
1411
1412         v4l2_ctrl_handler_init(&ctx->ctrl_hdl, 1);
1413
1414         ctrl = v4l2_ctrl_new_std(&ctx->ctrl_hdl,
1415                                 &mtk_vcodec_dec_ctrl_ops,
1416                                 V4L2_CID_MIN_BUFFERS_FOR_CAPTURE,
1417                                 0, 32, 1, 1);
1418         ctrl->flags |= V4L2_CTRL_FLAG_VOLATILE;
1419         v4l2_ctrl_new_std_menu(&ctx->ctrl_hdl,
1420                                 &mtk_vcodec_dec_ctrl_ops,
1421                                 V4L2_CID_MPEG_VIDEO_VP9_PROFILE,
1422                                 V4L2_MPEG_VIDEO_VP9_PROFILE_0,
1423                                 0, V4L2_MPEG_VIDEO_VP9_PROFILE_0);
1424
1425         if (ctx->ctrl_hdl.error) {
1426                 mtk_v4l2_err("Adding control failed %d",
1427                                 ctx->ctrl_hdl.error);
1428                 return ctx->ctrl_hdl.error;
1429         }
1430
1431         v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
1432         return 0;
1433 }
1434
1435 const struct v4l2_m2m_ops mtk_vdec_m2m_ops = {
1436         .device_run     = m2mops_vdec_device_run,
1437         .job_ready      = m2mops_vdec_job_ready,
1438         .job_abort      = m2mops_vdec_job_abort,
1439 };
1440
1441 static const struct vb2_ops mtk_vdec_vb2_ops = {
1442         .queue_setup    = vb2ops_vdec_queue_setup,
1443         .buf_prepare    = vb2ops_vdec_buf_prepare,
1444         .buf_queue      = vb2ops_vdec_buf_queue,
1445         .wait_prepare   = vb2_ops_wait_prepare,
1446         .wait_finish    = vb2_ops_wait_finish,
1447         .buf_init       = vb2ops_vdec_buf_init,
1448         .buf_finish     = vb2ops_vdec_buf_finish,
1449         .start_streaming        = vb2ops_vdec_start_streaming,
1450         .stop_streaming = vb2ops_vdec_stop_streaming,
1451 };
1452
1453 const struct v4l2_ioctl_ops mtk_vdec_ioctl_ops = {
1454         .vidioc_streamon        = v4l2_m2m_ioctl_streamon,
1455         .vidioc_streamoff       = v4l2_m2m_ioctl_streamoff,
1456         .vidioc_reqbufs         = v4l2_m2m_ioctl_reqbufs,
1457         .vidioc_querybuf        = v4l2_m2m_ioctl_querybuf,
1458         .vidioc_expbuf          = v4l2_m2m_ioctl_expbuf,
1459
1460         .vidioc_qbuf            = vidioc_vdec_qbuf,
1461         .vidioc_dqbuf           = vidioc_vdec_dqbuf,
1462
1463         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap_mplane,
1464         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out_mplane,
1465
1466         .vidioc_s_fmt_vid_cap_mplane    = vidioc_vdec_s_fmt,
1467         .vidioc_s_fmt_vid_out_mplane    = vidioc_vdec_s_fmt,
1468         .vidioc_g_fmt_vid_cap_mplane    = vidioc_vdec_g_fmt,
1469         .vidioc_g_fmt_vid_out_mplane    = vidioc_vdec_g_fmt,
1470
1471         .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
1472
1473         .vidioc_enum_fmt_vid_cap        = vidioc_vdec_enum_fmt_vid_cap,
1474         .vidioc_enum_fmt_vid_out        = vidioc_vdec_enum_fmt_vid_out,
1475         .vidioc_enum_framesizes = vidioc_enum_framesizes,
1476
1477         .vidioc_querycap                = vidioc_vdec_querycap,
1478         .vidioc_subscribe_event         = vidioc_vdec_subscribe_evt,
1479         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
1480         .vidioc_g_selection             = vidioc_vdec_g_selection,
1481         .vidioc_s_selection             = vidioc_vdec_s_selection,
1482
1483         .vidioc_decoder_cmd = vidioc_decoder_cmd,
1484         .vidioc_try_decoder_cmd = vidioc_try_decoder_cmd,
1485 };
1486
1487 int mtk_vcodec_dec_queue_init(void *priv, struct vb2_queue *src_vq,
1488                            struct vb2_queue *dst_vq)
1489 {
1490         struct mtk_vcodec_ctx *ctx = priv;
1491         int ret = 0;
1492
1493         mtk_v4l2_debug(3, "[%d]", ctx->id);
1494
1495         src_vq->type            = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1496         src_vq->io_modes        = VB2_DMABUF | VB2_MMAP;
1497         src_vq->drv_priv        = ctx;
1498         src_vq->buf_struct_size = sizeof(struct mtk_video_dec_buf);
1499         src_vq->ops             = &mtk_vdec_vb2_ops;
1500         src_vq->mem_ops         = &vb2_dma_contig_memops;
1501         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1502         src_vq->lock            = &ctx->dev->dev_mutex;
1503         src_vq->dev             = &ctx->dev->plat_dev->dev;
1504
1505         ret = vb2_queue_init(src_vq);
1506         if (ret) {
1507                 mtk_v4l2_err("Failed to initialize videobuf2 queue(output)");
1508                 return ret;
1509         }
1510         dst_vq->type            = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1511         dst_vq->io_modes        = VB2_DMABUF | VB2_MMAP;
1512         dst_vq->drv_priv        = ctx;
1513         dst_vq->buf_struct_size = sizeof(struct mtk_video_dec_buf);
1514         dst_vq->ops             = &mtk_vdec_vb2_ops;
1515         dst_vq->mem_ops         = &vb2_dma_contig_memops;
1516         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1517         dst_vq->lock            = &ctx->dev->dev_mutex;
1518         dst_vq->dev             = &ctx->dev->plat_dev->dev;
1519
1520         ret = vb2_queue_init(dst_vq);
1521         if (ret)
1522                 mtk_v4l2_err("Failed to initialize videobuf2 queue(capture)");
1523
1524         return ret;
1525 }