path: root/drivers/media/platform/amlogic/c3/isp/c3-isp-resizer.c

// SPDX-License-Identifier: (GPL-2.0-only OR MIT)
/*
 * Copyright (C) 2024 Amlogic, Inc. All rights reserved
 */

#include <linux/pm_runtime.h>

#include "c3-isp-common.h"
#include "c3-isp-regs.h"

#define C3_ISP_RSZ_DEF_PAD_FMT		MEDIA_BUS_FMT_YUV10_1X30
#define C3_ISP_DISP_REG(base, id)	((base) + (id) * 0x400)
#define C3_ISP_PPS_LUT_H_NUM		33
#define C3_ISP_PPS_LUT_CTYPE_0		0
#define C3_ISP_PPS_LUT_CTYPE_2		2
#define C3_ISP_SCL_EN			1
#define C3_ISP_SCL_DIS			0

/*
 * struct c3_isp_rsz_format_info - ISP resizer format information
 *
 * @mbus_code: the mbus code
 * @pads: bitmask detailing valid pads for this mbus_code
 * @is_raw: the raw format flag of mbus code
 */
struct c3_isp_rsz_format_info {
	u32 mbus_code;
	u32 pads;
	bool is_raw;
};

static const struct c3_isp_rsz_format_info c3_isp_rsz_fmts[] = {
	/* RAW formats */
	{
		.mbus_code	= MEDIA_BUS_FMT_SBGGR16_1X16,
		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
				| BIT(C3_ISP_RSZ_PAD_SOURCE),
		.is_raw = true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGBRG16_1X16,
		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
				| BIT(C3_ISP_RSZ_PAD_SOURCE),
		.is_raw = true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SGRBG16_1X16,
		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
				| BIT(C3_ISP_RSZ_PAD_SOURCE),
		.is_raw = true,
	}, {
		.mbus_code	= MEDIA_BUS_FMT_SRGGB16_1X16,
		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
				| BIT(C3_ISP_RSZ_PAD_SOURCE),
		.is_raw = true,
	},
	/* YUV formats */
	{
		.mbus_code	= MEDIA_BUS_FMT_YUV10_1X30,
		.pads		= BIT(C3_ISP_RSZ_PAD_SINK)
				| BIT(C3_ISP_RSZ_PAD_SOURCE),
		.is_raw = false,
	},
};

/*
 * struct c3_isp_pps_io_size - ISP scaler input and output size
 *
 * @thsize: input horizontal size of after preprocessing
 * @tvsize: input vertical size of after preprocessing
 * @ohsize: output horizontal size
 * @ovsize: output vertical size
 * @ihsize: input horizontal size
 * @max_hsize: maximum horizontal size
 */
struct c3_isp_pps_io_size {
	u32 thsize;
	u32 tvsize;
	u32 ohsize;
	u32 ovsize;
	u32 ihsize;
	u32 max_hsize;
};

/* The normal parameters of pps module */
static const int c3_isp_pps_lut[C3_ISP_PPS_LUT_H_NUM][4] =  {
	{  0, 511,   0,   0}, { -5, 511,   5,   0}, {-10, 511,  11,   0},
	{-14, 510,  17,  -1}, {-18, 508,  23,  -1}, {-22, 506,  29,  -1},
	{-25, 503,  36,  -2}, {-28, 500,  43,  -3}, {-32, 496,  51,  -3},
	{-34, 491,  59,  -4}, {-37, 487,  67,  -5}, {-39, 482,  75,  -6},
	{-41, 476,  84,  -7}, {-42, 470,  92,  -8}, {-44, 463, 102,  -9},
	{-45, 456, 111, -10}, {-45, 449, 120, -12}, {-47, 442, 130, -13},
	{-47, 434, 140, -15}, {-47, 425, 151, -17}, {-47, 416, 161, -18},
	{-47, 407, 172, -20}, {-47, 398, 182, -21}, {-47, 389, 193, -23},
	{-46, 379, 204, -25}, {-45, 369, 215, -27}, {-44, 358, 226, -28},
	{-43, 348, 237, -30}, {-43, 337, 249, -31}, {-41, 326, 260, -33},
	{-40, 316, 271, -35}, {-39, 305, 282, -36}, {-37, 293, 293, -37}
};

