c3-isp-resizer.c

  1// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
  2/*
  3 * Copyright (C) 2024 Amlogic, Inc. All rights reserved
  4 */
  5
  6#include <linux/pm_runtime.h>
  7
  8#include "c3-isp-common.h"
  9#include "c3-isp-regs.h"
 10
 11#define C3_ISP_RSZ_DEF_PAD_FMT		MEDIA_BUS_FMT_YUV10_1X30
 12#define C3_ISP_DISP_REG(base, id)	((base) + (id) * 0x400)
 13#define C3_ISP_PPS_LUT_H_NUM		33
 14#define C3_ISP_PPS_LUT_CTYPE_0		0
 15#define C3_ISP_PPS_LUT_CTYPE_2		2
 16#define C3_ISP_SCL_EN			1
 17#define C3_ISP_SCL_DIS			0
 18
 19/*
 20 * struct c3_isp_rsz_format_info - ISP resizer format information
 21 *
 22 * @mbus_code: the mbus code
 23 * @pads: bitmask detailing valid pads for this mbus_code
 24 * @is_raw: the raw format flag of mbus code
 25 */
 26struct c3_isp_rsz_format_info {
 27	u32 mbus_code;
 28	u32 pads;
 29	bool is_raw;
 30};
 31
 32static const struct c3_isp_rsz_format_info c3_isp_rsz_fmts[] = {
 33	/* RAW formats */
 34	{
 35		.mbus_code	= MEDIA_BUS_FMT_SBGGR16_1X16,
 36		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
 37				| BIT(C3_ISP_RSZ_PAD_SOURCE),
 38		.is_raw = true,
 39	}, {
 40		.mbus_code	= MEDIA_BUS_FMT_SGBRG16_1X16,
 41		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
 42				| BIT(C3_ISP_RSZ_PAD_SOURCE),
 43		.is_raw = true,
 44	}, {
 45		.mbus_code	= MEDIA_BUS_FMT_SGRBG16_1X16,
 46		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
 47				| BIT(C3_ISP_RSZ_PAD_SOURCE),
 48		.is_raw = true,
 49	}, {
 50		.mbus_code	= MEDIA_BUS_FMT_SRGGB16_1X16,
 51		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
 52				| BIT(C3_ISP_RSZ_PAD_SOURCE),
 53		.is_raw = true,
 54	},
 55	/* YUV formats */
 56	{
 57		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
 58		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
 59				| BIT(C3_ISP_RSZ_PAD_SOURCE),
 60		.is_raw = false,
 61	},
 62};
 63
 64/*
 65 * struct c3_isp_pps_io_size - ISP scaler input and output size
 66 *
 67 * @thsize: input horizontal size of after preprocessing
 68 * @tvsize: input vertical size of after preprocessing
 69 * @ohsize: output horizontal size
 70 * @ovsize: output vertical size
 71 * @ihsize: input horizontal size
 72 * @max_hsize: maximum horizontal size
 73 */
 74struct c3_isp_pps_io_size {
 75	u32 thsize;
 76	u32 tvsize;
 77	u32 ohsize;
 78	u32 ovsize;
 79	u32 ihsize;
 80	u32 max_hsize;
 81};
 82
 83/* The normal parameters of pps module */
 84static const int c3_isp_pps_lut[C3_ISP_PPS_LUT_H_NUM][4] =  {
 85	{  0, 511,   0,   0}, { -5, 511,   5,   0}, {-10, 511,  11,   0},
 86	{-14, 510,  17,  -1}, {-18, 508,  23,  -1}, {-22, 506,  29,  -1},
 87	{-25, 503,  36,  -2}, {-28, 500,  43,  -3}, {-32, 496,  51,  -3},
 88	{-34, 491,  59,  -4}, {-37, 487,  67,  -5}, {-39, 482,  75,  -6},
 89	{-41, 476,  84,  -7}, {-42, 470,  92,  -8}, {-44, 463, 102,  -9},
 90	{-45, 456, 111, -10}, {-45, 449, 120, -12}, {-47, 442, 130, -13},
 91	{-47, 434, 140, -15}, {-47, 425, 151, -17}, {-47, 416, 161, -18},
 92	{-47, 407, 172, -20}, {-47, 398, 182, -21}, {-47, 389, 193, -23},
 93	{-46, 379, 204, -25}, {-45, 369, 215, -27}, {-44, 358, 226, -28},
 94	{-43, 348, 237, -30}, {-43, 337, 249, -31}, {-41, 326, 260, -33},
 95	{-40, 316, 271, -35}, {-39, 305, 282, -36}, {-37, 293, 293, -37}
 96};
 97
 98static const struct c3_isp_rsz_format_info
 99*rsz_find_format_by_code(u32 code, u32 pad)
