media: i2c: ov772x: Support frame interval handling (35089491) · Commits · e / devices / android_kernel_oneplus_sm7250

drivers/media/i2c/ov772x.c

+196 −17

Original line number	Diff line number	Diff line
		@@ -250,6 +250,7 @@
		#define AEC_1p2 0x10 /* 01: 1/2 window */
		#define AEC_1p4 0x20 /* 10: 1/4 window */
		#define AEC_2p3 0x30 /* 11: Low 2/3 window */
		#define COM4_RESERVED 0x01 /* Reserved bit */

		/* COM5 */
		#define AFR_ON_OFF 0x80 /* Auto frame rate control ON/OFF selection */
		@@ -267,6 +268,10 @@
		/* AEC max step control */
		#define AEC_NO_LIMIT 0x01 /* 0 : AEC incease step has limit */
		/* 1 : No limit to AEC increase step */
		/* CLKRC */
		/* Input clock divider register */
		#define CLKRC_RESERVED 0x80 /* Reserved bit */
		#define CLKRC_DIV(n) ((n) - 1)

		/* COM7 */
		/* SCCB Register Reset */
		@@ -372,6 +377,19 @@
		#define OV7725 0x7721
		#define VERSION(pid, ver) ((pid<<8)\|(ver&0xFF))

		/*
		* PLL multipliers
		*/
		static struct {
		unsigned int mult;
		u8 com4;
		} ov772x_pll[] = {
		{ 1, PLL_BYPASS, },
		{ 4, PLL_4x, },
		{ 6, PLL_6x, },
		{ 8, PLL_8x, },
		};

		/*
		* struct
		*/
		@@ -388,6 +406,7 @@ struct ov772x_color_format {
		struct ov772x_win_size {
		char *name;
		unsigned char com7_bit;
		unsigned int sizeimage;
		struct v4l2_rect rect;
		};

		@@ -404,6 +423,7 @@ struct ov772x_priv {
		unsigned short flag_hflip:1;
		/* band_filter = COM8[5] ? 256 - BDBASE : 0 */
		unsigned short band_filter;
		unsigned int fps;
		};

		/*
		@@ -489,6 +509,7 @@ static const struct ov772x_win_size ov772x_win_sizes[] = {
		{
		.name = "VGA",
		.com7_bit = SLCT_VGA,
		.sizeimage = 510 * 748,
		.rect = {
		.left = 140,
		.top = 14,
		@@ -498,6 +519,7 @@ static const struct ov772x_win_size ov772x_win_sizes[] = {
		}, {
		.name = "QVGA",
		.com7_bit = SLCT_QVGA,
		.sizeimage = 278 * 576,
		.rect = {
		.left = 252,
		.top = 6,
		@@ -507,6 +529,11 @@ static const struct ov772x_win_size ov772x_win_sizes[] = {
		},
		};

		/*
		* frame rate settings lists
		*/
		static unsigned int ov772x_frame_intervals[] = { 5, 10, 15, 20, 30, 60 };

		/*
		* general function
		*/
		@@ -574,6 +601,128 @@ static int ov772x_s_stream(struct v4l2_subdev *sd, int enable)
		return 0;
		}

		static int ov772x_set_frame_rate(struct ov772x_priv *priv,
		struct v4l2_fract *tpf,
		const struct ov772x_color_format *cfmt,
		const struct ov772x_win_size *win)
		{
		struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
		unsigned long fin = clk_get_rate(priv->clk);
		unsigned int fps = tpf->numerator ?
		tpf->denominator / tpf->numerator :
		tpf->denominator;
		unsigned int best_diff;
		unsigned int fsize;
		unsigned int pclk;
		unsigned int diff;
		unsigned int idx;
		unsigned int i;
		u8 clkrc = 0;
		u8 com4 = 0;
		int ret;

		/* Approximate to the closest supported frame interval. */
		best_diff = ~0L;
		for (i = 0, idx = 0; i < ARRAY_SIZE(ov772x_frame_intervals); i++) {
		diff = abs(fps - ov772x_frame_intervals[i]);
		if (diff < best_diff) {
		idx = i;
		best_diff = diff;
		}
		}
		fps = ov772x_frame_intervals[idx];

		/* Use image size (with blankings) to calculate desired pixel clock. */
		switch (cfmt->com7 & OFMT_MASK) {
		case OFMT_BRAW:
		fsize = win->sizeimage;
		break;
		case OFMT_RGB:
		case OFMT_YUV:
		default:
		fsize = win->sizeimage * 2;
		break;
		}

		pclk = fps * fsize;

		/*
		* Pixel clock generation circuit is pretty simple:
		*
		* Fin -> [ / CLKRC_div] -> [ * PLL_mult] -> pclk
		*
		* Try to approximate the desired pixel clock testing all available
		* PLL multipliers (1x, 4x, 6x, 8x) and calculate corresponding
		* divisor with:
		*
		* div = PLL_mult * Fin / pclk
		*
		* and re-calculate the pixel clock using it:
		*
		* pclk = Fin * PLL_mult / CLKRC_div
		*
		* Choose the PLL_mult and CLKRC_div pair that gives a pixel clock
		* closer to the desired one.
		*
		* The desired pixel clock is calculated using a known frame size
		* (blanking included) and FPS.
		*/
		best_diff = ~0L;
		for (i = 0; i < ARRAY_SIZE(ov772x_pll); i++) {
		unsigned int pll_mult = ov772x_pll[i].mult;
		unsigned int pll_out = pll_mult * fin;
		unsigned int t_pclk;
		unsigned int div;

		if (pll_out < pclk)
		continue;

		div = DIV_ROUND_CLOSEST(pll_out, pclk);
		t_pclk = DIV_ROUND_CLOSEST(fin * pll_mult, div);
		diff = abs(pclk - t_pclk);
		if (diff < best_diff) {
		best_diff = diff;
		clkrc = CLKRC_DIV(div);
		com4 = ov772x_pll[i].com4;
		}
		}

		ret = ov772x_write(client, COM4, com4 \| COM4_RESERVED);
		if (ret < 0)
		return ret;

		ret = ov772x_write(client, CLKRC, clkrc \| CLKRC_RESERVED);
		if (ret < 0)
		return ret;

		tpf->numerator = 1;
		tpf->denominator = fps;
		priv->fps = tpf->denominator;

		return 0;
		}

		static int ov772x_g_frame_interval(struct v4l2_subdev *sd,
		struct v4l2_subdev_frame_interval *ival)
		{
		struct ov772x_priv *priv = to_ov772x(sd);
		struct v4l2_fract *tpf = &ival->interval;

		tpf->numerator = 1;
		tpf->denominator = priv->fps;

		return 0;
		}

		static int ov772x_s_frame_interval(struct v4l2_subdev *sd,
		struct v4l2_subdev_frame_interval *ival)
		{
		struct ov772x_priv *priv = to_ov772x(sd);
		struct v4l2_fract *tpf = &ival->interval;

		return ov772x_set_frame_rate(priv, tpf, priv->cfmt, priv->win);
		}

		static int ov772x_s_ctrl(struct v4l2_ctrl *ctrl)
		{
		struct ov772x_priv *priv = container_of(ctrl->handler,
		@@ -757,6 +906,7 @@ static int ov772x_set_params(struct ov772x_priv *priv,
		const struct ov772x_win_size *win)
		{
		struct i2c_client *client = v4l2_get_subdevdata(&priv->subdev);
		struct v4l2_fract tpf;
		int ret;
		u8 val;

		@@ -885,6 +1035,13 @@ static int ov772x_set_params(struct ov772x_priv *priv,
		if (ret < 0)
		goto ov772x_set_fmt_error;

		/* COM4, CLKRC: Set pixel clock and framerate. */
		tpf.numerator = 1;
		tpf.denominator = priv->fps;
		ret = ov772x_set_frame_rate(priv, &tpf, cfmt, win);
		if (ret < 0)
		goto ov772x_set_fmt_error;

		/*
		* set COM8
		*/
		@@ -1043,6 +1200,24 @@ static const struct v4l2_subdev_core_ops ov772x_subdev_core_ops = {
		.s_power = ov772x_s_power,
		};

		static int ov772x_enum_frame_interval(struct v4l2_subdev *sd,
		struct v4l2_subdev_pad_config *cfg,
		struct v4l2_subdev_frame_interval_enum *fie)
		{
		if (fie->pad \|\| fie->index >= ARRAY_SIZE(ov772x_frame_intervals))
		return -EINVAL;

		if (fie->width != VGA_WIDTH && fie->width != QVGA_WIDTH)
		return -EINVAL;
		if (fie->height != VGA_HEIGHT && fie->height != QVGA_HEIGHT)
		return -EINVAL;

		fie->interval.numerator = 1;
		fie->interval.denominator = ov772x_frame_intervals[fie->index];

		return 0;
		}

		static int ov772x_enum_mbus_code(struct v4l2_subdev *sd,
		struct v4l2_subdev_pad_config *cfg,
		struct v4l2_subdev_mbus_code_enum *code)
		@@ -1056,9 +1231,12 @@ static int ov772x_enum_mbus_code(struct v4l2_subdev *sd,

		static const struct v4l2_subdev_video_ops ov772x_subdev_video_ops = {
		.s_stream = ov772x_s_stream,
		.s_frame_interval = ov772x_s_frame_interval,
		.g_frame_interval = ov772x_g_frame_interval,
		};

		static const struct v4l2_subdev_pad_ops ov772x_subdev_pad_ops = {
		.enum_frame_interval = ov772x_enum_frame_interval,
		.enum_mbus_code = ov772x_enum_mbus_code,
		.get_selection = ov772x_get_selection,
		.get_fmt = ov772x_get_fmt,
		@@ -1134,6 +1312,7 @@ static int ov772x_probe(struct i2c_client *client,

		priv->cfmt = &ov772x_cfmts[0];
		priv->win = &ov772x_win_sizes[0];
		priv->fps = 15;

		ret = v4l2_async_register_subdev(&priv->subdev);
		if (ret)