drivers/media/platform/mediatek/mdp3/mtk-mdp3-regs.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (c) 2022 MediaTek Inc.
   4  * Author: Ping-Hsun Wu <ping-hsun.wu@mediatek.com>
   5  */
   6
   7 #include <linux/math64.h>
   8 #include <media/v4l2-common.h>
   9 #include <media/videobuf2-v4l2.h>
  10 #include <media/videobuf2-dma-contig.h>
  11 #include "mtk-mdp3-core.h"
  12 #include "mtk-mdp3-regs.h"
  13 #include "mtk-mdp3-m2m.h"
  14
  15 static const struct mdp_format *mdp_find_fmt(const struct mtk_mdp_driver_data *mdp_data,
  16                                              u32 pixelformat, u32 type)
  17 {
  18         u32 i, flag;
  19
  20         flag = V4L2_TYPE_IS_OUTPUT(type) ? MDP_FMT_FLAG_OUTPUT :
  21                                         MDP_FMT_FLAG_CAPTURE;
  22         for (i = 0; i < mdp_data->format_len; ++i) {
  23                 if (!(mdp_data->format[i].flags & flag))
  24                         continue;
  25                 if (mdp_data->format[i].pixelformat == pixelformat)
  26                         return &mdp_data->format[i];
  27         }
  28         return NULL;
  29 }
  30
  31 static const struct mdp_format *mdp_find_fmt_by_index(const struct mtk_mdp_driver_data *mdp_data,
  32                                                       u32 index, u32 type)
  33 {
  34         u32 i, flag, num = 0;
  35
  36         flag = V4L2_TYPE_IS_OUTPUT(type) ? MDP_FMT_FLAG_OUTPUT :
  37                                         MDP_FMT_FLAG_CAPTURE;
  38         for (i = 0; i < mdp_data->format_len; ++i) {
  39                 if (!(mdp_data->format[i].flags & flag))
  40                         continue;
  41                 if (index == num)
  42                         return &mdp_data->format[i];
  43                 num++;
  44         }
  45         return NULL;
  46 }
  47
  48 enum mdp_ycbcr_profile mdp_map_ycbcr_prof_mplane(struct v4l2_format *f,
  49                                                  u32 mdp_color)
  50 {
  51         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
  52
  53         if (MDP_COLOR_IS_RGB(mdp_color))
  54                 return MDP_YCBCR_PROFILE_FULL_BT601;
  55
  56         switch (pix_mp->colorspace) {
  57         case V4L2_COLORSPACE_JPEG:
  58                 return MDP_YCBCR_PROFILE_JPEG;
  59         case V4L2_COLORSPACE_REC709:
  60         case V4L2_COLORSPACE_DCI_P3:
  61                 if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
  62                         return MDP_YCBCR_PROFILE_FULL_BT709;
  63                 return MDP_YCBCR_PROFILE_BT709;
  64         case V4L2_COLORSPACE_BT2020:
  65                 if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
  66                         return MDP_YCBCR_PROFILE_FULL_BT2020;
  67                 return MDP_YCBCR_PROFILE_BT2020;
  68         default:
  69                 if (pix_mp->quantization == V4L2_QUANTIZATION_FULL_RANGE)
  70                         return MDP_YCBCR_PROFILE_FULL_BT601;
  71                 return MDP_YCBCR_PROFILE_BT601;
  72         }
  73 }
  74
  75 static void mdp_bound_align_image(u32 *w, u32 *h,
  76                                   struct v4l2_frmsize_stepwise *s,
  77                                   unsigned int salign)
  78 {
  79         unsigned int org_w, org_h;
  80
  81         org_w = *w;
  82         org_h = *h;
  83         v4l_bound_align_image(w, s->min_width, s->max_width, s->step_width,
  84                               h, s->min_height, s->max_height, s->step_height,
  85                               salign);
  86
  87         s->min_width = org_w;
  88         s->min_height = org_h;
  89         v4l2_apply_frmsize_constraints(w, h, s);
  90 }
  91
  92 static int mdp_clamp_align(s32 *x, int min, int max, unsigned int align)
  93 {
  94         unsigned int mask;
  95
  96         if (min < 0 || max < 0)
  97                 return -ERANGE;
  98
  99         /* Bits that must be zero to be aligned */
 100         mask = ~((1 << align) - 1);
 101
 102         min = 0 ? 0 : ((min + ~mask) & mask);
 103         max = max & mask;
 104         if ((unsigned int)min > (unsigned int)max)
 105                 return -ERANGE;
 106
 107         /* Clamp to aligned min and max */
 108         *x = clamp(*x, min, max);
 109
 110         /* Round to nearest aligned value */
 111         if (align)
 112                 *x = (*x + (1 << (align - 1))) & mask;
 113         return 0;
 114 }
 115
 116 int mdp_enum_fmt_mplane(struct mdp_dev *mdp, struct v4l2_fmtdesc *f)
 117 {
 118         const struct mdp_format *fmt;
 119
 120         fmt = mdp_find_fmt_by_index(mdp->mdp_data, f->index, f->type);
 121         if (!fmt)
 122                 return -EINVAL;
 123
 124         f->pixelformat = fmt->pixelformat;
 125         return 0;
 126 }
 127
 128 const struct mdp_format *mdp_try_fmt_mplane(struct mdp_dev *mdp,
 129                                             struct v4l2_format *f,
 130                                             struct mdp_frameparam *param,
 131                                             u32 ctx_id)
 132 {
 133         struct device *dev = &param->ctx->mdp_dev->pdev->dev;
 134         struct v4l2_pix_format_mplane *pix_mp = &f->fmt.pix_mp;
 135         const struct mdp_format *fmt;
 136         const struct mdp_pix_limit *pix_limit;
 137         struct v4l2_frmsize_stepwise s;
 138         u32 org_w, org_h;
 139         unsigned int i;
 140
 141         fmt = mdp_find_fmt(mdp->mdp_data, pix_mp->pixelformat, f->type);
 142         if (!fmt) {
 143                 fmt = mdp_find_fmt_by_index(mdp->mdp_data, 0, f->type);
 144                 if (!fmt) {
 145                         dev_dbg(dev, "%d: pixelformat %c%c%c%c invalid", ctx_id,
 146                                 (pix_mp->pixelformat & 0xff),
 147                                 (pix_mp->pixelformat >>  8) & 0xff,
 148                                 (pix_mp->pixelformat >> 16) & 0xff,
 149                                 (pix_mp->pixelformat >> 24) & 0xff);
 150                         return NULL;
 151                 }
 152         }
 153
 154         pix_mp->field = V4L2_FIELD_NONE;
 155         pix_mp->flags = 0;
 156         pix_mp->pixelformat = fmt->pixelformat;
 157         if (V4L2_TYPE_IS_CAPTURE(f->type)) {
 158                 pix_mp->colorspace = param->colorspace;
 159                 pix_mp->xfer_func = param->xfer_func;
 160                 pix_mp->ycbcr_enc = param->ycbcr_enc;
 161                 pix_mp->quantization = param->quant;
 162         }
 163
 164         pix_limit = V4L2_TYPE_IS_OUTPUT(f->type) ? &param->limit->out_limit :
 165                                                 &param->limit->cap_limit;
 166         s.min_width = pix_limit->wmin;
 167         s.max_width = pix_limit->wmax;
 168         s.step_width = fmt->walign;
 169         s.min_height = pix_limit->hmin;
 170         s.max_height = pix_limit->hmax;
 171         s.step_height = fmt->halign;
 172         org_w = pix_mp->width;
 173         org_h = pix_mp->height;
 174
 175         mdp_bound_align_image(&pix_mp->width, &pix_mp->height, &s, fmt->salign);
 176         if (org_w != pix_mp->width || org_h != pix_mp->height)
 177                 dev_dbg(dev, "%d: size change: %ux%u to %ux%u", ctx_id,
 178                         org_w, org_h, pix_mp->width, pix_mp->height);
 179
 180         if (pix_mp->num_planes && pix_mp->num_planes != fmt->num_planes)
 181                 dev_dbg(dev, "%d num of planes change: %u to %u", ctx_id,
 182                         pix_mp->num_planes, fmt->num_planes);
 183         pix_mp->num_planes = fmt->num_planes;
 184
 185         for (i = 0; i < pix_mp->num_planes; ++i) {
 186                 u32 min_bpl = (pix_mp->width * fmt->row_depth[i]) >> 3;
 187                 u32 max_bpl = (pix_limit->wmax * fmt->row_depth[i]) >> 3;
 188                 u32 bpl = pix_mp->plane_fmt[i].bytesperline;
 189                 u32 min_si, max_si;
 190                 u32 si = pix_mp->plane_fmt[i].sizeimage;
 191                 u64 di;
 192
 193                 bpl = clamp(bpl, min_bpl, max_bpl);
 194                 pix_mp->plane_fmt[i].bytesperline = bpl;
 195
 196                 di = (u64)bpl * pix_mp->height * fmt->depth[i];
 197                 min_si = (u32)div_u64(di, fmt->row_depth[i]);
 198                 di = (u64)bpl * s.max_height * fmt->depth[i];
 199                 max_si = (u32)div_u64(di, fmt->row_depth[i]);
 200
 201                 si = clamp(si, min_si, max_si);
 202                 pix_mp->plane_fmt[i].sizeimage = si;
 203
 204                 dev_dbg(dev, "%d: p%u, bpl:%u [%u, %u], sizeimage:%u [%u, %u]",
 205                         ctx_id, i, bpl, min_bpl, max_bpl, si, min_si, max_si);
 206         }
 207
 208         return fmt;
 209 }
 210
 211 static int mdp_clamp_start(s32 *x, int min, int max, unsigned int align,
 212                            u32 flags)
 213 {
 214         if (flags & V4L2_SEL_FLAG_GE)
 215                 max = *x;
 216         if (flags & V4L2_SEL_FLAG_LE)
 217                 min = *x;
 218         return mdp_clamp_align(x, min, max, align);
 219 }
 220
 221 static int mdp_clamp_end(s32 *x, int min, int max, unsigned int align,
 222                          u32 flags)
 223 {
 224         if (flags & V4L2_SEL_FLAG_GE)
 225                 min = *x;
 226         if (flags & V4L2_SEL_FLAG_LE)
 227                 max = *x;
 228         return mdp_clamp_align(x, min, max, align);
 229 }
 230
 231 int mdp_try_crop(struct mdp_m2m_ctx *ctx, struct v4l2_rect *r,
 232                  const struct v4l2_selection *s, struct mdp_frame *frame)
 233 {
 234         struct device *dev = &ctx->mdp_dev->pdev->dev;
 235         s32 left, top, right, bottom;
 236         u32 framew, frameh, walign, halign;
 237         int ret;
 238
 239         dev_dbg(dev, "%d target:%d, set:(%d,%d) %ux%u", ctx->id,
 240                 s->target, s->r.left, s->r.top, s->r.width, s->r.height);
 241
 242         left = s->r.left;
 243         top = s->r.top;
 244         right = s->r.left + s->r.width;
 245         bottom = s->r.top + s->r.height;
 246         framew = frame->format.fmt.pix_mp.width;
 247         frameh = frame->format.fmt.pix_mp.height;
 248
 249         if (mdp_target_is_crop(s->target)) {
 250                 walign = 1;
 251                 halign = 1;
 252         } else {
 253                 walign = frame->mdp_fmt->walign;
 254                 halign = frame->mdp_fmt->halign;
 255         }
 256
 257         dev_dbg(dev, "%d align:%u,%u, bound:%ux%u", ctx->id,
 258                 walign, halign, framew, frameh);
 259
 260         ret = mdp_clamp_start(&left, 0, right, walign, s->flags);
 261         if (ret)
 262                 return ret;
 263         ret = mdp_clamp_start(&top, 0, bottom, halign, s->flags);
 264         if (ret)
 265                 return ret;
 266         ret = mdp_clamp_end(&right, left, framew, walign, s->flags);
 267         if (ret)
 268                 return ret;
 269         ret = mdp_clamp_end(&bottom, top, frameh, halign, s->flags);
 270         if (ret)
 271                 return ret;
 272
 273         r->left = left;
 274         r->top = top;
 275         r->width = right - left;
 276         r->height = bottom - top;
 277
 278         dev_dbg(dev, "%d crop:(%d,%d) %ux%u", ctx->id,
 279                 r->left, r->top, r->width, r->height);
 280         return 0;
 281 }
 282
 283 int mdp_check_scaling_ratio(const struct v4l2_rect *crop,
 284                             const struct v4l2_rect *compose, s32 rotation,
 285         const struct mdp_limit *limit)
 286 {
 287         u32 crop_w, crop_h, comp_w, comp_h;
 288
 289         crop_w = crop->width;
 290         crop_h = crop->height;
 291         if (90 == rotation || 270 == rotation) {
 292                 comp_w = compose->height;
 293                 comp_h = compose->width;
 294         } else {
 295                 comp_w = compose->width;
 296                 comp_h = compose->height;
 297         }
 298
 299         if ((crop_w / comp_w) > limit->h_scale_down_max ||
 300             (crop_h / comp_h) > limit->v_scale_down_max ||
 301             (comp_w / crop_w) > limit->h_scale_up_max ||
 302             (comp_h / crop_h) > limit->v_scale_up_max)
 303                 return -ERANGE;
 304         return 0;
 305 }
 306
 307 /* Stride that is accepted by MDP HW */
 308 static u32 mdp_fmt_get_stride(const struct mdp_format *fmt,
 309                               u32 bytesperline, unsigned int plane)
 310 {
 311         enum mdp_color c = fmt->mdp_color;
 312         u32 stride;
 313
 314         stride = (bytesperline * MDP_COLOR_BITS_PER_PIXEL(c))
 315                 / fmt->row_depth[0];
 316         if (plane == 0)
 317                 return stride;
 318         if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
 319                 if (MDP_COLOR_IS_BLOCK_MODE(c))
 320                         stride = stride / 2;
 321                 return stride;
 322         }
 323         return 0;
 324 }
 325
 326 /* Stride that is accepted by MDP HW of format with contiguous planes */
 327 static u32 mdp_fmt_get_stride_contig(const struct mdp_format *fmt,
 328                                      u32 pix_stride, unsigned int plane)
 329 {
 330         enum mdp_color c = fmt->mdp_color;
 331         u32 stride = pix_stride;
 332
 333         if (plane == 0)
 334                 return stride;
 335         if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
 336                 stride = stride >> MDP_COLOR_GET_H_SUBSAMPLE(c);
 337                 if (MDP_COLOR_IS_UV_COPLANE(c) && !MDP_COLOR_IS_BLOCK_MODE(c))
 338                         stride = stride * 2;
 339                 return stride;
 340         }
 341         return 0;
 342 }
 343
 344 /* Plane size that is accepted by MDP HW */
 345 static u32 mdp_fmt_get_plane_size(const struct mdp_format *fmt,
 346                                   u32 stride, u32 height, unsigned int plane)
 347 {
 348         enum mdp_color c = fmt->mdp_color;
 349         u32 bytesperline;
 350
 351         bytesperline = (stride * fmt->row_depth[0])
 352                 / MDP_COLOR_BITS_PER_PIXEL(c);
 353         if (plane == 0)
 354                 return bytesperline * height;
 355         if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {
 356                 height = height >> MDP_COLOR_GET_V_SUBSAMPLE(c);
 357                 if (MDP_COLOR_IS_BLOCK_MODE(c))
 358                         bytesperline = bytesperline * 2;
 359                 return bytesperline * height;
 360         }
 361         return 0;
 362 }
 363
 364 static void mdp_prepare_buffer(struct img_image_buffer *b,
 365                                struct mdp_frame *frame, struct vb2_buffer *vb)
 366 {
 367         struct v4l2_pix_format_mplane *pix_mp = &frame->format.fmt.pix_mp;
 368         unsigned int i;
 369
 370         b->format.colorformat = frame->mdp_fmt->mdp_color;
 371         b->format.ycbcr_prof = frame->ycbcr_prof;
 372         for (i = 0; i < pix_mp->num_planes; ++i) {
 373                 u32 stride = mdp_fmt_get_stride(frame->mdp_fmt,
 374                         pix_mp->plane_fmt[i].bytesperline, i);
 375
 376                 b->format.plane_fmt[i].stride = stride;
 377                 b->format.plane_fmt[i].size =
 378                         mdp_fmt_get_plane_size(frame->mdp_fmt, stride,
 379                                                pix_mp->height, i);
 380                 b->iova[i] = vb2_dma_contig_plane_dma_addr(vb, i);
 381         }
 382         for (; i < MDP_COLOR_GET_PLANE_COUNT(b->format.colorformat); ++i) {
 383                 u32 stride = mdp_fmt_get_stride_contig(frame->mdp_fmt,
 384                         b->format.plane_fmt[0].stride, i);
 385
 386                 b->format.plane_fmt[i].stride = stride;
 387                 b->format.plane_fmt[i].size =
 388                         mdp_fmt_get_plane_size(frame->mdp_fmt, stride,
 389                                                pix_mp->height, i);
 390                 b->iova[i] = b->iova[i - 1] + b->format.plane_fmt[i - 1].size;
 391         }
 392         b->usage = frame->usage;
 393 }
 394
 395 void mdp_set_src_config(struct img_input *in,
 396                         struct mdp_frame *frame, struct vb2_buffer *vb)
 397 {
 398         in->buffer.format.width = frame->format.fmt.pix_mp.width;
 399         in->buffer.format.height = frame->format.fmt.pix_mp.height;
 400         mdp_prepare_buffer(&in->buffer, frame, vb);
 401 }
 402
 403 static u32 mdp_to_fixed(u32 *r, struct v4l2_fract *f)
 404 {
 405         u32 q;
 406
 407         if (f->denominator == 0) {
 408                 *r = 0;
 409                 return 0;
 410         }
 411
 412         q = f->numerator / f->denominator;
 413         *r = div_u64(((u64)f->numerator - q * f->denominator) <<
 414                      IMG_SUBPIXEL_SHIFT, f->denominator);
 415         return q;
 416 }
 417
 418 static void mdp_set_src_crop(struct img_crop *c, struct mdp_crop *crop)
 419 {
 420         c->left = crop->c.left
 421                 + mdp_to_fixed(&c->left_subpix, &crop->left_subpix);
 422         c->top = crop->c.top
 423                 + mdp_to_fixed(&c->top_subpix, &crop->top_subpix);
 424         c->width = crop->c.width
 425                 + mdp_to_fixed(&c->width_subpix, &crop->width_subpix);
 426         c->height = crop->c.height
 427                 + mdp_to_fixed(&c->height_subpix, &crop->height_subpix);
 428 }
 429
 430 static void mdp_set_orientation(struct img_output *out,
 431                                 s32 rotation, bool hflip, bool vflip)
 432 {
 433         u8 flip = 0;
 434
 435         if (hflip)
 436                 flip ^= 1;
 437         if (vflip) {
 438                 /*
 439                  * A vertical flip is equivalent to
 440                  * a 180-degree rotation with a horizontal flip
 441                  */
 442                 rotation += 180;
 443                 flip ^= 1;
 444         }
 445
 446         out->rotation = rotation % 360;
 447         if (flip != 0)
 448                 out->flags |= IMG_CTRL_FLAG_HFLIP;
 449         else
 450                 out->flags &= ~IMG_CTRL_FLAG_HFLIP;
 451 }
 452
 453 void mdp_set_dst_config(struct img_output *out,
 454                         struct mdp_frame *frame, struct vb2_buffer *vb)
 455 {
 456         out->buffer.format.width = frame->compose.width;
 457         out->buffer.format.height = frame->compose.height;
 458         mdp_prepare_buffer(&out->buffer, frame, vb);
 459         mdp_set_src_crop(&out->crop, &frame->crop);
 460         mdp_set_orientation(out, frame->rotation, frame->hflip, frame->vflip);
 461 }
 462
 463 int mdp_frameparam_init(struct mdp_dev *mdp, struct mdp_frameparam *param)
 464 {
 465         struct mdp_frame *frame;
 466
 467         if (!param)
 468                 return -EINVAL;
 469
 470         INIT_LIST_HEAD(&param->list);
 471         param->limit = mdp->mdp_data->def_limit;
 472         param->type = MDP_STREAM_TYPE_BITBLT;
 473
 474         frame = &param->output;
 475         frame->format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 476         frame->mdp_fmt = mdp_try_fmt_mplane(mdp, &frame->format, param, 0);
 477         frame->ycbcr_prof =
 478                 mdp_map_ycbcr_prof_mplane(&frame->format,
 479                                           frame->mdp_fmt->mdp_color);
 480         frame->usage = MDP_BUFFER_USAGE_HW_READ;
 481
 482         param->num_captures = 1;
 483         frame = &param->captures[0];
 484         frame->format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
 485         frame->mdp_fmt = mdp_try_fmt_mplane(mdp, &frame->format, param, 0);
 486         frame->ycbcr_prof =
 487                 mdp_map_ycbcr_prof_mplane(&frame->format,
 488                                           frame->mdp_fmt->mdp_color);
 489         frame->usage = MDP_BUFFER_USAGE_MDP;
 490         frame->crop.c.width = param->output.format.fmt.pix_mp.width;
 491         frame->crop.c.height = param->output.format.fmt.pix_mp.height;
 492         frame->compose.width = frame->format.fmt.pix_mp.width;
 493         frame->compose.height = frame->format.fmt.pix_mp.height;
 494
 495         return 0;
 496 }