100{
101	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
102		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];
103
104		if (info->mbus_code == code && info->pads & BIT(pad))
105			return info;
106	}
107
108	return NULL;
109}
110
111static const struct c3_isp_rsz_format_info
112*rsz_find_format_by_index(u32 index, u32 pad)
113{
114	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
115		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];
116
117		if (!(info->pads & BIT(pad)))
118			continue;
119
120		if (!index)
121			return info;
122
123		index--;
124	}
125
126	return NULL;
127}
128
129static void c3_isp_rsz_pps_size(struct c3_isp_resizer *rsz,
130				struct c3_isp_pps_io_size *io_size)
131{
132	int thsize = io_size->thsize;
133	int tvsize = io_size->tvsize;
134	u32 ohsize = io_size->ohsize;
135	u32 ovsize = io_size->ovsize;
136	u32 ihsize = io_size->ihsize;
137	u32 max_hsize = io_size->max_hsize;
138	int h_int;
139	int v_int;
140	int h_fract;
141	int v_fract;
142	int yuv444to422_en;
143
144	/* Calculate the integer part of horizonal scaler step */
145	h_int = thsize / ohsize;
146
147	/* Calculate the vertical part of horizonal scaler step */
148	v_int = tvsize / ovsize;
149
150	/*
151	 * Calculate the fraction part of horizonal scaler step.
152	 * step_h_fraction = (source / dest) * 2^24,
153	 * so step_h_fraction = ((source << 12) / dest) << 12.
154	 */
155	h_fract = ((thsize << 12) / ohsize) << 12;
156
157	/*
158	 * Calculate the fraction part of vertical scaler step
159	 * step_v_fraction = (source / dest) * 2^24,
160	 * so step_v_fraction = ((source << 12) / dest) << 12.
161	 */
162	v_fract = ((tvsize << 12) / ovsize) << 12;
163
164	yuv444to422_en = ihsize > (max_hsize / 2) ? 1 : 0;
165
166	c3_isp_update_bits(rsz->isp,
167			   C3_ISP_DISP_REG(DISP0_PPS_444TO422, rsz->id),
168			   DISP0_PPS_444TO422_EN_MASK,
169			   DISP0_PPS_444TO422_EN(yuv444to422_en));
170
171	c3_isp_write(rsz->isp,
172		     C3_ISP_DISP_REG(DISP0_PPS_VSC_START_PHASE_STEP, rsz->id),
173		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_FRAC(v_fract) |
174		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_INTE(v_int));
175
176	c3_isp_write(rsz->isp,
177		     C3_ISP_DISP_REG(DISP0_PPS_HSC_START_PHASE_STEP, rsz->id),
178		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_FRAC(h_fract) |
179		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_INTE(h_int));
180}
181
182static void c3_isp_rsz_pps_lut(struct c3_isp_resizer *rsz, u32 ctype)
183{
184	unsigned int i;
185
186	/*
187	 * Default value of this register is 0, so only need to set
188	 * SCALE_LUMA_COEF_S11_MODE and SCALE_LUMA_CTYPE. This register needs
189	 * to be written in one time.
190	 */
191	c3_isp_write(rsz->isp,
192		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_LUMA, rsz->id),
193		     ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_EN |
194		     ISP_SCALE0_COEF_IDX_LUMA_CTYPE(ctype));
195
196	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
197		c3_isp_write(rsz->isp,
198			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
199			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][0]) |
200			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][1]));
201		c3_isp_write(rsz->isp,
202			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
203			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][2]) |
204			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][3]));
205	}
206
207	/*
208	 * Default value of this register is 0, so only need to set
209	 * SCALE_CHRO_COEF_S11_MODE and SCALE_CHRO_CTYPE. This register needs
210	 * to be written in one time.
211	 */
212	c3_isp_write(rsz->isp,
213		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_CHRO, rsz->id),
214		     ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_EN |
215		     ISP_SCALE0_COEF_IDX_CHRO_CTYPE(ctype));
216
217	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
218		c3_isp_write(rsz->isp,
219			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
220			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][0]) |
221			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][1]));
222		c3_isp_write(rsz->isp,
223			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
224			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][2]) |
225			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][3]));
226	}
227}
228
229static void c3_isp_rsz_pps_disable(struct c3_isp_resizer *rsz)
230{
231	c3_isp_update_bits(rsz->isp,
232			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
233			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
234			   DISP0_PPS_SCALE_EN_HSC_DIS);
235	c3_isp_update_bits(rsz->isp,
236			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
237			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
238			   DISP0_PPS_SCALE_EN_VSC_DIS);
239	c3_isp_update_bits(rsz->isp,
240			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
241			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
242			   DISP0_PPS_SCALE_EN_PREVSC_DIS);
243	c3_isp_update_bits(rsz->isp,
244			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
245			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
246			   DISP0_PPS_SCALE_EN_PREHSC_DIS);
247}
248
249static int c3_isp_rsz_pps_enable(struct c3_isp_resizer *rsz,
250				 struct v4l2_subdev_state *state)
251{
252	struct v4l2_rect *crop;
253	struct v4l2_rect *cmps;
254	int max_hsize;
255	int hsc_en, vsc_en;
256	int preh_en, prev_en;
257	u32 prehsc_rate;
258	u32 prevsc_flt_num;
259	int pre_vscale_max_hsize;
260	u32 ihsize_after_pre_hsc;
261	u32 ihsize_after_pre_hsc_alt;
262	u32 vsc_tap_num_alt;
263	u32 ihsize;
264	u32 ivsize;
265	struct c3_isp_pps_io_size io_size;
266
267	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
268	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
269
270	ihsize = crop->width;
271	ivsize = crop->height;
272
273	hsc_en = (ihsize == cmps->width) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;
274	vsc_en = (ivsize == cmps->height) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;
275
276	/* Disable pps when there no need to use pps */
277	if (!hsc_en && !vsc_en) {
278		c3_isp_rsz_pps_disable(rsz);
279		return 0;
280	}
281
282	/* Pre-scale needs to be enable if the down scaling factor exceeds 4 */
283	preh_en = (ihsize > cmps->width * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;
284	prev_en = (ivsize > cmps->height * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;
285
286	if (rsz->id == C3_ISP_RSZ_2) {
287		max_hsize = C3_ISP_MAX_WIDTH;
288
289		/* Set vertical tap number */
290		prevsc_flt_num = 4;
291
292		/* Set the max hsize of pre-vertical scale */
293		pre_vscale_max_hsize = max_hsize / 2;
294	} else {
295		max_hsize = C3_ISP_DEFAULT_WIDTH;
296
297		/* Set vertical tap number and the max hsize of pre-vertical */
298		if (ihsize > (max_hsize / 2) &&
299		    ihsize <= max_hsize && prev_en) {
300			prevsc_flt_num = 2;
301			pre_vscale_max_hsize = max_hsize;
302		} else {
303			prevsc_flt_num = 4;
304			pre_vscale_max_hsize = max_hsize / 2;
305		}
306	}
307
308	/*
309	 * Set pre-horizonal scale rate and the hsize of after
310	 * pre-horizonal scale.
311	 */
312	if (preh_en) {
313		prehsc_rate = 1;
314		ihsize_after_pre_hsc = DIV_ROUND_UP(ihsize, 2);
315	} else {
316		prehsc_rate = 0;
317		ihsize_after_pre_hsc = ihsize;
318	}
319
320	/* Change pre-horizonal scale rate */
321	if (prev_en && ihsize_after_pre_hsc >= pre_vscale_max_hsize)
322		prehsc_rate += 1;
323
324	/* Set the actual hsize of after pre-horizonal scale */
325	if (preh_en)
326		ihsize_after_pre_hsc_alt =
327			DIV_ROUND_UP(ihsize, 1 << prehsc_rate);
328	else
329		ihsize_after_pre_hsc_alt = ihsize;
330
331	/* Set vertical scaler bank length */
332	if (ihsize_after_pre_hsc_alt <= (max_hsize / 2))
333		vsc_tap_num_alt = 4;
334	else if (ihsize_after_pre_hsc_alt <= max_hsize)
335		vsc_tap_num_alt = prev_en ? 2 : 4;
336	else
337		vsc_tap_num_alt = prev_en ? 4 : 2;
338
339	io_size.thsize = ihsize_after_pre_hsc_alt;
340	io_size.tvsize = prev_en ? DIV_ROUND_UP(ivsize, 2) : ivsize;
341	io_size.ohsize = cmps->width;
342	io_size.ovsize = cmps->height;
343	io_size.ihsize = ihsize;
344	io_size.max_hsize = max_hsize;
345
346	c3_isp_rsz_pps_size(rsz, &io_size);
347	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_0);
348	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_2);
349
350	c3_isp_update_bits(rsz->isp,
351			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
352			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM_MASK,
353			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM(vsc_tap_num_alt));
354	c3_isp_update_bits(rsz->isp,
355			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
356			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM_MASK,
357			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM(prevsc_flt_num));
358
359	c3_isp_update_bits(rsz->isp,
360			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
361			   DISP0_PPS_SCALE_EN_PREVSC_RATE_MASK,
362			   DISP0_PPS_SCALE_EN_PREVSC_RATE(prev_en));
363	c3_isp_update_bits(rsz->isp,
364			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
365			   DISP0_PPS_SCALE_EN_PREHSC_RATE_MASK,
366			   DISP0_PPS_SCALE_EN_PREHSC_RATE(prehsc_rate));
367
368	c3_isp_update_bits(rsz->isp,
369			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
370			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
371			   DISP0_PPS_SCALE_EN_HSC_EN(hsc_en));
372	c3_isp_update_bits(rsz->isp,
373			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
374			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
375			   DISP0_PPS_SCALE_EN_VSC_EN(vsc_en));
376	c3_isp_update_bits(rsz->isp,
377			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
378			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
379			   DISP0_PPS_SCALE_EN_PREVSC_EN(prev_en));
380	c3_isp_update_bits(rsz->isp,
381			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
382			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
383			   DISP0_PPS_SCALE_EN_PREHSC_EN(preh_en));
384
385	c3_isp_update_bits(rsz->isp,
386			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
387			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS_MASK,
388			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS(9));
389	c3_isp_update_bits(rsz->isp,
390			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
391			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS_MASK,
392			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS(9));
393
394	return 0;
395}
396
397static void c3_isp_rsz_start(struct c3_isp_resizer *rsz,
398			     struct v4l2_subdev_state *state)
399{
400	struct v4l2_mbus_framefmt *sink_fmt;
401	struct v4l2_mbus_framefmt *src_fmt;
402	const struct c3_isp_rsz_format_info *rsz_fmt;
403	struct v4l2_rect *sink_crop;
404	u32 mask;
405	u32 val;
406
407	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
408	sink_crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
409	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
410	rsz_fmt = rsz_find_format_by_code(sink_fmt->code, C3_ISP_RSZ_PAD_SINK);
411
412	if (rsz->id == C3_ISP_RSZ_0) {
413		mask = ISP_TOP_DISPIN_SEL_DISP0_MASK;
414		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP0_MIPI_OUT
415				      : ISP_TOP_DISPIN_SEL_DISP0_CORE_OUT;
416	} else if (rsz->id == C3_ISP_RSZ_1) {
417		mask = ISP_TOP_DISPIN_SEL_DISP1_MASK;
418		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP1_MIPI_OUT
419				      : ISP_TOP_DISPIN_SEL_DISP1_CORE_OUT;
420	} else {
421		mask = ISP_TOP_DISPIN_SEL_DISP2_MASK;
422		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP2_MIPI_OUT
423				      : ISP_TOP_DISPIN_SEL_DISP2_CORE_OUT;
424	}
425
426	c3_isp_update_bits(rsz->isp, ISP_TOP_DISPIN_SEL, mask, val);
427
428	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(ISP_DISP0_TOP_IN_SIZE, rsz->id),
429		     ISP_DISP0_TOP_IN_SIZE_HSIZE(sink_fmt->width) |
430		     ISP_DISP0_TOP_IN_SIZE_VSIZE(sink_fmt->height));
431
432	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_START, rsz->id),
433		     DISP0_TOP_CRP2_START_V_START(sink_crop->top) |
434		     DISP0_TOP_CRP2_START_H_START(sink_crop->left));
435
436	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_SIZE, rsz->id),
437		     DISP0_TOP_CRP2_SIZE_V_SIZE(sink_crop->height) |
438		     DISP0_TOP_CRP2_SIZE_H_SIZE(sink_crop->width));
439
440	c3_isp_update_bits(rsz->isp,
441			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
442			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
443			   DISP0_TOP_TOP_CTRL_CROP2_EN);
444
445	if (!rsz_fmt->is_raw)
446		c3_isp_rsz_pps_enable(rsz, state);
447
448	c3_isp_update_bits(rsz->isp,
449			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
450			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT_MASK,
451			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT(src_fmt->height));
452	c3_isp_update_bits(rsz->isp,
453			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
454			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH_MASK,
455			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH(src_fmt->width));
456
457	if (rsz->id == C3_ISP_RSZ_0) {
458		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
459		val = ISP_TOP_PATH_EN_DISP0_EN;
460	} else if (rsz->id == C3_ISP_RSZ_1) {
461		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
462		val = ISP_TOP_PATH_EN_DISP1_EN;
463	} else {
464		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
465		val = ISP_TOP_PATH_EN_DISP2_EN;
466	}
467
468	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);
469}
470
471static void c3_isp_rsz_stop(struct c3_isp_resizer *rsz)
472{
473	u32 mask;
474	u32 val;
475
476	if (rsz->id == C3_ISP_RSZ_0) {
477		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
478		val = ISP_TOP_PATH_EN_DISP0_DIS;
479	} else if (rsz->id == C3_ISP_RSZ_1) {
480		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
481		val = ISP_TOP_PATH_EN_DISP1_DIS;
482	} else {
483		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
484		val = ISP_TOP_PATH_EN_DISP2_DIS;
485	}
486
487	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);
488
489	c3_isp_update_bits(rsz->isp,
490			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
491			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
492			   DISP0_TOP_TOP_CTRL_CROP2_DIS);
493
494	c3_isp_rsz_pps_disable(rsz);
495}
496
497static int c3_isp_rsz_enable_streams(struct v4l2_subdev *sd,
498				     struct v4l2_subdev_state *state,
499				     u32 pad, u64 streams_mask)
500{
501	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);
502
503	c3_isp_rsz_start(rsz, state);
504
505	c3_isp_params_pre_cfg(rsz->isp);
506	c3_isp_stats_pre_cfg(rsz->isp);
507
508	return v4l2_subdev_enable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
509}
510
511static int c3_isp_rsz_disable_streams(struct v4l2_subdev *sd,
512				      struct v4l2_subdev_state *state,
513				      u32 pad, u64 streams_mask)
514{
515	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);
516
517	c3_isp_rsz_stop(rsz);
518
519	return v4l2_subdev_disable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
520}
521
522static int c3_isp_rsz_enum_mbus_code(struct v4l2_subdev *sd,
523				     struct v4l2_subdev_state *state,
524				     struct v4l2_subdev_mbus_code_enum *code)
525{
526	const struct c3_isp_rsz_format_info *info;
527
528	info = rsz_find_format_by_index(code->index, code->pad);
529	if (!info)
530		return -EINVAL;
531
532	code->code = info->mbus_code;
533
534	return 0;
535}
536
537static void c3_isp_rsz_set_sink_fmt(struct v4l2_subdev_state *state,
538				    struct v4l2_subdev_format *format)
539{
540	struct v4l2_mbus_framefmt *sink_fmt;
541	struct v4l2_mbus_framefmt *src_fmt;
542	struct v4l2_rect *sink_crop;
543	struct v4l2_rect *sink_cmps;
544	const struct c3_isp_rsz_format_info *rsz_fmt;
545
546	sink_fmt = v4l2_subdev_state_get_format(state, format->pad);
547	sink_crop = v4l2_subdev_state_get_crop(state, format->pad);
548	sink_cmps = v4l2_subdev_state_get_compose(state, format->pad);
549	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
550
551	rsz_fmt = rsz_find_format_by_code(format->format.code, format->pad);
552	if (rsz_fmt)
553		sink_fmt->code = format->format.code;
554	else
555		sink_fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
556
557	sink_fmt->width = clamp_t(u32, format->format.width,
558				  C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
559	sink_fmt->height = clamp_t(u32, format->format.height,
560				   C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
561	sink_fmt->field = V4L2_FIELD_NONE;
562	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
563
564	if (rsz_fmt && rsz_fmt->is_raw) {
565		sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
566		sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
567		sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
568	} else {
569		sink_fmt->colorspace = V4L2_COLORSPACE_SRGB;
570		sink_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
571		sink_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
572	}
573
574	sink_crop->width = sink_fmt->width;
575	sink_crop->height = sink_fmt->height;
576	sink_crop->left = 0;
577	sink_crop->top = 0;
578
579	sink_cmps->width = sink_crop->width;
580	sink_cmps->height = sink_crop->height;
581	sink_cmps->left = 0;
582	sink_cmps->top = 0;
583
584	src_fmt->code = sink_fmt->code;
585	src_fmt->width = sink_cmps->width;
586	src_fmt->height = sink_cmps->height;
587
588	format->format = *sink_fmt;
589}
590
591static void c3_isp_rsz_set_source_fmt(struct v4l2_subdev_state *state,
592				      struct v4l2_subdev_format *format)
593{
594	struct v4l2_mbus_framefmt *src_fmt;
595	struct v4l2_mbus_framefmt *sink_fmt;
596	struct v4l2_rect *sink_cmps;
597	const struct c3_isp_rsz_format_info *rsz_fmt;
598
599	src_fmt = v4l2_subdev_state_get_format(state, format->pad);
600	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
601	sink_cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
602
603	src_fmt->code = sink_fmt->code;
604	src_fmt->width = sink_cmps->width;
605	src_fmt->height = sink_cmps->height;
606	src_fmt->field = V4L2_FIELD_NONE;
607	src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
608
609	rsz_fmt = rsz_find_format_by_code(src_fmt->code, format->pad);
610	if (rsz_fmt->is_raw) {
611		src_fmt->colorspace = V4L2_COLORSPACE_RAW;
612		src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
613		src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
614	} else {
615		src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
616		src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
617		src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
618	}
619
620	format->format = *src_fmt;
621}
622
623static int c3_isp_rsz_set_fmt(struct v4l2_subdev *sd,
624			      struct v4l2_subdev_state *state,
625			      struct v4l2_subdev_format *format)
626{
627	if (format->pad == C3_ISP_RSZ_PAD_SINK)
628		c3_isp_rsz_set_sink_fmt(state, format);
629	else
630		c3_isp_rsz_set_source_fmt(state, format);
631
632	return 0;
633}
634
635static int c3_isp_rsz_get_selection(struct v4l2_subdev *sd,
636				    struct v4l2_subdev_state *state,
637				    struct v4l2_subdev_selection *sel)
638{
639	struct v4l2_mbus_framefmt *fmt;
640	struct v4l2_rect *crop;
641	struct v4l2_rect *cmps;
642
643	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
644		return -EINVAL;
645
646	switch (sel->target) {
647	case V4L2_SEL_TGT_CROP_BOUNDS:
648		fmt = v4l2_subdev_state_get_format(state, sel->pad);
649		sel->r.width = fmt->width;
650		sel->r.height = fmt->height;
651		sel->r.left = 0;
652		sel->r.top = 0;
653		break;
654	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
655		crop = v4l2_subdev_state_get_crop(state, sel->pad);
656		sel->r.width = crop->width;
657		sel->r.height = crop->height;
658		sel->r.left = 0;
659		sel->r.top = 0;
660		break;
661	case V4L2_SEL_TGT_CROP:
662		crop = v4l2_subdev_state_get_crop(state, sel->pad);
663		sel->r = *crop;
664		break;
665	case V4L2_SEL_TGT_COMPOSE:
666		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
667		sel->r = *cmps;
668		break;
669	default:
670		return -EINVAL;
671	}
672
673	return 0;
674}
675
676static int c3_isp_rsz_set_selection(struct v4l2_subdev *sd,
677				    struct v4l2_subdev_state *state,
678				    struct v4l2_subdev_selection *sel)
679{
680	struct v4l2_mbus_framefmt *fmt;
681	struct v4l2_mbus_framefmt *src_fmt;
682	struct v4l2_rect *crop;
683	struct v4l2_rect *cmps;
684
685	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
686		return -EINVAL;
687
688	switch (sel->target) {
689	case V4L2_SEL_TGT_CROP:
690		fmt = v4l2_subdev_state_get_format(state, sel->pad);
691		crop = v4l2_subdev_state_get_crop(state, sel->pad);
692		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
693		src_fmt = v4l2_subdev_state_get_format(state,
694						       C3_ISP_RSZ_PAD_SOURCE);
695
696		sel->r.left = clamp_t(s32, sel->r.left, 0, fmt->width - 1);
697		sel->r.top = clamp_t(s32, sel->r.top, 0, fmt->height - 1);
698		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
699				     fmt->width - sel->r.left);
700		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
701				      fmt->height - sel->r.top);
702
703		crop->width = ALIGN(sel->r.width, 2);
704		crop->height = ALIGN(sel->r.height, 2);
705		crop->left = sel->r.left;
706		crop->top = sel->r.top;
707
708		*cmps = *crop;
709
710		src_fmt->code = fmt->code;
711		src_fmt->width = cmps->width;
712		src_fmt->height = cmps->height;
713
714		sel->r = *crop;
715		break;
716	case V4L2_SEL_TGT_COMPOSE:
717		crop = v4l2_subdev_state_get_crop(state, sel->pad);
718		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
719
720		sel->r.left = 0;
721		sel->r.top = 0;
722		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
723				     crop->width);
724		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
725				      crop->height);
726
727		cmps->width = ALIGN(sel->r.width, 2);
728		cmps->height = ALIGN(sel->r.height, 2);
729		cmps->left = sel->r.left;
730		cmps->top = sel->r.top;
731
732		sel->r = *cmps;
733
734		fmt = v4l2_subdev_state_get_format(state,
735						   C3_ISP_RSZ_PAD_SOURCE);
736		fmt->width = cmps->width;
737		fmt->height = cmps->height;
738		break;
739	default:
740		return -EINVAL;
741	}
742
743	return 0;
744}
745
746static int c3_isp_rsz_init_state(struct v4l2_subdev *sd,
747				 struct v4l2_subdev_state *state)
748{
749	struct v4l2_mbus_framefmt *fmt;
750	struct v4l2_rect *crop;
751	struct v4l2_rect *cmps;
752
753	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
754	fmt->width = C3_ISP_DEFAULT_WIDTH;
755	fmt->height = C3_ISP_DEFAULT_HEIGHT;
756	fmt->field = V4L2_FIELD_NONE;
757	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
758	fmt->colorspace = V4L2_COLORSPACE_SRGB;
759	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
760	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
761	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
762
763	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
764	crop->width = C3_ISP_DEFAULT_WIDTH;
765	crop->height = C3_ISP_DEFAULT_HEIGHT;
766	crop->left = 0;
767	crop->top = 0;
768
769	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
770	cmps->width = C3_ISP_DEFAULT_WIDTH;
771	cmps->height = C3_ISP_DEFAULT_HEIGHT;
772	cmps->left = 0;
773	cmps->top = 0;
774
775	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
776	fmt->width = cmps->width;
777	fmt->height = cmps->height;
778	fmt->field = V4L2_FIELD_NONE;
779	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
780	fmt->colorspace = V4L2_COLORSPACE_SRGB;
781	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
782	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
783	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
784
785	return 0;
786}
787
788static const struct v4l2_subdev_pad_ops c3_isp_rsz_pad_ops = {
789	.enum_mbus_code = c3_isp_rsz_enum_mbus_code,
790	.get_fmt = v4l2_subdev_get_fmt,
791	.set_fmt = c3_isp_rsz_set_fmt,
792	.get_selection = c3_isp_rsz_get_selection,
793	.set_selection = c3_isp_rsz_set_selection,
794	.enable_streams = c3_isp_rsz_enable_streams,
795	.disable_streams = c3_isp_rsz_disable_streams,
796};
797
798static const struct v4l2_subdev_ops c3_isp_rsz_subdev_ops = {
799	.pad = &c3_isp_rsz_pad_ops,
800};
801
802static const struct v4l2_subdev_internal_ops c3_isp_rsz_internal_ops = {
803	.init_state = c3_isp_rsz_init_state,
804};
805
806/* Media entity operations */
807static const struct media_entity_operations c3_isp_rsz_entity_ops = {
808	.link_validate = v4l2_subdev_link_validate,
809};
810
811static int c3_isp_rsz_register(struct c3_isp_resizer *rsz)
812{
813	struct v4l2_subdev *sd = &rsz->sd;
814	int ret;
815
816	v4l2_subdev_init(sd, &c3_isp_rsz_subdev_ops);
817	sd->owner = THIS_MODULE;
818	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
819	sd->internal_ops = &c3_isp_rsz_internal_ops;
820	snprintf(sd->name, sizeof(sd->name), "c3-isp-resizer%u", rsz->id);
821
822	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
823	sd->entity.ops = &c3_isp_rsz_entity_ops;
824
825	sd->dev = rsz->isp->dev;
826	v4l2_set_subdevdata(sd, rsz);
827
828	rsz->pads[C3_ISP_RSZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
829	rsz->pads[C3_ISP_RSZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
830	ret = media_entity_pads_init(&sd->entity, C3_ISP_RSZ_PAD_MAX,
831				     rsz->pads);
832	if (ret)
833		return ret;
834
835	ret = v4l2_subdev_init_finalize(sd);
836	if (ret)
837		goto err_entity_cleanup;
838
839	ret = v4l2_device_register_subdev(&rsz->isp->v4l2_dev, sd);
840	if (ret)
841		goto err_subdev_cleanup;
842
843	return 0;
844
845err_subdev_cleanup:
846	v4l2_subdev_cleanup(sd);
847err_entity_cleanup:
848	media_entity_cleanup(&sd->entity);
849	return ret;
850}
851
852static void c3_isp_rsz_unregister(struct c3_isp_resizer *rsz)
853{
854	struct v4l2_subdev *sd = &rsz->sd;
855
856	v4l2_device_unregister_subdev(sd);
857	v4l2_subdev_cleanup(sd);
858	media_entity_cleanup(&sd->entity);
859}
860
861int c3_isp_resizers_register(struct c3_isp_device *isp)
862{
863	int ret;
864
865	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
866		struct c3_isp_resizer *rsz = &isp->resizers[i];
867
868		rsz->id = i;
869		rsz->isp = isp;
870		rsz->src_sd = &isp->core.sd;
871		rsz->src_pad = C3_ISP_CORE_PAD_SOURCE_VIDEO_0 + i;
872
873		ret = c3_isp_rsz_register(rsz);
874		if (ret) {
875			rsz->isp = NULL;
876			c3_isp_resizers_unregister(isp);
877			return ret;
878		}
879	}
880
881	return 0;
882}
883
884void c3_isp_resizers_unregister(struct c3_isp_device *isp)
885{
886	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
887		struct c3_isp_resizer *rsz = &isp->resizers[i];
888
889		if (rsz->isp)
890			c3_isp_rsz_unregister(rsz);
891	}
892}