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 }
497