static const struct c3_isp_rsz_format_info
*rsz_find_format_by_code(u32 code, u32 pad)
{
	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];

		if (info->mbus_code == code && info->pads & BIT(pad))
			return info;
	}

	return NULL;
}

static const struct c3_isp_rsz_format_info
*rsz_find_format_by_index(u32 index, u32 pad)
{
	for (unsigned int i = 0; i < ARRAY_SIZE(c3_isp_rsz_fmts); i++) {
		const struct c3_isp_rsz_format_info *info = &c3_isp_rsz_fmts[i];

		if (!(info->pads & BIT(pad)))
			continue;

		if (!index)
			return info;

		index--;
	}

	return NULL;
}

static void c3_isp_rsz_pps_size(struct c3_isp_resizer *rsz,
				struct c3_isp_pps_io_size *io_size)
{
	int thsize = io_size->thsize;
	int tvsize = io_size->tvsize;
	u32 ohsize = io_size->ohsize;
	u32 ovsize = io_size->ovsize;
	u32 ihsize = io_size->ihsize;
	u32 max_hsize = io_size->max_hsize;
	int h_int;
	int v_int;
	int h_fract;
	int v_fract;
	int yuv444to422_en;

	/* Calculate the integer part of horizonal scaler step */
	h_int = thsize / ohsize;

	/* Calculate the vertical part of horizonal scaler step */
	v_int = tvsize / ovsize;

	/*
	 * Calculate the fraction part of horizonal scaler step.
	 * step_h_fraction = (source / dest) * 2^24,
	 * so step_h_fraction = ((source << 12) / dest) << 12.
	 */
	h_fract = ((thsize << 12) / ohsize) << 12;

	/*
	 * Calculate the fraction part of vertical scaler step
	 * step_v_fraction = (source / dest) * 2^24,
	 * so step_v_fraction = ((source << 12) / dest) << 12.
	 */
	v_fract = ((tvsize << 12) / ovsize) << 12;

	yuv444to422_en = ihsize > (max_hsize / 2) ? 1 : 0;

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_444TO422, rsz->id),
			   DISP0_PPS_444TO422_EN_MASK,
			   DISP0_PPS_444TO422_EN(yuv444to422_en));

	c3_isp_write(rsz->isp,
		     C3_ISP_DISP_REG(DISP0_PPS_VSC_START_PHASE_STEP, rsz->id),
		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_FRAC(v_fract) |
		     DISP0_PPS_VSC_START_PHASE_STEP_VERT_INTE(v_int));

	c3_isp_write(rsz->isp,
		     C3_ISP_DISP_REG(DISP0_PPS_HSC_START_PHASE_STEP, rsz->id),
		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_FRAC(h_fract) |
		     DISP0_PPS_HSC_START_PHASE_STEP_HORIZ_INTE(h_int));
}

static void c3_isp_rsz_pps_lut(struct c3_isp_resizer *rsz, u32 ctype)
{
	unsigned int i;

	/*
	 * Default value of this register is 0, so only need to set
	 * SCALE_LUMA_COEF_S11_MODE and SCALE_LUMA_CTYPE. This register needs
	 * to be written in one time.
	 */
	c3_isp_write(rsz->isp,
		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_LUMA, rsz->id),
		     ISP_SCALE0_COEF_IDX_LUMA_COEF_S11_MODE_EN |
		     ISP_SCALE0_COEF_IDX_LUMA_CTYPE(ctype));

	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
		c3_isp_write(rsz->isp,
			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][0]) |
			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][1]));
		c3_isp_write(rsz->isp,
			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_LUMA, rsz->id),
			     ISP_SCALE0_COEF_LUMA_DATA0(c3_isp_pps_lut[i][2]) |
			     ISP_SCALE0_COEF_LUMA_DATA1(c3_isp_pps_lut[i][3]));
	}

	/*
	 * Default value of this register is 0, so only need to set
	 * SCALE_CHRO_COEF_S11_MODE and SCALE_CHRO_CTYPE. This register needs
	 * to be written in one time.
	 */
	c3_isp_write(rsz->isp,
		     C3_ISP_DISP_REG(ISP_SCALE0_COEF_IDX_CHRO, rsz->id),
		     ISP_SCALE0_COEF_IDX_CHRO_COEF_S11_MODE_EN |
		     ISP_SCALE0_COEF_IDX_CHRO_CTYPE(ctype));

	for (i = 0; i < C3_ISP_PPS_LUT_H_NUM; i++) {
		c3_isp_write(rsz->isp,
			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][0]) |
			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][1]));
		c3_isp_write(rsz->isp,
			     C3_ISP_DISP_REG(ISP_SCALE0_COEF_CHRO, rsz->id),
			     ISP_SCALE0_COEF_CHRO_DATA0(c3_isp_pps_lut[i][2]) |
			     ISP_SCALE0_COEF_CHRO_DATA1(c3_isp_pps_lut[i][3]));
	}
}

