Quelle  c3-isp-resizer.c   Sprache: 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);
 }
}