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

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3 * Copyright (c) 2016 MediaTek Inc.
   4 * Author: PC Chen <pc.chen@mediatek.com>
   5 *         Tiffany Lin <tiffany.lin@mediatek.com>
   6 */
   7
   8 #include <media/v4l2-event.h>
   9 #include <media/v4l2-mem2mem.h>
  10 #include <media/videobuf2-dma-contig.h>
  11 #include <soc/mediatek/smi.h>
  12
  13 #include "mtk_vcodec_drv.h"
  14 #include "mtk_vcodec_enc.h"
  15 #include "mtk_vcodec_intr.h"
  16 #include "mtk_vcodec_util.h"
  17 #include "venc_drv_if.h"
  18
  19 #define MTK_VENC_MIN_W  160U
  20 #define MTK_VENC_MIN_H  128U
  21 #define MTK_VENC_MAX_W  1920U
  22 #define MTK_VENC_MAX_H  1088U
  23 #define DFT_CFG_WIDTH   MTK_VENC_MIN_W
  24 #define DFT_CFG_HEIGHT  MTK_VENC_MIN_H
  25 #define MTK_MAX_CTRLS_HINT      20
  26 #define OUT_FMT_IDX             0
  27 #define CAP_FMT_IDX             4
  28
  29
  30 static void mtk_venc_worker(struct work_struct *work);
  31
  32 static const struct mtk_video_fmt mtk_video_formats[] = {
  33         {
  34                 .fourcc = V4L2_PIX_FMT_NV12M,
  35                 .type = MTK_FMT_FRAME,
  36                 .num_planes = 2,
  37         },
  38         {
  39                 .fourcc = V4L2_PIX_FMT_NV21M,
  40                 .type = MTK_FMT_FRAME,
  41                 .num_planes = 2,
  42         },
  43         {
  44                 .fourcc = V4L2_PIX_FMT_YUV420M,
  45                 .type = MTK_FMT_FRAME,
  46                 .num_planes = 3,
  47         },
  48         {
  49                 .fourcc = V4L2_PIX_FMT_YVU420M,
  50                 .type = MTK_FMT_FRAME,
  51                 .num_planes = 3,
  52         },
  53         {
  54                 .fourcc = V4L2_PIX_FMT_H264,
  55                 .type = MTK_FMT_ENC,
  56                 .num_planes = 1,
  57         },
  58         {
  59                 .fourcc = V4L2_PIX_FMT_VP8,
  60                 .type = MTK_FMT_ENC,
  61                 .num_planes = 1,
  62         },
  63 };
  64
  65 #define NUM_FORMATS ARRAY_SIZE(mtk_video_formats)
  66
  67 static const struct mtk_codec_framesizes mtk_venc_framesizes[] = {
  68         {
  69                 .fourcc = V4L2_PIX_FMT_H264,
  70                 .stepwise = { MTK_VENC_MIN_W, MTK_VENC_MAX_W, 16,
  71                               MTK_VENC_MIN_H, MTK_VENC_MAX_H, 16 },
  72         },
  73         {
  74                 .fourcc = V4L2_PIX_FMT_VP8,
  75                 .stepwise = { MTK_VENC_MIN_W, MTK_VENC_MAX_W, 16,
  76                               MTK_VENC_MIN_H, MTK_VENC_MAX_H, 16 },
  77         },
  78 };
  79
  80 #define NUM_SUPPORTED_FRAMESIZE ARRAY_SIZE(mtk_venc_framesizes)
  81
  82 static int vidioc_venc_s_ctrl(struct v4l2_ctrl *ctrl)
  83 {
  84         struct mtk_vcodec_ctx *ctx = ctrl_to_ctx(ctrl);
  85         struct mtk_enc_params *p = &ctx->enc_params;
  86         int ret = 0;
  87
  88         switch (ctrl->id) {
  89         case V4L2_CID_MPEG_VIDEO_BITRATE:
  90                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_BITRATE val = %d",
  91                                ctrl->val);
  92                 p->bitrate = ctrl->val;
  93                 ctx->param_change |= MTK_ENCODE_PARAM_BITRATE;
  94                 break;
  95         case V4L2_CID_MPEG_VIDEO_B_FRAMES:
  96                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_B_FRAMES val = %d",
  97                                ctrl->val);
  98                 p->num_b_frame = ctrl->val;
  99                 break;
 100         case V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE:
 101                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE val = %d",
 102                                ctrl->val);
 103                 p->rc_frame = ctrl->val;
 104                 break;
 105         case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
 106                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_MAX_QP val = %d",
 107                                ctrl->val);
 108                 p->h264_max_qp = ctrl->val;
 109                 break;
 110         case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
 111                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_HEADER_MODE val = %d",
 112                                ctrl->val);
 113                 p->seq_hdr_mode = ctrl->val;
 114                 break;
 115         case V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE:
 116                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE val = %d",
 117                                ctrl->val);
 118                 p->rc_mb = ctrl->val;
 119                 break;
 120         case V4L2_CID_MPEG_VIDEO_H264_PROFILE:
 121                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_PROFILE val = %d",
 122                                ctrl->val);
 123                 p->h264_profile = ctrl->val;
 124                 break;
 125         case V4L2_CID_MPEG_VIDEO_H264_LEVEL:
 126                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_LEVEL val = %d",
 127                                ctrl->val);
 128                 p->h264_level = ctrl->val;
 129                 break;
 130         case V4L2_CID_MPEG_VIDEO_H264_I_PERIOD:
 131                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_H264_I_PERIOD val = %d",
 132                                ctrl->val);
 133                 p->intra_period = ctrl->val;
 134                 ctx->param_change |= MTK_ENCODE_PARAM_INTRA_PERIOD;
 135                 break;
 136         case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
 137                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_GOP_SIZE val = %d",
 138                                ctrl->val);
 139                 p->gop_size = ctrl->val;
 140                 ctx->param_change |= MTK_ENCODE_PARAM_GOP_SIZE;
 141                 break;
 142         case V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME:
 143                 mtk_v4l2_debug(2, "V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME");
 144                 p->force_intra = 1;
 145                 ctx->param_change |= MTK_ENCODE_PARAM_FORCE_INTRA;
 146                 break;
 147         default:
 148                 ret = -EINVAL;
 149                 break;
 150         }
 151
 152         return ret;
 153 }
 154
 155 static const struct v4l2_ctrl_ops mtk_vcodec_enc_ctrl_ops = {
 156         .s_ctrl = vidioc_venc_s_ctrl,
 157 };
 158
 159 static int vidioc_enum_fmt(struct v4l2_fmtdesc *f, bool output_queue)
 160 {
 161         const struct mtk_video_fmt *fmt;
 162         int i, j = 0;
 163
 164         for (i = 0; i < NUM_FORMATS; ++i) {
 165                 if (output_queue && mtk_video_formats[i].type != MTK_FMT_FRAME)
 166                         continue;
 167                 if (!output_queue && mtk_video_formats[i].type != MTK_FMT_ENC)
 168                         continue;
 169
 170                 if (j == f->index) {
 171                         fmt = &mtk_video_formats[i];
 172                         f->pixelformat = fmt->fourcc;
 173                         memset(f->reserved, 0, sizeof(f->reserved));
 174                         return 0;
 175                 }
 176                 ++j;
 177         }
 178
 179         return -EINVAL;
 180 }
 181
 182 static int vidioc_enum_framesizes(struct file *file, void *fh,
 183                                   struct v4l2_frmsizeenum *fsize)
 184 {
 185         int i = 0;
 186
 187         if (fsize->index != 0)
 188                 return -EINVAL;
 189
 190         for (i = 0; i < NUM_SUPPORTED_FRAMESIZE; ++i) {
 191                 if (fsize->pixel_format != mtk_venc_framesizes[i].fourcc)
 192                         continue;
 193
 194                 fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
 195                 fsize->stepwise = mtk_venc_framesizes[i].stepwise;
 196                 return 0;
 197         }
 198
 199         return -EINVAL;
 200 }
 201
 202 static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
 203                                    struct v4l2_fmtdesc *f)
 204 {
 205         return vidioc_enum_fmt(f, false);
 206 }
 207
 208 static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
 209                                    struct v4l2_fmtdesc *f)
 210 {
 211         return vidioc_enum_fmt(f, true);
 212 }
 213
 214 static int vidioc_venc_querycap(struct file *file, void *priv,
 215                                 struct v4l2_capability *cap)
 216 {
 217         strscpy(cap->driver, MTK_VCODEC_ENC_NAME, sizeof(cap->driver));
 218         strscpy(cap->bus_info, MTK_PLATFORM_STR, sizeof(cap->bus_info));
 219         strscpy(cap->card, MTK_PLATFORM_STR, sizeof(cap->card));
 220
 221         return 0;
 222 }
 223
 224 static int vidioc_venc_s_parm(struct file *file, void *priv,
 225                               struct v4l2_streamparm *a)
 226 {
 227         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 228
 229         if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 230                 return -EINVAL;
 231
 232         ctx->enc_params.framerate_num =
 233                         a->parm.output.timeperframe.denominator;
 234         ctx->enc_params.framerate_denom =
 235                         a->parm.output.timeperframe.numerator;
 236         ctx->param_change |= MTK_ENCODE_PARAM_FRAMERATE;
 237
 238         a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
 239
 240         return 0;
 241 }
 242
 243 static int vidioc_venc_g_parm(struct file *file, void *priv,
 244                               struct v4l2_streamparm *a)
 245 {
 246         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 247
 248         if (a->type != V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE)
 249                 return -EINVAL;
 250
 251         a->parm.output.capability = V4L2_CAP_TIMEPERFRAME;
 252         a->parm.output.timeperframe.denominator =
 253                         ctx->enc_params.framerate_num;
 254         a->parm.output.timeperframe.numerator =
 255                         ctx->enc_params.framerate_denom;
 256
 257         return 0;
 258 }
 259
 260 static struct mtk_q_data *mtk_venc_get_q_data(struct mtk_vcodec_ctx *ctx,
 261                                               enum v4l2_buf_type type)
 262 {
 263         if (V4L2_TYPE_IS_OUTPUT(type))
 264                 return &ctx->q_data[MTK_Q_DATA_SRC];
 265
 266         return &ctx->q_data[MTK_Q_DATA_DST];
 267 }
 268
 269 static const struct mtk_video_fmt *mtk_venc_find_format(struct v4l2_format *f)
 270 {
 271         const struct mtk_video_fmt *fmt;
 272         unsigned int k;
 273
 274         for (k = 0; k < NUM_FORMATS; k++) {
 275                 fmt = &mtk_video_formats[k];
 276                 if (fmt->fourcc == f->fmt.pix.pixelformat)
 277                         return fmt;
 278         }
 279
 280         return NULL;
 281 }
 282
 283 /* V4L2 specification suggests the driver corrects the format struct if any of
 284  * the dimensions is unsupported
 285  */
 286 static int vidioc_try_fmt(struct v4l2_format *f,
 287                           const struct mtk_video_fmt *fmt)
 288 {
 289         struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
 290         int i;
 291
 292         pix_fmt_mp->field = V4L2_FIELD_NONE;
 293
 294         if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 295                 pix_fmt_mp->num_planes = 1;
 296                 pix_fmt_mp->plane_fmt[0].bytesperline = 0;
 297         } else if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 298                 int tmp_w, tmp_h;
 299
 300                 pix_fmt_mp->height = clamp(pix_fmt_mp->height,
 301                                         MTK_VENC_MIN_H,
 302                                         MTK_VENC_MAX_H);
 303                 pix_fmt_mp->width = clamp(pix_fmt_mp->width,
 304                                         MTK_VENC_MIN_W,
 305                                         MTK_VENC_MAX_W);
 306
 307                 /* find next closer width align 16, heign align 32, size align
 308                  * 64 rectangle
 309                  */
 310                 tmp_w = pix_fmt_mp->width;
 311                 tmp_h = pix_fmt_mp->height;
 312                 v4l_bound_align_image(&pix_fmt_mp->width,
 313                                         MTK_VENC_MIN_W,
 314                                         MTK_VENC_MAX_W, 4,
 315                                         &pix_fmt_mp->height,
 316                                         MTK_VENC_MIN_H,
 317                                         MTK_VENC_MAX_H, 5, 6);
 318
 319                 if (pix_fmt_mp->width < tmp_w &&
 320                         (pix_fmt_mp->width + 16) <= MTK_VENC_MAX_W)
 321                         pix_fmt_mp->width += 16;
 322                 if (pix_fmt_mp->height < tmp_h &&
 323                         (pix_fmt_mp->height + 32) <= MTK_VENC_MAX_H)
 324                         pix_fmt_mp->height += 32;
 325
 326                 mtk_v4l2_debug(0,
 327                         "before resize width=%d, height=%d, after resize width=%d, height=%d, sizeimage=%d %d",
 328                         tmp_w, tmp_h, pix_fmt_mp->width,
 329                         pix_fmt_mp->height,
 330                         pix_fmt_mp->plane_fmt[0].sizeimage,
 331                         pix_fmt_mp->plane_fmt[1].sizeimage);
 332
 333                 pix_fmt_mp->num_planes = fmt->num_planes;
 334                 pix_fmt_mp->plane_fmt[0].sizeimage =
 335                                 pix_fmt_mp->width * pix_fmt_mp->height +
 336                                 ((ALIGN(pix_fmt_mp->width, 16) * 2) * 16);
 337                 pix_fmt_mp->plane_fmt[0].bytesperline = pix_fmt_mp->width;
 338
 339                 if (pix_fmt_mp->num_planes == 2) {
 340                         pix_fmt_mp->plane_fmt[1].sizeimage =
 341                                 (pix_fmt_mp->width * pix_fmt_mp->height) / 2 +
 342                                 (ALIGN(pix_fmt_mp->width, 16) * 16);
 343                         pix_fmt_mp->plane_fmt[2].sizeimage = 0;
 344                         pix_fmt_mp->plane_fmt[1].bytesperline =
 345                                                         pix_fmt_mp->width;
 346                         pix_fmt_mp->plane_fmt[2].bytesperline = 0;
 347                 } else if (pix_fmt_mp->num_planes == 3) {
 348                         pix_fmt_mp->plane_fmt[1].sizeimage =
 349                         pix_fmt_mp->plane_fmt[2].sizeimage =
 350                                 (pix_fmt_mp->width * pix_fmt_mp->height) / 4 +
 351                                 ((ALIGN(pix_fmt_mp->width, 16) / 2) * 16);
 352                         pix_fmt_mp->plane_fmt[1].bytesperline =
 353                                 pix_fmt_mp->plane_fmt[2].bytesperline =
 354                                 pix_fmt_mp->width / 2;
 355                 }
 356         }
 357
 358         for (i = 0; i < pix_fmt_mp->num_planes; i++)
 359                 memset(&(pix_fmt_mp->plane_fmt[i].reserved[0]), 0x0,
 360                            sizeof(pix_fmt_mp->plane_fmt[0].reserved));
 361
 362         pix_fmt_mp->flags = 0;
 363         memset(&pix_fmt_mp->reserved, 0x0,
 364                 sizeof(pix_fmt_mp->reserved));
 365
 366         return 0;
 367 }
 368
 369 static void mtk_venc_set_param(struct mtk_vcodec_ctx *ctx,
 370                                 struct venc_enc_param *param)
 371 {
 372         struct mtk_q_data *q_data_src = &ctx->q_data[MTK_Q_DATA_SRC];
 373         struct mtk_enc_params *enc_params = &ctx->enc_params;
 374
 375         switch (q_data_src->fmt->fourcc) {
 376         case V4L2_PIX_FMT_YUV420M:
 377                 param->input_yuv_fmt = VENC_YUV_FORMAT_I420;
 378                 break;
 379         case V4L2_PIX_FMT_YVU420M:
 380                 param->input_yuv_fmt = VENC_YUV_FORMAT_YV12;
 381                 break;
 382         case V4L2_PIX_FMT_NV12M:
 383                 param->input_yuv_fmt = VENC_YUV_FORMAT_NV12;
 384                 break;
 385         case V4L2_PIX_FMT_NV21M:
 386                 param->input_yuv_fmt = VENC_YUV_FORMAT_NV21;
 387                 break;
 388         default:
 389                 mtk_v4l2_err("Unsupported fourcc =%d", q_data_src->fmt->fourcc);
 390                 break;
 391         }
 392         param->h264_profile = enc_params->h264_profile;
 393         param->h264_level = enc_params->h264_level;
 394
 395         /* Config visible resolution */
 396         param->width = q_data_src->visible_width;
 397         param->height = q_data_src->visible_height;
 398         /* Config coded resolution */
 399         param->buf_width = q_data_src->coded_width;
 400         param->buf_height = q_data_src->coded_height;
 401         param->frm_rate = enc_params->framerate_num /
 402                         enc_params->framerate_denom;
 403         param->intra_period = enc_params->intra_period;
 404         param->gop_size = enc_params->gop_size;
 405         param->bitrate = enc_params->bitrate;
 406
 407         mtk_v4l2_debug(0,
 408                 "fmt 0x%x, P/L %d/%d, w/h %d/%d, buf %d/%d, fps/bps %d/%d, gop %d, i_period %d",
 409                 param->input_yuv_fmt, param->h264_profile,
 410                 param->h264_level, param->width, param->height,
 411                 param->buf_width, param->buf_height,
 412                 param->frm_rate, param->bitrate,
 413                 param->gop_size, param->intra_period);
 414 }
 415
 416 static int vidioc_venc_s_fmt_cap(struct file *file, void *priv,
 417                              struct v4l2_format *f)
 418 {
 419         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 420         struct vb2_queue *vq;
 421         struct mtk_q_data *q_data;
 422         int i, ret;
 423         const struct mtk_video_fmt *fmt;
 424
 425         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 426         if (!vq) {
 427                 mtk_v4l2_err("fail to get vq");
 428                 return -EINVAL;
 429         }
 430
 431         if (vb2_is_busy(vq)) {
 432                 mtk_v4l2_err("queue busy");
 433                 return -EBUSY;
 434         }
 435
 436         q_data = mtk_venc_get_q_data(ctx, f->type);
 437         if (!q_data) {
 438                 mtk_v4l2_err("fail to get q data");
 439                 return -EINVAL;
 440         }
 441
 442         fmt = mtk_venc_find_format(f);
 443         if (!fmt) {
 444                 f->fmt.pix.pixelformat = mtk_video_formats[CAP_FMT_IDX].fourcc;
 445                 fmt = mtk_venc_find_format(f);
 446         }
 447
 448         q_data->fmt = fmt;
 449         ret = vidioc_try_fmt(f, q_data->fmt);
 450         if (ret)
 451                 return ret;
 452
 453         q_data->coded_width = f->fmt.pix_mp.width;
 454         q_data->coded_height = f->fmt.pix_mp.height;
 455         q_data->field = f->fmt.pix_mp.field;
 456
 457         for (i = 0; i < f->fmt.pix_mp.num_planes; i++) {
 458                 struct v4l2_plane_pix_format    *plane_fmt;
 459
 460                 plane_fmt = &f->fmt.pix_mp.plane_fmt[i];
 461                 q_data->bytesperline[i] = plane_fmt->bytesperline;
 462                 q_data->sizeimage[i] = plane_fmt->sizeimage;
 463         }
 464
 465         if (ctx->state == MTK_STATE_FREE) {
 466                 ret = venc_if_init(ctx, q_data->fmt->fourcc);
 467                 if (ret) {
 468                         mtk_v4l2_err("venc_if_init failed=%d, codec type=%x",
 469                                         ret, q_data->fmt->fourcc);
 470                         return -EBUSY;
 471                 }
 472                 ctx->state = MTK_STATE_INIT;
 473         }
 474
 475         return 0;
 476 }
 477
 478 static int vidioc_venc_s_fmt_out(struct file *file, void *priv,
 479                              struct v4l2_format *f)
 480 {
 481         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 482         struct vb2_queue *vq;
 483         struct mtk_q_data *q_data;
 484         int ret, i;
 485         const struct mtk_video_fmt *fmt;
 486         struct v4l2_pix_format_mplane *pix_fmt_mp = &f->fmt.pix_mp;
 487
 488         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 489         if (!vq) {
 490                 mtk_v4l2_err("fail to get vq");
 491                 return -EINVAL;
 492         }
 493
 494         if (vb2_is_busy(vq)) {
 495                 mtk_v4l2_err("queue busy");
 496                 return -EBUSY;
 497         }
 498
 499         q_data = mtk_venc_get_q_data(ctx, f->type);
 500         if (!q_data) {
 501                 mtk_v4l2_err("fail to get q data");
 502                 return -EINVAL;
 503         }
 504
 505         fmt = mtk_venc_find_format(f);
 506         if (!fmt) {
 507                 f->fmt.pix.pixelformat = mtk_video_formats[OUT_FMT_IDX].fourcc;
 508                 fmt = mtk_venc_find_format(f);
 509         }
 510
 511         pix_fmt_mp->height = clamp(pix_fmt_mp->height,
 512                                 MTK_VENC_MIN_H,
 513                                 MTK_VENC_MAX_H);
 514         pix_fmt_mp->width = clamp(pix_fmt_mp->width,
 515                                 MTK_VENC_MIN_W,
 516                                 MTK_VENC_MAX_W);
 517
 518         q_data->visible_width = f->fmt.pix_mp.width;
 519         q_data->visible_height = f->fmt.pix_mp.height;
 520         q_data->fmt = fmt;
 521         ret = vidioc_try_fmt(f, q_data->fmt);
 522         if (ret)
 523                 return ret;
 524
 525         q_data->coded_width = f->fmt.pix_mp.width;
 526         q_data->coded_height = f->fmt.pix_mp.height;
 527
 528         q_data->field = f->fmt.pix_mp.field;
 529         ctx->colorspace = f->fmt.pix_mp.colorspace;
 530         ctx->ycbcr_enc = f->fmt.pix_mp.ycbcr_enc;
 531         ctx->quantization = f->fmt.pix_mp.quantization;
 532         ctx->xfer_func = f->fmt.pix_mp.xfer_func;
 533
 534         for (i = 0; i < f->fmt.pix_mp.num_planes; i++) {
 535                 struct v4l2_plane_pix_format *plane_fmt;
 536
 537                 plane_fmt = &f->fmt.pix_mp.plane_fmt[i];
 538                 q_data->bytesperline[i] = plane_fmt->bytesperline;
 539                 q_data->sizeimage[i] = plane_fmt->sizeimage;
 540         }
 541
 542         return 0;
 543 }
 544
 545 static int vidioc_venc_g_fmt(struct file *file, void *priv,
 546                              struct v4l2_format *f)
 547 {
 548         struct v4l2_pix_format_mplane *pix = &f->fmt.pix_mp;
 549         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 550         struct vb2_queue *vq;
 551         struct mtk_q_data *q_data;
 552         int i;
 553
 554         vq = v4l2_m2m_get_vq(ctx->m2m_ctx, f->type);
 555         if (!vq)
 556                 return -EINVAL;
 557
 558         q_data = mtk_venc_get_q_data(ctx, f->type);
 559
 560         pix->width = q_data->coded_width;
 561         pix->height = q_data->coded_height;
 562         pix->pixelformat = q_data->fmt->fourcc;
 563         pix->field = q_data->field;
 564         pix->num_planes = q_data->fmt->num_planes;
 565         for (i = 0; i < pix->num_planes; i++) {
 566                 pix->plane_fmt[i].bytesperline = q_data->bytesperline[i];
 567                 pix->plane_fmt[i].sizeimage = q_data->sizeimage[i];
 568                 memset(&(pix->plane_fmt[i].reserved[0]), 0x0,
 569                        sizeof(pix->plane_fmt[i].reserved));
 570         }
 571
 572         pix->flags = 0;
 573         pix->colorspace = ctx->colorspace;
 574         pix->ycbcr_enc = ctx->ycbcr_enc;
 575         pix->quantization = ctx->quantization;
 576         pix->xfer_func = ctx->xfer_func;
 577
 578         return 0;
 579 }
 580
 581 static int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
 582                                          struct v4l2_format *f)
 583 {
 584         const struct mtk_video_fmt *fmt;
 585         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 586
 587         fmt = mtk_venc_find_format(f);
 588         if (!fmt) {
 589                 f->fmt.pix.pixelformat = mtk_video_formats[CAP_FMT_IDX].fourcc;
 590                 fmt = mtk_venc_find_format(f);
 591         }
 592         f->fmt.pix_mp.colorspace = ctx->colorspace;
 593         f->fmt.pix_mp.ycbcr_enc = ctx->ycbcr_enc;
 594         f->fmt.pix_mp.quantization = ctx->quantization;
 595         f->fmt.pix_mp.xfer_func = ctx->xfer_func;
 596
 597         return vidioc_try_fmt(f, fmt);
 598 }
 599
 600 static int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
 601                                          struct v4l2_format *f)
 602 {
 603         const struct mtk_video_fmt *fmt;
 604
 605         fmt = mtk_venc_find_format(f);
 606         if (!fmt) {
 607                 f->fmt.pix.pixelformat = mtk_video_formats[OUT_FMT_IDX].fourcc;
 608                 fmt = mtk_venc_find_format(f);
 609         }
 610         if (!f->fmt.pix_mp.colorspace) {
 611                 f->fmt.pix_mp.colorspace = V4L2_COLORSPACE_REC709;
 612                 f->fmt.pix_mp.ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
 613                 f->fmt.pix_mp.quantization = V4L2_QUANTIZATION_DEFAULT;
 614                 f->fmt.pix_mp.xfer_func = V4L2_XFER_FUNC_DEFAULT;
 615         }
 616
 617         return vidioc_try_fmt(f, fmt);
 618 }
 619
 620 static int vidioc_venc_g_selection(struct file *file, void *priv,
 621                                      struct v4l2_selection *s)
 622 {
 623         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 624         struct mtk_q_data *q_data;
 625
 626         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 627                 return -EINVAL;
 628
 629         q_data = mtk_venc_get_q_data(ctx, s->type);
 630         if (!q_data)
 631                 return -EINVAL;
 632
 633         switch (s->target) {
 634         case V4L2_SEL_TGT_CROP_DEFAULT:
 635         case V4L2_SEL_TGT_CROP_BOUNDS:
 636                 s->r.top = 0;
 637                 s->r.left = 0;
 638                 s->r.width = q_data->coded_width;
 639                 s->r.height = q_data->coded_height;
 640                 break;
 641         case V4L2_SEL_TGT_CROP:
 642                 s->r.top = 0;
 643                 s->r.left = 0;
 644                 s->r.width = q_data->visible_width;
 645                 s->r.height = q_data->visible_height;
 646                 break;
 647         default:
 648                 return -EINVAL;
 649         }
 650
 651         return 0;
 652 }
 653
 654 static int vidioc_venc_s_selection(struct file *file, void *priv,
 655                                      struct v4l2_selection *s)
 656 {
 657         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 658         struct mtk_q_data *q_data;
 659
 660         if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 661                 return -EINVAL;
 662
 663         q_data = mtk_venc_get_q_data(ctx, s->type);
 664         if (!q_data)
 665                 return -EINVAL;
 666
 667         switch (s->target) {
 668         case V4L2_SEL_TGT_CROP:
 669                 /* Only support crop from (0,0) */
 670                 s->r.top = 0;
 671                 s->r.left = 0;
 672                 s->r.width = min(s->r.width, q_data->coded_width);
 673                 s->r.height = min(s->r.height, q_data->coded_height);
 674                 q_data->visible_width = s->r.width;
 675                 q_data->visible_height = s->r.height;
 676                 break;
 677         default:
 678                 return -EINVAL;
 679         }
 680         return 0;
 681 }
 682
 683 static int vidioc_venc_qbuf(struct file *file, void *priv,
 684                             struct v4l2_buffer *buf)
 685 {
 686         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 687
 688         if (ctx->state == MTK_STATE_ABORT) {
 689                 mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
 690                                 ctx->id);
 691                 return -EIO;
 692         }
 693
 694         return v4l2_m2m_qbuf(file, ctx->m2m_ctx, buf);
 695 }
 696
 697 static int vidioc_venc_dqbuf(struct file *file, void *priv,
 698                              struct v4l2_buffer *buf)
 699 {
 700         struct mtk_vcodec_ctx *ctx = fh_to_ctx(priv);
 701
 702         if (ctx->state == MTK_STATE_ABORT) {
 703                 mtk_v4l2_err("[%d] Call on QBUF after unrecoverable error",
 704                                 ctx->id);
 705                 return -EIO;
 706         }
 707
 708         return v4l2_m2m_dqbuf(file, ctx->m2m_ctx, buf);
 709 }
 710
 711 const struct v4l2_ioctl_ops mtk_venc_ioctl_ops = {
 712         .vidioc_streamon                = v4l2_m2m_ioctl_streamon,
 713         .vidioc_streamoff               = v4l2_m2m_ioctl_streamoff,
 714
 715         .vidioc_reqbufs                 = v4l2_m2m_ioctl_reqbufs,
 716         .vidioc_querybuf                = v4l2_m2m_ioctl_querybuf,
 717         .vidioc_qbuf                    = vidioc_venc_qbuf,
 718         .vidioc_dqbuf                   = vidioc_venc_dqbuf,
 719
 720         .vidioc_querycap                = vidioc_venc_querycap,
 721         .vidioc_enum_fmt_vid_cap        = vidioc_enum_fmt_vid_cap,
 722         .vidioc_enum_fmt_vid_out        = vidioc_enum_fmt_vid_out,
 723         .vidioc_enum_framesizes         = vidioc_enum_framesizes,
 724
 725         .vidioc_try_fmt_vid_cap_mplane  = vidioc_try_fmt_vid_cap_mplane,
 726         .vidioc_try_fmt_vid_out_mplane  = vidioc_try_fmt_vid_out_mplane,
 727         .vidioc_expbuf                  = v4l2_m2m_ioctl_expbuf,
 728         .vidioc_subscribe_event         = v4l2_ctrl_subscribe_event,
 729         .vidioc_unsubscribe_event       = v4l2_event_unsubscribe,
 730
 731         .vidioc_s_parm                  = vidioc_venc_s_parm,
 732         .vidioc_g_parm                  = vidioc_venc_g_parm,
 733         .vidioc_s_fmt_vid_cap_mplane    = vidioc_venc_s_fmt_cap,
 734         .vidioc_s_fmt_vid_out_mplane    = vidioc_venc_s_fmt_out,
 735
 736         .vidioc_g_fmt_vid_cap_mplane    = vidioc_venc_g_fmt,
 737         .vidioc_g_fmt_vid_out_mplane    = vidioc_venc_g_fmt,
 738
 739         .vidioc_create_bufs             = v4l2_m2m_ioctl_create_bufs,
 740         .vidioc_prepare_buf             = v4l2_m2m_ioctl_prepare_buf,
 741
 742         .vidioc_g_selection             = vidioc_venc_g_selection,
 743         .vidioc_s_selection             = vidioc_venc_s_selection,
 744 };
 745
 746 static int vb2ops_venc_queue_setup(struct vb2_queue *vq,
 747                                    unsigned int *nbuffers,
 748                                    unsigned int *nplanes,
 749                                    unsigned int sizes[],
 750                                    struct device *alloc_devs[])
 751 {
 752         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vq);
 753         struct mtk_q_data *q_data;
 754         unsigned int i;
 755
 756         q_data = mtk_venc_get_q_data(ctx, vq->type);
 757
 758         if (q_data == NULL)
 759                 return -EINVAL;
 760
 761         if (*nplanes) {
 762                 for (i = 0; i < *nplanes; i++)
 763                         if (sizes[i] < q_data->sizeimage[i])
 764                                 return -EINVAL;
 765         } else {
 766                 *nplanes = q_data->fmt->num_planes;
 767                 for (i = 0; i < *nplanes; i++)
 768                         sizes[i] = q_data->sizeimage[i];
 769         }
 770
 771         return 0;
 772 }
 773
 774 static int vb2ops_venc_buf_prepare(struct vb2_buffer *vb)
 775 {
 776         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 777         struct mtk_q_data *q_data;
 778         int i;
 779
 780         q_data = mtk_venc_get_q_data(ctx, vb->vb2_queue->type);
 781
 782         for (i = 0; i < q_data->fmt->num_planes; i++) {
 783                 if (vb2_plane_size(vb, i) < q_data->sizeimage[i]) {
 784                         mtk_v4l2_err("data will not fit into plane %d (%lu < %d)",
 785                                 i, vb2_plane_size(vb, i),
 786                                 q_data->sizeimage[i]);
 787                         return -EINVAL;
 788                 }
 789         }
 790
 791         return 0;
 792 }
 793
 794 static void vb2ops_venc_buf_queue(struct vb2_buffer *vb)
 795 {
 796         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
 797         struct vb2_v4l2_buffer *vb2_v4l2 =
 798                         container_of(vb, struct vb2_v4l2_buffer, vb2_buf);
 799
 800         struct mtk_video_enc_buf *mtk_buf =
 801                         container_of(vb2_v4l2, struct mtk_video_enc_buf, vb);
 802
 803         if ((vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) &&
 804             (ctx->param_change != MTK_ENCODE_PARAM_NONE)) {
 805                 mtk_v4l2_debug(1, "[%d] Before id=%d encode parameter change %x",
 806                                ctx->id,
 807                                mtk_buf->vb.vb2_buf.index,
 808                                ctx->param_change);
 809                 mtk_buf->param_change = ctx->param_change;
 810                 mtk_buf->enc_params = ctx->enc_params;
 811                 ctx->param_change = MTK_ENCODE_PARAM_NONE;
 812         }
 813
 814         v4l2_m2m_buf_queue(ctx->m2m_ctx, to_vb2_v4l2_buffer(vb));
 815 }
 816
 817 static int vb2ops_venc_start_streaming(struct vb2_queue *q, unsigned int count)
 818 {
 819         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
 820         struct venc_enc_param param;
 821         int ret;
 822         int i;
 823
 824         /* Once state turn into MTK_STATE_ABORT, we need stop_streaming
 825           * to clear it
 826           */
 827         if ((ctx->state == MTK_STATE_ABORT) || (ctx->state == MTK_STATE_FREE)) {
 828                 ret = -EIO;
 829                 goto err_set_param;
 830         }
 831
 832         /* Do the initialization when both start_streaming have been called */
 833         if (V4L2_TYPE_IS_OUTPUT(q->type)) {
 834                 if (!vb2_start_streaming_called(&ctx->m2m_ctx->cap_q_ctx.q))
 835                         return 0;
 836         } else {
 837                 if (!vb2_start_streaming_called(&ctx->m2m_ctx->out_q_ctx.q))
 838                         return 0;
 839         }
 840
 841         mtk_venc_set_param(ctx, &param);
 842         ret = venc_if_set_param(ctx, VENC_SET_PARAM_ENC, &param);
 843         if (ret) {
 844                 mtk_v4l2_err("venc_if_set_param failed=%d", ret);
 845                 ctx->state = MTK_STATE_ABORT;
 846                 goto err_set_param;
 847         }
 848         ctx->param_change = MTK_ENCODE_PARAM_NONE;
 849
 850         if ((ctx->q_data[MTK_Q_DATA_DST].fmt->fourcc == V4L2_PIX_FMT_H264) &&
 851             (ctx->enc_params.seq_hdr_mode !=
 852                                 V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE)) {
 853                 ret = venc_if_set_param(ctx,
 854                                         VENC_SET_PARAM_PREPEND_HEADER,
 855                                         NULL);
 856                 if (ret) {
 857                         mtk_v4l2_err("venc_if_set_param failed=%d", ret);
 858                         ctx->state = MTK_STATE_ABORT;
 859                         goto err_set_param;
 860                 }
 861                 ctx->state = MTK_STATE_HEADER;
 862         }
 863
 864         return 0;
 865
 866 err_set_param:
 867         for (i = 0; i < q->num_buffers; ++i) {
 868                 struct vb2_buffer *buf = vb2_get_buffer(q, i);
 869
 870                 /*
 871                  * FIXME: This check is not needed as only active buffers
 872                  * can be marked as done.
 873                  */
 874                 if (buf->state == VB2_BUF_STATE_ACTIVE) {
 875                         mtk_v4l2_debug(0, "[%d] id=%d, type=%d, %d -> VB2_BUF_STATE_QUEUED",
 876                                         ctx->id, i, q->type,
 877                                         (int)buf->state);
 878                         v4l2_m2m_buf_done(to_vb2_v4l2_buffer(buf),
 879                                           VB2_BUF_STATE_QUEUED);
 880                 }
 881         }
 882
 883         return ret;
 884 }
 885
 886 static void vb2ops_venc_stop_streaming(struct vb2_queue *q)
 887 {
 888         struct mtk_vcodec_ctx *ctx = vb2_get_drv_priv(q);
 889         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 890         int ret;
 891
 892         mtk_v4l2_debug(2, "[%d]-> type=%d", ctx->id, q->type);
 893
 894         if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 895                 while ((dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx))) {
 896                         dst_buf->vb2_buf.planes[0].bytesused = 0;
 897                         v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
 898                 }
 899         } else {
 900                 while ((src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx)))
 901                         v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
 902         }
 903
 904         if ((q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE &&
 905              vb2_is_streaming(&ctx->m2m_ctx->out_q_ctx.q)) ||
 906             (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE &&
 907              vb2_is_streaming(&ctx->m2m_ctx->cap_q_ctx.q))) {
 908                 mtk_v4l2_debug(1, "[%d]-> q type %d out=%d cap=%d",
 909                                ctx->id, q->type,
 910                                vb2_is_streaming(&ctx->m2m_ctx->out_q_ctx.q),
 911                                vb2_is_streaming(&ctx->m2m_ctx->cap_q_ctx.q));
 912                 return;
 913         }
 914
 915         /* Release the encoder if both streams are stopped. */
 916         ret = venc_if_deinit(ctx);
 917         if (ret)
 918                 mtk_v4l2_err("venc_if_deinit failed=%d", ret);
 919
 920         ctx->state = MTK_STATE_FREE;
 921 }
 922
 923 static const struct vb2_ops mtk_venc_vb2_ops = {
 924         .queue_setup            = vb2ops_venc_queue_setup,
 925         .buf_prepare            = vb2ops_venc_buf_prepare,
 926         .buf_queue              = vb2ops_venc_buf_queue,
 927         .wait_prepare           = vb2_ops_wait_prepare,
 928         .wait_finish            = vb2_ops_wait_finish,
 929         .start_streaming        = vb2ops_venc_start_streaming,
 930         .stop_streaming         = vb2ops_venc_stop_streaming,
 931 };
 932
 933 static int mtk_venc_encode_header(void *priv)
 934 {
 935         struct mtk_vcodec_ctx *ctx = priv;
 936         int ret;
 937         struct vb2_v4l2_buffer *src_buf, *dst_buf;
 938         struct mtk_vcodec_mem bs_buf;
 939         struct venc_done_result enc_result;
 940
 941         dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
 942         if (!dst_buf) {
 943                 mtk_v4l2_debug(1, "No dst buffer");
 944                 return -EINVAL;
 945         }
 946
 947         bs_buf.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
 948         bs_buf.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
 949         bs_buf.size = (size_t)dst_buf->vb2_buf.planes[0].length;
 950
 951         mtk_v4l2_debug(1,
 952                         "[%d] buf id=%d va=0x%p dma_addr=0x%llx size=%zu",
 953                         ctx->id,
 954                         dst_buf->vb2_buf.index, bs_buf.va,
 955                         (u64)bs_buf.dma_addr,
 956                         bs_buf.size);
 957
 958         ret = venc_if_encode(ctx,
 959                         VENC_START_OPT_ENCODE_SEQUENCE_HEADER,
 960                         NULL, &bs_buf, &enc_result);
 961
 962         if (ret) {
 963                 dst_buf->vb2_buf.planes[0].bytesused = 0;
 964                 ctx->state = MTK_STATE_ABORT;
 965                 v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
 966                 mtk_v4l2_err("venc_if_encode failed=%d", ret);
 967                 return -EINVAL;
 968         }
 969         src_buf = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 970         if (src_buf) {
 971                 dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
 972                 dst_buf->timecode = src_buf->timecode;
 973         } else {
 974                 mtk_v4l2_err("No timestamp for the header buffer.");
 975         }
 976
 977         ctx->state = MTK_STATE_HEADER;
 978         dst_buf->vb2_buf.planes[0].bytesused = enc_result.bs_size;
 979         v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
 980
 981         return 0;
 982 }
 983
 984 static int mtk_venc_param_change(struct mtk_vcodec_ctx *ctx)
 985 {
 986         struct venc_enc_param enc_prm;
 987         struct vb2_v4l2_buffer *vb2_v4l2 = v4l2_m2m_next_src_buf(ctx->m2m_ctx);
 988         struct mtk_video_enc_buf *mtk_buf =
 989                         container_of(vb2_v4l2, struct mtk_video_enc_buf, vb);
 990
 991         int ret = 0;
 992
 993         memset(&enc_prm, 0, sizeof(enc_prm));
 994         if (mtk_buf->param_change == MTK_ENCODE_PARAM_NONE)
 995                 return 0;
 996
 997         if (mtk_buf->param_change & MTK_ENCODE_PARAM_BITRATE) {
 998                 enc_prm.bitrate = mtk_buf->enc_params.bitrate;
 999                 mtk_v4l2_debug(1, "[%d] id=%d, change param br=%d",
1000                                 ctx->id,
1001                                 mtk_buf->vb.vb2_buf.index,
1002                                 enc_prm.bitrate);
1003                 ret |= venc_if_set_param(ctx,
1004                                          VENC_SET_PARAM_ADJUST_BITRATE,
1005                                          &enc_prm);
1006         }
1007         if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_FRAMERATE) {
1008                 enc_prm.frm_rate = mtk_buf->enc_params.framerate_num /
1009                                    mtk_buf->enc_params.framerate_denom;
1010                 mtk_v4l2_debug(1, "[%d] id=%d, change param fr=%d",
1011                                ctx->id,
1012                                mtk_buf->vb.vb2_buf.index,
1013                                enc_prm.frm_rate);
1014                 ret |= venc_if_set_param(ctx,
1015                                          VENC_SET_PARAM_ADJUST_FRAMERATE,
1016                                          &enc_prm);
1017         }
1018         if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_GOP_SIZE) {
1019                 enc_prm.gop_size = mtk_buf->enc_params.gop_size;
1020                 mtk_v4l2_debug(1, "change param intra period=%d",
1021                                enc_prm.gop_size);
1022                 ret |= venc_if_set_param(ctx,
1023                                          VENC_SET_PARAM_GOP_SIZE,
1024                                          &enc_prm);
1025         }
1026         if (!ret && mtk_buf->param_change & MTK_ENCODE_PARAM_FORCE_INTRA) {
1027                 mtk_v4l2_debug(1, "[%d] id=%d, change param force I=%d",
1028                                 ctx->id,
1029                                 mtk_buf->vb.vb2_buf.index,
1030                                 mtk_buf->enc_params.force_intra);
1031                 if (mtk_buf->enc_params.force_intra)
1032                         ret |= venc_if_set_param(ctx,
1033                                                  VENC_SET_PARAM_FORCE_INTRA,
1034                                                  NULL);
1035         }
1036
1037         mtk_buf->param_change = MTK_ENCODE_PARAM_NONE;
1038
1039         if (ret) {
1040                 ctx->state = MTK_STATE_ABORT;
1041                 mtk_v4l2_err("venc_if_set_param %d failed=%d",
1042                                 mtk_buf->param_change, ret);
1043                 return -1;
1044         }
1045
1046         return 0;
1047 }
1048
1049 /*
1050  * v4l2_m2m_streamoff() holds dev_mutex and waits mtk_venc_worker()
1051  * to call v4l2_m2m_job_finish().
1052  * If mtk_venc_worker() tries to acquire dev_mutex, it will deadlock.
1053  * So this function must not try to acquire dev->dev_mutex.
1054  * This means v4l2 ioctls and mtk_venc_worker() can run at the same time.
1055  * mtk_venc_worker() should be carefully implemented to avoid bugs.
1056  */
1057 static void mtk_venc_worker(struct work_struct *work)
1058 {
1059         struct mtk_vcodec_ctx *ctx = container_of(work, struct mtk_vcodec_ctx,
1060                                     encode_work);
1061         struct vb2_v4l2_buffer *src_buf, *dst_buf;
1062         struct venc_frm_buf frm_buf;
1063         struct mtk_vcodec_mem bs_buf;
1064         struct venc_done_result enc_result;
1065         int ret, i;
1066
1067         /* check dst_buf, dst_buf may be removed in device_run
1068          * to stored encdoe header so we need check dst_buf and
1069          * call job_finish here to prevent recursion
1070          */
1071         dst_buf = v4l2_m2m_dst_buf_remove(ctx->m2m_ctx);
1072         if (!dst_buf) {
1073                 v4l2_m2m_job_finish(ctx->dev->m2m_dev_enc, ctx->m2m_ctx);
1074                 return;
1075         }
1076
1077         src_buf = v4l2_m2m_src_buf_remove(ctx->m2m_ctx);
1078         memset(&frm_buf, 0, sizeof(frm_buf));
1079         for (i = 0; i < src_buf->vb2_buf.num_planes ; i++) {
1080                 frm_buf.fb_addr[i].dma_addr =
1081                                 vb2_dma_contig_plane_dma_addr(&src_buf->vb2_buf, i);
1082                 frm_buf.fb_addr[i].size =
1083                                 (size_t)src_buf->vb2_buf.planes[i].length;
1084         }
1085         bs_buf.va = vb2_plane_vaddr(&dst_buf->vb2_buf, 0);
1086         bs_buf.dma_addr = vb2_dma_contig_plane_dma_addr(&dst_buf->vb2_buf, 0);
1087         bs_buf.size = (size_t)dst_buf->vb2_buf.planes[0].length;
1088
1089         mtk_v4l2_debug(2,
1090                         "Framebuf PA=%llx Size=0x%zx;PA=0x%llx Size=0x%zx;PA=0x%llx Size=%zu",
1091                         (u64)frm_buf.fb_addr[0].dma_addr,
1092                         frm_buf.fb_addr[0].size,
1093                         (u64)frm_buf.fb_addr[1].dma_addr,
1094                         frm_buf.fb_addr[1].size,
1095                         (u64)frm_buf.fb_addr[2].dma_addr,
1096                         frm_buf.fb_addr[2].size);
1097
1098         ret = venc_if_encode(ctx, VENC_START_OPT_ENCODE_FRAME,
1099                              &frm_buf, &bs_buf, &enc_result);
1100
1101         dst_buf->vb2_buf.timestamp = src_buf->vb2_buf.timestamp;
1102         dst_buf->timecode = src_buf->timecode;
1103
1104         if (enc_result.is_key_frm)
1105                 dst_buf->flags |= V4L2_BUF_FLAG_KEYFRAME;
1106
1107         if (ret) {
1108                 v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_ERROR);
1109                 dst_buf->vb2_buf.planes[0].bytesused = 0;
1110                 v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_ERROR);
1111                 mtk_v4l2_err("venc_if_encode failed=%d", ret);
1112         } else {
1113                 v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
1114                 dst_buf->vb2_buf.planes[0].bytesused = enc_result.bs_size;
1115                 v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
1116                 mtk_v4l2_debug(2, "venc_if_encode bs size=%d",
1117                                  enc_result.bs_size);
1118         }
1119
1120         v4l2_m2m_job_finish(ctx->dev->m2m_dev_enc, ctx->m2m_ctx);
1121
1122         mtk_v4l2_debug(1, "<=== src_buf[%d] dst_buf[%d] venc_if_encode ret=%d Size=%u===>",
1123                         src_buf->vb2_buf.index, dst_buf->vb2_buf.index, ret,
1124                         enc_result.bs_size);
1125 }
1126
1127 static void m2mops_venc_device_run(void *priv)
1128 {
1129         struct mtk_vcodec_ctx *ctx = priv;
1130
1131         if ((ctx->q_data[MTK_Q_DATA_DST].fmt->fourcc == V4L2_PIX_FMT_H264) &&
1132             (ctx->state != MTK_STATE_HEADER)) {
1133                 /* encode h264 sps/pps header */
1134                 mtk_venc_encode_header(ctx);
1135                 queue_work(ctx->dev->encode_workqueue, &ctx->encode_work);
1136                 return;
1137         }
1138
1139         mtk_venc_param_change(ctx);
1140         queue_work(ctx->dev->encode_workqueue, &ctx->encode_work);
1141 }
1142
1143 static int m2mops_venc_job_ready(void *m2m_priv)
1144 {
1145         struct mtk_vcodec_ctx *ctx = m2m_priv;
1146
1147         if (ctx->state == MTK_STATE_ABORT || ctx->state == MTK_STATE_FREE) {
1148                 mtk_v4l2_debug(3, "[%d]Not ready: state=0x%x.",
1149                                ctx->id, ctx->state);
1150                 return 0;
1151         }
1152
1153         return 1;
1154 }
1155
1156 static void m2mops_venc_job_abort(void *priv)
1157 {
1158         struct mtk_vcodec_ctx *ctx = priv;
1159
1160         ctx->state = MTK_STATE_ABORT;
1161 }
1162
1163 const struct v4l2_m2m_ops mtk_venc_m2m_ops = {
1164         .device_run     = m2mops_venc_device_run,
1165         .job_ready      = m2mops_venc_job_ready,
1166         .job_abort      = m2mops_venc_job_abort,
1167 };
1168
1169 void mtk_vcodec_enc_set_default_params(struct mtk_vcodec_ctx *ctx)
1170 {
1171         struct mtk_q_data *q_data;
1172
1173         ctx->m2m_ctx->q_lock = &ctx->dev->dev_mutex;
1174         ctx->fh.m2m_ctx = ctx->m2m_ctx;
1175         ctx->fh.ctrl_handler = &ctx->ctrl_hdl;
1176         INIT_WORK(&ctx->encode_work, mtk_venc_worker);
1177
1178         ctx->colorspace = V4L2_COLORSPACE_REC709;
1179         ctx->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
1180         ctx->quantization = V4L2_QUANTIZATION_DEFAULT;
1181         ctx->xfer_func = V4L2_XFER_FUNC_DEFAULT;
1182
1183         q_data = &ctx->q_data[MTK_Q_DATA_SRC];
1184         memset(q_data, 0, sizeof(struct mtk_q_data));
1185         q_data->visible_width = DFT_CFG_WIDTH;
1186         q_data->visible_height = DFT_CFG_HEIGHT;
1187         q_data->coded_width = DFT_CFG_WIDTH;
1188         q_data->coded_height = DFT_CFG_HEIGHT;
1189         q_data->field = V4L2_FIELD_NONE;
1190
1191         q_data->fmt = &mtk_video_formats[OUT_FMT_IDX];
1192
1193         v4l_bound_align_image(&q_data->coded_width,
1194                                 MTK_VENC_MIN_W,
1195                                 MTK_VENC_MAX_W, 4,
1196                                 &q_data->coded_height,
1197                                 MTK_VENC_MIN_H,
1198                                 MTK_VENC_MAX_H, 5, 6);
1199
1200         if (q_data->coded_width < DFT_CFG_WIDTH &&
1201                 (q_data->coded_width + 16) <= MTK_VENC_MAX_W)
1202                 q_data->coded_width += 16;
1203         if (q_data->coded_height < DFT_CFG_HEIGHT &&
1204                 (q_data->coded_height + 32) <= MTK_VENC_MAX_H)
1205                 q_data->coded_height += 32;
1206
1207         q_data->sizeimage[0] =
1208                 q_data->coded_width * q_data->coded_height+
1209                 ((ALIGN(q_data->coded_width, 16) * 2) * 16);
1210         q_data->bytesperline[0] = q_data->coded_width;
1211         q_data->sizeimage[1] =
1212                 (q_data->coded_width * q_data->coded_height) / 2 +
1213                 (ALIGN(q_data->coded_width, 16) * 16);
1214         q_data->bytesperline[1] = q_data->coded_width;
1215
1216         q_data = &ctx->q_data[MTK_Q_DATA_DST];
1217         memset(q_data, 0, sizeof(struct mtk_q_data));
1218         q_data->coded_width = DFT_CFG_WIDTH;
1219         q_data->coded_height = DFT_CFG_HEIGHT;
1220         q_data->fmt = &mtk_video_formats[CAP_FMT_IDX];
1221         q_data->field = V4L2_FIELD_NONE;
1222         ctx->q_data[MTK_Q_DATA_DST].sizeimage[0] =
1223                 DFT_CFG_WIDTH * DFT_CFG_HEIGHT;
1224         ctx->q_data[MTK_Q_DATA_DST].bytesperline[0] = 0;
1225
1226 }
1227
1228 int mtk_vcodec_enc_ctrls_setup(struct mtk_vcodec_ctx *ctx)
1229 {
1230         const struct v4l2_ctrl_ops *ops = &mtk_vcodec_enc_ctrl_ops;
1231         struct v4l2_ctrl_handler *handler = &ctx->ctrl_hdl;
1232
1233         v4l2_ctrl_handler_init(handler, MTK_MAX_CTRLS_HINT);
1234
1235         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_BITRATE,
1236                         1, 4000000, 1, 4000000);
1237         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_B_FRAMES,
1238                         0, 2, 1, 0);
1239         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE,
1240                         0, 1, 1, 1);
1241         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_H264_MAX_QP,
1242                         0, 51, 1, 51);
1243         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_H264_I_PERIOD,
1244                         0, 65535, 1, 0);
1245         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_GOP_SIZE,
1246                         0, 65535, 1, 0);
1247         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_MB_RC_ENABLE,
1248                         0, 1, 1, 0);
1249         v4l2_ctrl_new_std(handler, ops, V4L2_CID_MPEG_VIDEO_FORCE_KEY_FRAME,
1250                         0, 0, 0, 0);
1251         v4l2_ctrl_new_std_menu(handler, ops,
1252                         V4L2_CID_MPEG_VIDEO_HEADER_MODE,
1253                         V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
1254                         0, V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE);
1255         v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_H264_PROFILE,
1256                         V4L2_MPEG_VIDEO_H264_PROFILE_HIGH,
1257                         0, V4L2_MPEG_VIDEO_H264_PROFILE_HIGH);
1258         v4l2_ctrl_new_std_menu(handler, ops, V4L2_CID_MPEG_VIDEO_H264_LEVEL,
1259                         V4L2_MPEG_VIDEO_H264_LEVEL_4_2,
1260                         0, V4L2_MPEG_VIDEO_H264_LEVEL_4_0);
1261         if (handler->error) {
1262                 mtk_v4l2_err("Init control handler fail %d",
1263                                 handler->error);
1264                 return handler->error;
1265         }
1266
1267         v4l2_ctrl_handler_setup(&ctx->ctrl_hdl);
1268
1269         return 0;
1270 }
1271
1272 int mtk_vcodec_enc_queue_init(void *priv, struct vb2_queue *src_vq,
1273                               struct vb2_queue *dst_vq)
1274 {
1275         struct mtk_vcodec_ctx *ctx = priv;
1276         int ret;
1277
1278         /* Note: VB2_USERPTR works with dma-contig because mt8173
1279          * support iommu
1280          * https://patchwork.kernel.org/patch/8335461/
1281          * https://patchwork.kernel.org/patch/7596181/
1282          */
1283         src_vq->type            = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
1284         src_vq->io_modes        = VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
1285         src_vq->drv_priv        = ctx;
1286         src_vq->buf_struct_size = sizeof(struct mtk_video_enc_buf);
1287         src_vq->ops             = &mtk_venc_vb2_ops;
1288         src_vq->mem_ops         = &vb2_dma_contig_memops;
1289         src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1290         src_vq->lock            = &ctx->dev->dev_mutex;
1291         src_vq->dev             = &ctx->dev->plat_dev->dev;
1292
1293         ret = vb2_queue_init(src_vq);
1294         if (ret)
1295                 return ret;
1296
1297         dst_vq->type            = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
1298         dst_vq->io_modes        = VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
1299         dst_vq->drv_priv        = ctx;
1300         dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
1301         dst_vq->ops             = &mtk_venc_vb2_ops;
1302         dst_vq->mem_ops         = &vb2_dma_contig_memops;
1303         dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
1304         dst_vq->lock            = &ctx->dev->dev_mutex;
1305         dst_vq->dev             = &ctx->dev->plat_dev->dev;
1306
1307         return vb2_queue_init(dst_vq);
1308 }
1309
1310 int mtk_venc_unlock(struct mtk_vcodec_ctx *ctx)
1311 {
1312         struct mtk_vcodec_dev *dev = ctx->dev;
1313
1314         mutex_unlock(&dev->enc_mutex);
1315         return 0;
1316 }
1317
1318 int mtk_venc_lock(struct mtk_vcodec_ctx *ctx)
1319 {
1320         struct mtk_vcodec_dev *dev = ctx->dev;
1321
1322         mutex_lock(&dev->enc_mutex);
1323         return 0;
1324 }
1325
1326 void mtk_vcodec_enc_release(struct mtk_vcodec_ctx *ctx)
1327 {
1328         int ret = venc_if_deinit(ctx);
1329
1330         if (ret)
1331                 mtk_v4l2_err("venc_if_deinit failed=%d", ret);
1332
1333         ctx->state = MTK_STATE_FREE;
1334 }