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
mdp_find_fmt(const struct mtk_mdp_driver_data * mdp_data,u32 pixelformat,u32 type)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
mdp_find_fmt_by_index(const struct mtk_mdp_driver_data * mdp_data,u32 index,u32 type)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
mdp_map_ycbcr_prof_mplane(struct v4l2_format * f,u32 mdp_color)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
mdp_bound_align_image(u32 * w,u32 * h,struct v4l2_frmsize_stepwise * s,unsigned int salign)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
mdp_clamp_align(s32 * x,int min,int max,unsigned int align)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
mdp_enum_fmt_mplane(struct mdp_dev * mdp,struct v4l2_fmtdesc * f)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
mdp_try_fmt_mplane(struct mdp_dev * mdp,struct v4l2_format * f,struct mdp_frameparam * param,u32 ctx_id)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
mdp_clamp_start(s32 * x,int min,int max,unsigned int align,u32 flags)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
mdp_clamp_end(s32 * x,int min,int max,unsigned int align,u32 flags)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
mdp_try_crop(struct mdp_m2m_ctx * ctx,struct v4l2_rect * r,const struct v4l2_selection * s,struct mdp_frame * frame)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
mdp_check_scaling_ratio(const struct v4l2_rect * crop,const struct v4l2_rect * compose,s32 rotation,const struct mdp_limit * limit)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
mdp_check_pp_enable(struct mdp_dev * mdp,struct mdp_frame * frame)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 */
mdp_fmt_get_stride(const struct mdp_format * fmt,u32 bytesperline,unsigned int plane)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 */
mdp_fmt_get_stride_contig(const struct mdp_format * fmt,u32 pix_stride,unsigned int plane)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 */
mdp_fmt_get_plane_size(const struct mdp_format * fmt,u32 stride,u32 height,unsigned int plane)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
mdp_prepare_buffer(struct img_image_buffer * b,struct mdp_frame * frame,struct vb2_buffer * vb)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
mdp_set_src_config(struct img_input * in,struct mdp_frame * frame,struct vb2_buffer * vb)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
mdp_to_fixed(u32 * r,struct v4l2_fract * f)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
mdp_set_src_crop(struct img_crop * c,struct mdp_crop * crop)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
mdp_set_orientation(struct img_output * out,s32 rotation,bool hflip,bool vflip)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
mdp_set_dst_config(struct img_output * out,struct mdp_frame * frame,struct vb2_buffer * vb)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
mdp_frameparam_init(struct mdp_dev * mdp,struct mdp_frameparam * param)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