515 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/*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 15static 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 31static 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 48enum 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 75static 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 92static 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 116int 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 128const 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 = ¶m->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) ? ¶m->limit->out_limit :165 ¶m->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 211static 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 221static 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 231int 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 283int 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 307bool mdp_check_pp_enable(struct mdp_dev *mdp, struct mdp_frame *frame)308{309 u32 s, r1, r2;310 311 if (!mdp || !frame)312 return false;313 314 if (!mdp->mdp_data->pp_criteria)315 return false;316 317 s = mdp->mdp_data->pp_criteria->width *318 mdp->mdp_data->pp_criteria->height;319 r1 = frame->crop.c.width * frame->crop.c.height;320 r2 = frame->compose.width * frame->compose.height;321 322 return (r1 >= s || r2 >= s);323}324 325/* Stride that is accepted by MDP HW */326static u32 mdp_fmt_get_stride(const struct mdp_format *fmt,327 u32 bytesperline, unsigned int plane)328{329 enum mdp_color c = fmt->mdp_color;330 u32 stride;331 332 stride = (bytesperline * MDP_COLOR_BITS_PER_PIXEL(c))333 / fmt->row_depth[0];334 if (plane == 0)335 return stride;336 if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {337 if (MDP_COLOR_IS_BLOCK_MODE(c))338 stride = stride / 2;339 return stride;340 }341 return 0;342}343 344/* Stride that is accepted by MDP HW of format with contiguous planes */345static u32 mdp_fmt_get_stride_contig(const struct mdp_format *fmt,346 u32 pix_stride, unsigned int plane)347{348 enum mdp_color c = fmt->mdp_color;349 u32 stride = pix_stride;350 351 if (plane == 0)352 return stride;353 if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {354 stride = stride >> MDP_COLOR_GET_H_SUBSAMPLE(c);355 if (MDP_COLOR_IS_UV_COPLANE(c) && !MDP_COLOR_IS_BLOCK_MODE(c))356 stride = stride * 2;357 return stride;358 }359 return 0;360}361 362/* Plane size that is accepted by MDP HW */363static u32 mdp_fmt_get_plane_size(const struct mdp_format *fmt,364 u32 stride, u32 height, unsigned int plane)365{366 enum mdp_color c = fmt->mdp_color;367 u32 bytesperline;368 369 bytesperline = (stride * fmt->row_depth[0])370 / MDP_COLOR_BITS_PER_PIXEL(c);371 if (plane == 0)372 return bytesperline * height;373 if (plane < MDP_COLOR_GET_PLANE_COUNT(c)) {374 height = height >> MDP_COLOR_GET_V_SUBSAMPLE(c);375 if (MDP_COLOR_IS_BLOCK_MODE(c))376 bytesperline = bytesperline * 2;377 return bytesperline * height;378 }379 return 0;380}381 382static void mdp_prepare_buffer(struct img_image_buffer *b,383 struct mdp_frame *frame, struct vb2_buffer *vb)384{385 struct v4l2_pix_format_mplane *pix_mp = &frame->format.fmt.pix_mp;386 unsigned int i;387 388 b->format.colorformat = frame->mdp_fmt->mdp_color;389 b->format.ycbcr_prof = frame->ycbcr_prof;390 for (i = 0; i < pix_mp->num_planes; ++i) {391 u32 stride = mdp_fmt_get_stride(frame->mdp_fmt,392 pix_mp->plane_fmt[i].bytesperline, i);393 394 b->format.plane_fmt[i].stride = stride;395 b->format.plane_fmt[i].size =396 mdp_fmt_get_plane_size(frame->mdp_fmt, stride,397 pix_mp->height, i);398 b->iova[i] = vb2_dma_contig_plane_dma_addr(vb, i);399 }400 for (; i < MDP_COLOR_GET_PLANE_COUNT(b->format.colorformat); ++i) {401 u32 stride = mdp_fmt_get_stride_contig(frame->mdp_fmt,402 b->format.plane_fmt[0].stride, i);403 404 b->format.plane_fmt[i].stride = stride;405 b->format.plane_fmt[i].size =406 mdp_fmt_get_plane_size(frame->mdp_fmt, stride,407 pix_mp->height, i);408 b->iova[i] = b->iova[i - 1] + b->format.plane_fmt[i - 1].size;409 }410 b->usage = frame->usage;411}412 413void mdp_set_src_config(struct img_input *in,414 struct mdp_frame *frame, struct vb2_buffer *vb)415{416 in->buffer.format.width = frame->format.fmt.pix_mp.width;417 in->buffer.format.height = frame->format.fmt.pix_mp.height;418 mdp_prepare_buffer(&in->buffer, frame, vb);419}420 421static u32 mdp_to_fixed(u32 *r, struct v4l2_fract *f)422{423 u32 q;424 425 if (f->denominator == 0) {426 *r = 0;427 return 0;428 }429 430 q = f->numerator / f->denominator;431 *r = div_u64(((u64)f->numerator - q * f->denominator) <<432 IMG_SUBPIXEL_SHIFT, f->denominator);433 return q;434}435 436static void mdp_set_src_crop(struct img_crop *c, struct mdp_crop *crop)437{438 c->left = crop->c.left439 + mdp_to_fixed(&c->left_subpix, &crop->left_subpix);440 c->top = crop->c.top441 + mdp_to_fixed(&c->top_subpix, &crop->top_subpix);442 c->width = crop->c.width443 + mdp_to_fixed(&c->width_subpix, &crop->width_subpix);444 c->height = crop->c.height445 + mdp_to_fixed(&c->height_subpix, &crop->height_subpix);446}447 448static void mdp_set_orientation(struct img_output *out,449 s32 rotation, bool hflip, bool vflip)450{451 u8 flip = 0;452 453 if (hflip)454 flip ^= 1;455 if (vflip) {456 /*457 * A vertical flip is equivalent to458 * a 180-degree rotation with a horizontal flip459 */460 rotation += 180;461 flip ^= 1;462 }463 464 out->rotation = rotation % 360;465 if (flip != 0)466 out->flags |= IMG_CTRL_FLAG_HFLIP;467 else468 out->flags &= ~IMG_CTRL_FLAG_HFLIP;469}470 471void mdp_set_dst_config(struct img_output *out,472 struct mdp_frame *frame, struct vb2_buffer *vb)473{474 out->buffer.format.width = frame->compose.width;475 out->buffer.format.height = frame->compose.height;476 mdp_prepare_buffer(&out->buffer, frame, vb);477 mdp_set_src_crop(&out->crop, &frame->crop);478 mdp_set_orientation(out, frame->rotation, frame->hflip, frame->vflip);479}480 481int mdp_frameparam_init(struct mdp_dev *mdp, struct mdp_frameparam *param)482{483 struct mdp_frame *frame;484 485 if (!param)486 return -EINVAL;487 488 INIT_LIST_HEAD(¶m->list);489 param->limit = mdp->mdp_data->def_limit;490 param->type = MDP_STREAM_TYPE_BITBLT;491 492 frame = ¶m->output;493 frame->format.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;494 frame->mdp_fmt = mdp_try_fmt_mplane(mdp, &frame->format, param, 0);495 frame->ycbcr_prof =496 mdp_map_ycbcr_prof_mplane(&frame->format,497 frame->mdp_fmt->mdp_color);498 frame->usage = MDP_BUFFER_USAGE_HW_READ;499 500 param->num_captures = 1;501 frame = ¶m->captures[0];502 frame->format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;503 frame->mdp_fmt = mdp_try_fmt_mplane(mdp, &frame->format, param, 0);504 frame->ycbcr_prof =505 mdp_map_ycbcr_prof_mplane(&frame->format,506 frame->mdp_fmt->mdp_color);507 frame->usage = MDP_BUFFER_USAGE_MDP;508 frame->crop.c.width = param->output.format.fmt.pix_mp.width;509 frame->crop.c.height = param->output.format.fmt.pix_mp.height;510 frame->compose.width = frame->format.fmt.pix_mp.width;511 frame->compose.height = frame->format.fmt.pix_mp.height;512 513 return 0;514}515