static void c3_isp_rsz_pps_disable(struct c3_isp_resizer *rsz)
{
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_HSC_DIS);
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_VSC_DIS);
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_PREVSC_DIS);
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_PREHSC_DIS);
}

static int c3_isp_rsz_pps_enable(struct c3_isp_resizer *rsz,
				 struct v4l2_subdev_state *state)
{
	struct v4l2_rect *crop;
	struct v4l2_rect *cmps;
	int max_hsize;
	int hsc_en, vsc_en;
	int preh_en, prev_en;
	u32 prehsc_rate;
	u32 prevsc_flt_num;
	int pre_vscale_max_hsize;
	u32 ihsize_after_pre_hsc;
	u32 ihsize_after_pre_hsc_alt;
	u32 vsc_tap_num_alt;
	u32 ihsize;
	u32 ivsize;
	struct c3_isp_pps_io_size io_size;

	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);

	ihsize = crop->width;
	ivsize = crop->height;

	hsc_en = (ihsize == cmps->width) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;
	vsc_en = (ivsize == cmps->height) ? C3_ISP_SCL_DIS : C3_ISP_SCL_EN;

	/* Disable pps when there no need to use pps */
	if (!hsc_en && !vsc_en) {
		c3_isp_rsz_pps_disable(rsz);
		return 0;
	}

	/* Pre-scale needs to be enable if the down scaling factor exceeds 4 */
	preh_en = (ihsize > cmps->width * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;
	prev_en = (ivsize > cmps->height * 4) ? C3_ISP_SCL_EN : C3_ISP_SCL_DIS;

	if (rsz->id == C3_ISP_RSZ_2) {
		max_hsize = C3_ISP_MAX_WIDTH;

		/* Set vertical tap number */
		prevsc_flt_num = 4;

		/* Set the max hsize of pre-vertical scale */
		pre_vscale_max_hsize = max_hsize / 2;
	} else {
		max_hsize = C3_ISP_DEFAULT_WIDTH;

		/* Set vertical tap number and the max hsize of pre-vertical */
		if (ihsize > (max_hsize / 2) &&
		    ihsize <= max_hsize && prev_en) {
			prevsc_flt_num = 2;
			pre_vscale_max_hsize = max_hsize;
		} else {
			prevsc_flt_num = 4;
			pre_vscale_max_hsize = max_hsize / 2;
		}
	}

	/*
	 * Set pre-horizonal scale rate and the hsize of after
	 * pre-horizonal scale.
	 */
	if (preh_en) {
		prehsc_rate = 1;
		ihsize_after_pre_hsc = DIV_ROUND_UP(ihsize, 2);
	} else {
		prehsc_rate = 0;
		ihsize_after_pre_hsc = ihsize;
	}

	/* Change pre-horizonal scale rate */
	if (prev_en && ihsize_after_pre_hsc >= pre_vscale_max_hsize)
		prehsc_rate += 1;

	/* Set the actual hsize of after pre-horizonal scale */
	if (preh_en)
		ihsize_after_pre_hsc_alt =
			DIV_ROUND_UP(ihsize, 1 << prehsc_rate);
	else
		ihsize_after_pre_hsc_alt = ihsize;

	/* Set vertical scaler bank length */
	if (ihsize_after_pre_hsc_alt <= (max_hsize / 2))
		vsc_tap_num_alt = 4;
	else if (ihsize_after_pre_hsc_alt <= max_hsize)
		vsc_tap_num_alt = prev_en ? 2 : 4;
	else
		vsc_tap_num_alt = prev_en ? 4 : 2;

	io_size.thsize = ihsize_after_pre_hsc_alt;
	io_size.tvsize = prev_en ? DIV_ROUND_UP(ivsize, 2) : ivsize;
	io_size.ohsize = cmps->width;
	io_size.ovsize = cmps->height;
	io_size.ihsize = ihsize;
	io_size.max_hsize = max_hsize;

	c3_isp_rsz_pps_size(rsz, &io_size);
	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_0);
	c3_isp_rsz_pps_lut(rsz, C3_ISP_PPS_LUT_CTYPE_2);

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM_MASK,
			   DISP0_PPS_SCALE_EN_VSC_TAP_NUM(vsc_tap_num_alt));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM_MASK,
			   DISP0_PPS_SCALE_EN_PREVSC_FLT_NUM(prevsc_flt_num));

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREVSC_RATE_MASK,
			   DISP0_PPS_SCALE_EN_PREVSC_RATE(prev_en));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREHSC_RATE_MASK,
			   DISP0_PPS_SCALE_EN_PREHSC_RATE(prehsc_rate));

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_HSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_HSC_EN(hsc_en));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_VSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_VSC_EN(vsc_en));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREVSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_PREVSC_EN(prev_en));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_PREHSC_EN_MASK,
			   DISP0_PPS_SCALE_EN_PREHSC_EN(preh_en));

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS_MASK,
			   DISP0_PPS_SCALE_EN_HSC_NOR_RS_BITS(9));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_PPS_SCALE_EN, rsz->id),
			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS_MASK,
			   DISP0_PPS_SCALE_EN_VSC_NOR_RS_BITS(9));

	return 0;
}

