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 = ¶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 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 bool 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 */ 326 static 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 */ 345 static 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 */ 363 static 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 382 static 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 413 void 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 421 static 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 436 static void mdp_set_src_crop(struct img_crop *c, struct mdp_crop *crop) 437 { 438 c->left = crop->c.left 439 + mdp_to_fixed(&c->left_subpix, &crop->left_subpix); 440 c->top = crop->c.top 441 + mdp_to_fixed(&c->top_subpix, &crop->top_subpix); 442 c->width = crop->c.width 443 + mdp_to_fixed(&c->width_subpix, &crop->width_subpix); 444 c->height = crop->c.height 445 + mdp_to_fixed(&c->height_subpix, &crop->height_subpix); 446 } 447 448 static 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 to 458 * a 180-degree rotation with a horizontal flip 459 */ 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 else 468 out->flags &= ~IMG_CTRL_FLAG_HFLIP; 469 } 470 471 void 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 481 int 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