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