static void c3_isp_rsz_start(struct c3_isp_resizer *rsz,
			     struct v4l2_subdev_state *state)
{
	struct v4l2_mbus_framefmt *sink_fmt;
	struct v4l2_mbus_framefmt *src_fmt;
	const struct c3_isp_rsz_format_info *rsz_fmt;
	struct v4l2_rect *sink_crop;
	u32 mask;
	u32 val;

	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
	sink_crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
	rsz_fmt = rsz_find_format_by_code(sink_fmt->code, C3_ISP_RSZ_PAD_SINK);

	if (rsz->id == C3_ISP_RSZ_0) {
		mask = ISP_TOP_DISPIN_SEL_DISP0_MASK;
		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP0_MIPI_OUT
				      : ISP_TOP_DISPIN_SEL_DISP0_CORE_OUT;
	} else if (rsz->id == C3_ISP_RSZ_1) {
		mask = ISP_TOP_DISPIN_SEL_DISP1_MASK;
		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP1_MIPI_OUT
				      : ISP_TOP_DISPIN_SEL_DISP1_CORE_OUT;
	} else {
		mask = ISP_TOP_DISPIN_SEL_DISP2_MASK;
		val = rsz_fmt->is_raw ? ISP_TOP_DISPIN_SEL_DISP2_MIPI_OUT
				      : ISP_TOP_DISPIN_SEL_DISP2_CORE_OUT;
	}

