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
 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 = &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
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.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
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 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
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(&param->list);
489	param->limit = mdp->mdp_data->def_limit;
490	param->type = MDP_STREAM_TYPE_BITBLT;
491
492	frame = &param->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 = &param->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}