	c3_isp_update_bits(rsz->isp, ISP_TOP_DISPIN_SEL, mask, val);

	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(ISP_DISP0_TOP_IN_SIZE, rsz->id),
		     ISP_DISP0_TOP_IN_SIZE_HSIZE(sink_fmt->width) |
		     ISP_DISP0_TOP_IN_SIZE_VSIZE(sink_fmt->height));

	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_START, rsz->id),
		     DISP0_TOP_CRP2_START_V_START(sink_crop->top) |
		     DISP0_TOP_CRP2_START_H_START(sink_crop->left));

	c3_isp_write(rsz->isp, C3_ISP_DISP_REG(DISP0_TOP_CRP2_SIZE, rsz->id),
		     DISP0_TOP_CRP2_SIZE_V_SIZE(sink_crop->height) |
		     DISP0_TOP_CRP2_SIZE_H_SIZE(sink_crop->width));

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
			   DISP0_TOP_TOP_CTRL_CROP2_EN);

	if (!rsz_fmt->is_raw)
		c3_isp_rsz_pps_enable(rsz, state);

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT_MASK,
			   DISP0_TOP_OUT_SIZE_SCL_OUT_HEIGHT(src_fmt->height));
	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_TOP_OUT_SIZE, rsz->id),
			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH_MASK,
			   DISP0_TOP_OUT_SIZE_SCL_OUT_WIDTH(src_fmt->width));

	if (rsz->id == C3_ISP_RSZ_0) {
		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP0_EN;
	} else if (rsz->id == C3_ISP_RSZ_1) {
		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP1_EN;
	} else {
		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP2_EN;
	}

	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);
}

static void c3_isp_rsz_stop(struct c3_isp_resizer *rsz)
{
	u32 mask;
	u32 val;

	if (rsz->id == C3_ISP_RSZ_0) {
		mask = ISP_TOP_PATH_EN_DISP0_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP0_DIS;
	} else if (rsz->id == C3_ISP_RSZ_1) {
		mask = ISP_TOP_PATH_EN_DISP1_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP1_DIS;
	} else {
		mask = ISP_TOP_PATH_EN_DISP2_EN_MASK;
		val = ISP_TOP_PATH_EN_DISP2_DIS;
	}

	c3_isp_update_bits(rsz->isp, ISP_TOP_PATH_EN, mask, val);

	c3_isp_update_bits(rsz->isp,
			   C3_ISP_DISP_REG(DISP0_TOP_TOP_CTRL, rsz->id),
			   DISP0_TOP_TOP_CTRL_CROP2_EN_MASK,
			   DISP0_TOP_TOP_CTRL_CROP2_DIS);

	c3_isp_rsz_pps_disable(rsz);
}

static int c3_isp_rsz_enable_streams(struct v4l2_subdev *sd,
				     struct v4l2_subdev_state *state,
				     u32 pad, u64 streams_mask)
{
	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);

	c3_isp_rsz_start(rsz, state);

	c3_isp_params_pre_cfg(rsz->isp);
	c3_isp_stats_pre_cfg(rsz->isp);

	return v4l2_subdev_enable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
}

static int c3_isp_rsz_disable_streams(struct v4l2_subdev *sd,
				      struct v4l2_subdev_state *state,
				      u32 pad, u64 streams_mask)
{
	struct c3_isp_resizer *rsz = v4l2_get_subdevdata(sd);

	c3_isp_rsz_stop(rsz);

	return v4l2_subdev_disable_streams(rsz->src_sd, rsz->src_pad, BIT(0));
}

static int c3_isp_rsz_enum_mbus_code(struct v4l2_subdev *sd,
				     struct v4l2_subdev_state *state,
				     struct v4l2_subdev_mbus_code_enum *code)
{
	const struct c3_isp_rsz_format_info *info;

	info = rsz_find_format_by_index(code->index, code->pad);
	if (!info)
		return -EINVAL;

	code->code = info->mbus_code;

	return 0;
}

static void c3_isp_rsz_set_sink_fmt(struct v4l2_subdev_state *state,
				    struct v4l2_subdev_format *format)
{
	struct v4l2_mbus_framefmt *sink_fmt;
	struct v4l2_mbus_framefmt *src_fmt;
	struct v4l2_rect *sink_crop;
	struct v4l2_rect *sink_cmps;
	const struct c3_isp_rsz_format_info *rsz_fmt;

	sink_fmt = v4l2_subdev_state_get_format(state, format->pad);
	sink_crop = v4l2_subdev_state_get_crop(state, format->pad);
	sink_cmps = v4l2_subdev_state_get_compose(state, format->pad);
	src_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);

	rsz_fmt = rsz_find_format_by_code(format->format.code, format->pad);
	if (rsz_fmt)
		sink_fmt->code = format->format.code;
	else
		sink_fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;

	sink_fmt->width = clamp_t(u32, format->format.width,
				  C3_ISP_MIN_WIDTH, C3_ISP_MAX_WIDTH);
	sink_fmt->height = clamp_t(u32, format->format.height,
				   C3_ISP_MIN_HEIGHT, C3_ISP_MAX_HEIGHT);
	sink_fmt->field = V4L2_FIELD_NONE;
	sink_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;

	if (rsz_fmt && rsz_fmt->is_raw) {
		sink_fmt->colorspace = V4L2_COLORSPACE_RAW;
		sink_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
		sink_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
	} else {
		sink_fmt->colorspace = V4L2_COLORSPACE_SRGB;
		sink_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
		sink_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
	}

	sink_crop->width = sink_fmt->width;
	sink_crop->height = sink_fmt->height;
	sink_crop->left = 0;
	sink_crop->top = 0;

	sink_cmps->width = sink_crop->width;
	sink_cmps->height = sink_crop->height;
	sink_cmps->left = 0;
	sink_cmps->top = 0;

	src_fmt->code = sink_fmt->code;
	src_fmt->width = sink_cmps->width;
	src_fmt->height = sink_cmps->height;

	format->format = *sink_fmt;
}

static void c3_isp_rsz_set_source_fmt(struct v4l2_subdev_state *state,
				      struct v4l2_subdev_format *format)
{
	struct v4l2_mbus_framefmt *src_fmt;
	struct v4l2_mbus_framefmt *sink_fmt;
	struct v4l2_rect *sink_cmps;
	const struct c3_isp_rsz_format_info *rsz_fmt;

	src_fmt = v4l2_subdev_state_get_format(state, format->pad);
	sink_fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
	sink_cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);

	src_fmt->code = sink_fmt->code;
	src_fmt->width = sink_cmps->width;
	src_fmt->height = sink_cmps->height;
	src_fmt->field = V4L2_FIELD_NONE;
	src_fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;

	rsz_fmt = rsz_find_format_by_code(src_fmt->code, format->pad);
	if (rsz_fmt->is_raw) {
		src_fmt->colorspace = V4L2_COLORSPACE_RAW;
		src_fmt->xfer_func = V4L2_XFER_FUNC_NONE;
		src_fmt->quantization = V4L2_QUANTIZATION_FULL_RANGE;
	} else {
		src_fmt->colorspace = V4L2_COLORSPACE_SRGB;
		src_fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
		src_fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;
	}

	format->format = *src_fmt;
}

static int c3_isp_rsz_set_fmt(struct v4l2_subdev *sd,
			      struct v4l2_subdev_state *state,
			      struct v4l2_subdev_format *format)
{
	if (format->pad == C3_ISP_RSZ_PAD_SINK)
		c3_isp_rsz_set_sink_fmt(state, format);
	else
		c3_isp_rsz_set_source_fmt(state, format);

	return 0;
}

static int c3_isp_rsz_get_selection(struct v4l2_subdev *sd,
				    struct v4l2_subdev_state *state,
				    struct v4l2_subdev_selection *sel)
{
	struct v4l2_mbus_framefmt *fmt;
	struct v4l2_rect *crop;
	struct v4l2_rect *cmps;

	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
		return -EINVAL;

	switch (sel->target) {
	case V4L2_SEL_TGT_CROP_BOUNDS:
		fmt = v4l2_subdev_state_get_format(state, sel->pad);
		sel->r.width = fmt->width;
		sel->r.height = fmt->height;
		sel->r.left = 0;
		sel->r.top = 0;
		break;
	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
		crop = v4l2_subdev_state_get_crop(state, sel->pad);
		sel->r.width = crop->width;
		sel->r.height = crop->height;
		sel->r.left = 0;
		sel->r.top = 0;
		break;
	case V4L2_SEL_TGT_CROP:
		crop = v4l2_subdev_state_get_crop(state, sel->pad);
		sel->r = *crop;
		break;
	case V4L2_SEL_TGT_COMPOSE:
		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
		sel->r = *cmps;
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int c3_isp_rsz_set_selection(struct v4l2_subdev *sd,
				    struct v4l2_subdev_state *state,
				    struct v4l2_subdev_selection *sel)
{
	struct v4l2_mbus_framefmt *fmt;
	struct v4l2_mbus_framefmt *src_fmt;
	struct v4l2_rect *crop;
	struct v4l2_rect *cmps;

	if (sel->pad == C3_ISP_RSZ_PAD_SOURCE)
		return -EINVAL;

	switch (sel->target) {
	case V4L2_SEL_TGT_CROP:
		fmt = v4l2_subdev_state_get_format(state, sel->pad);
		crop = v4l2_subdev_state_get_crop(state, sel->pad);
		cmps = v4l2_subdev_state_get_compose(state, sel->pad);
		src_fmt = v4l2_subdev_state_get_format(state,
						       C3_ISP_RSZ_PAD_SOURCE);

		sel->r.left = clamp_t(s32, sel->r.left, 0, fmt->width - 1);
		sel->r.top = clamp_t(s32, sel->r.top, 0, fmt->height - 1);
		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
				     fmt->width - sel->r.left);
		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
				      fmt->height - sel->r.top);

		crop->width = ALIGN(sel->r.width, 2);
		crop->height = ALIGN(sel->r.height, 2);
		crop->left = sel->r.left;
		crop->top = sel->r.top;

		*cmps = *crop;

		src_fmt->code = fmt->code;
		src_fmt->width = cmps->width;
		src_fmt->height = cmps->height;

		sel->r = *crop;
		break;
	case V4L2_SEL_TGT_COMPOSE:
		crop = v4l2_subdev_state_get_crop(state, sel->pad);
		cmps = v4l2_subdev_state_get_compose(state, sel->pad);

		sel->r.left = 0;
		sel->r.top = 0;
		sel->r.width = clamp(sel->r.width, C3_ISP_MIN_WIDTH,
				     crop->width);
		sel->r.height = clamp(sel->r.height, C3_ISP_MIN_HEIGHT,
				      crop->height);

		cmps->width = ALIGN(sel->r.width, 2);
		cmps->height = ALIGN(sel->r.height, 2);
		cmps->left = sel->r.left;
		cmps->top = sel->r.top;

		sel->r = *cmps;

		fmt = v4l2_subdev_state_get_format(state,
						   C3_ISP_RSZ_PAD_SOURCE);
		fmt->width = cmps->width;
		fmt->height = cmps->height;
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

static int c3_isp_rsz_init_state(struct v4l2_subdev *sd,
				 struct v4l2_subdev_state *state)
{
	struct v4l2_mbus_framefmt *fmt;
	struct v4l2_rect *crop;
	struct v4l2_rect *cmps;

	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SINK);
	fmt->width = C3_ISP_DEFAULT_WIDTH;
	fmt->height = C3_ISP_DEFAULT_HEIGHT;
	fmt->field = V4L2_FIELD_NONE;
	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
	fmt->colorspace = V4L2_COLORSPACE_SRGB;
	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;

	crop = v4l2_subdev_state_get_crop(state, C3_ISP_RSZ_PAD_SINK);
	crop->width = C3_ISP_DEFAULT_WIDTH;
	crop->height = C3_ISP_DEFAULT_HEIGHT;
	crop->left = 0;
	crop->top = 0;

	cmps = v4l2_subdev_state_get_compose(state, C3_ISP_RSZ_PAD_SINK);
	cmps->width = C3_ISP_DEFAULT_WIDTH;
	cmps->height = C3_ISP_DEFAULT_HEIGHT;
	cmps->left = 0;
	cmps->top = 0;

	fmt = v4l2_subdev_state_get_format(state, C3_ISP_RSZ_PAD_SOURCE);
	fmt->width = cmps->width;
	fmt->height = cmps->height;
	fmt->field = V4L2_FIELD_NONE;
	fmt->code = C3_ISP_RSZ_DEF_PAD_FMT;
	fmt->colorspace = V4L2_COLORSPACE_SRGB;
	fmt->xfer_func = V4L2_XFER_FUNC_SRGB;
	fmt->ycbcr_enc = V4L2_YCBCR_ENC_601;
	fmt->quantization = V4L2_QUANTIZATION_LIM_RANGE;

	return 0;
}

static const struct v4l2_subdev_pad_ops c3_isp_rsz_pad_ops = {
	.enum_mbus_code = c3_isp_rsz_enum_mbus_code,
	.get_fmt = v4l2_subdev_get_fmt,
	.set_fmt = c3_isp_rsz_set_fmt,
	.get_selection = c3_isp_rsz_get_selection,
	.set_selection = c3_isp_rsz_set_selection,
	.enable_streams = c3_isp_rsz_enable_streams,
	.disable_streams = c3_isp_rsz_disable_streams,
};

static const struct v4l2_subdev_ops c3_isp_rsz_subdev_ops = {
	.pad = &c3_isp_rsz_pad_ops,
};

static const struct v4l2_subdev_internal_ops c3_isp_rsz_internal_ops = {
	.init_state = c3_isp_rsz_init_state,
};

/* Media entity operations */
static const struct media_entity_operations c3_isp_rsz_entity_ops = {
	.link_validate = v4l2_subdev_link_validate,
};

static int c3_isp_rsz_register(struct c3_isp_resizer *rsz)
{
	struct v4l2_subdev *sd = &rsz->sd;
	int ret;

	v4l2_subdev_init(sd, &c3_isp_rsz_subdev_ops);
	sd->owner = THIS_MODULE;
	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
	sd->internal_ops = &c3_isp_rsz_internal_ops;
	snprintf(sd->name, sizeof(sd->name), "c3-isp-resizer%u", rsz->id);

	sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_SCALER;
	sd->entity.ops = &c3_isp_rsz_entity_ops;

	sd->dev = rsz->isp->dev;
	v4l2_set_subdevdata(sd, rsz);

	rsz->pads[C3_ISP_RSZ_PAD_SINK].flags = MEDIA_PAD_FL_SINK;
	rsz->pads[C3_ISP_RSZ_PAD_SOURCE].flags = MEDIA_PAD_FL_SOURCE;
	ret = media_entity_pads_init(&sd->entity, C3_ISP_RSZ_PAD_MAX,
				     rsz->pads);
	if (ret)
		return ret;

	ret = v4l2_subdev_init_finalize(sd);
	if (ret)
		goto err_entity_cleanup;

	ret = v4l2_device_register_subdev(&rsz->isp->v4l2_dev, sd);
	if (ret)
		goto err_subdev_cleanup;

	return 0;

err_subdev_cleanup:
	v4l2_subdev_cleanup(sd);
err_entity_cleanup:
	media_entity_cleanup(&sd->entity);
	return ret;
}

static void c3_isp_rsz_unregister(struct c3_isp_resizer *rsz)
{
	struct v4l2_subdev *sd = &rsz->sd;

	v4l2_device_unregister_subdev(sd);
	v4l2_subdev_cleanup(sd);
	media_entity_cleanup(&sd->entity);
}

int c3_isp_resizers_register(struct c3_isp_device *isp)
{
	int ret;

	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
		struct c3_isp_resizer *rsz = &isp->resizers[i];

		rsz->id = i;
		rsz->isp = isp;
		rsz->src_sd = &isp->core.sd;
		rsz->src_pad = C3_ISP_CORE_PAD_SOURCE_VIDEO_0 + i;

		ret = c3_isp_rsz_register(rsz);
		if (ret) {
			rsz->isp = NULL;
			c3_isp_resizers_unregister(isp);
			return ret;
		}
	}

	return 0;
}

void c3_isp_resizers_unregister(struct c3_isp_device *isp)
{
	for (unsigned int i = C3_ISP_RSZ_0; i < C3_ISP_NUM_RSZ; i++) {
		struct c3_isp_resizer *rsz = &isp->resizers[i];

		if (rsz->isp)
			c3_isp_rsz_unregister(rsz);
	}
}