Received-SPF: pass (google.com: domain of linux-kernel-owner@vger.kernel.org designates 2620:137:e000::1:20 as permitted sender) client-ip=2620:137:e000::1:20;
Date:   Mon, 22 Aug 2022 20:20:58 +0200
From:   =?utf-8?Q?Ond=C5=99ej?= Jirman <megous@megous.com>
To:     Pavel Machek <pavel@ucw.cz>
Cc:     icenowy@aosc.xyz, martijn@brixit.nl,
        kernel list <linux-kernel@vger.kernel.org>,
        phone-devel@vger.kernel.org, mchehab@kernel.org,
        linux-media@vger.kernel.org
Subject: Re: gc2145 camera driver (front camera on PinePhone)
Message-ID: <20220822182058.xnpwfdjm7zmoid7y@core>
Mail-Followup-To: =?utf-8?Q?Ond=C5=99ej?= Jirman <megous@megous.com>,
        Pavel Machek <pavel@ucw.cz>, icenowy@aosc.xyz, martijn@brixit.nl,
        kernel list <linux-kernel@vger.kernel.org>,
        phone-devel@vger.kernel.org, mchehab@kernel.org,
        linux-media@vger.kernel.org
References: <20220822180531.GA27226@duo.ucw.cz>
MIME-Version: 1.0
Content-Type: text/plain; charset=utf-8
Content-Disposition: inline
Content-Transfer-Encoding: 8bit
In-Reply-To: <20220822180531.GA27226@duo.ucw.cz>
Precedence: bulk

Hello Pavel,

On Mon, Aug 22, 2022 at 08:05:31PM +0200, Pavel Machek wrote:
> Hi!
> 
> I'd like to mainline support for gc2145 camera on pinephone. So far I
> did trivial cleanups over the Megi's driver. I started with 5.15
> version from Mobian.  I guess yaml description of the binding needs to
> be provided, too? (Would someone have a good starting point here?)
> 
> Any hints? Are there better starting points?

Here's some reverse engineering work: https://xnux.eu/tablet-hacking/#017
that went into it, that may be helpful.

kind regards,
	o.

> Best regards,
> 							Pavel
> 
> diff --git a/drivers/media/i2c/Kconfig b/drivers/media/i2c/Kconfig
> index 6157e73eef24..ead851b93e64 100644
> --- a/drivers/media/i2c/Kconfig
> +++ b/drivers/media/i2c/Kconfig
> @@ -1390,6 +1390,15 @@ config VIDEO_S5C73M3
>  	  This is a V4L2 sensor driver for Samsung S5C73M3
>  	  8 Mpixel camera.
>  
> +config VIDEO_GC2145
> +	tristate "GalaxyCore GC2145 sensor support"
> +	depends on I2C && VIDEO_V4L2
> +	select VIDEO_V4L2_SUBDEV_API
> +	select V4L2_FWNODE
> +	help
> +	  This is a V4L2 sensor-level driver for GalaxyCore GC2145
> +	  2 Mpixel camera.
> +
>  endmenu
>  
>  menu "Lens drivers"
> diff --git a/drivers/media/i2c/Makefile b/drivers/media/i2c/Makefile
> index 83268f20aa3a..3916c2818522 100644
> --- a/drivers/media/i2c/Makefile
> +++ b/drivers/media/i2c/Makefile
> @@ -133,4 +133,5 @@ obj-$(CONFIG_VIDEO_MAX9271_LIB)	+= max9271.o
>  obj-$(CONFIG_VIDEO_RDACM20)	+= rdacm20.o
>  obj-$(CONFIG_VIDEO_RDACM21)	+= rdacm21.o
>  obj-$(CONFIG_VIDEO_ST_MIPID02) += st-mipid02.o
> +obj-$(CONFIG_VIDEO_GC2145)	+= gc2145.o
>  obj-$(CONFIG_SDR_MAX2175) += max2175.o
> diff --git a/drivers/media/i2c/gc2145.c b/drivers/media/i2c/gc2145.c
> new file mode 100644
> index 000000000000..b110bb42da37
> --- /dev/null
> +++ b/drivers/media/i2c/gc2145.c
> @@ -0,0 +1,1821 @@
> +/*
> + * Galaxycore GC2145 driver.
> + * Copyright (C) 2018 Ondřej Jirman <megi@xff.cz>.
> + *
> + * This program is free software; you can redistribute it and/or modify
> + * it under the terms of the GNU General Public License as published by
> + * the Free Software Foundation; either version 2 of the License, or
> + * (at your option) any later version.
> + */
> +
> +#define DEBUG
> +
> +#include <asm/div64.h>
> +#include <linux/clk.h>
> +#include <linux/clk-provider.h>
> +#include <linux/clkdev.h>
> +#include <linux/ctype.h>
> +#include <linux/delay.h>
> +#include <linux/device.h>
> +#include <linux/firmware.h>
> +#include <linux/i2c.h>
> +#include <linux/init.h>
> +#include <linux/module.h>
> +#include <linux/of_device.h>
> +#include <linux/slab.h>
> +#include <linux/types.h>
> +#include <linux/gpio/consumer.h>
> +#include <linux/regulator/consumer.h>
> +#include <media/v4l2-async.h>
> +#include <media/v4l2-ctrls.h>
> +#include <media/v4l2-device.h>
> +#include <media/v4l2-fwnode.h>
> +#include <media/v4l2-subdev.h>
> +
> +/*
> + * GC2145
> + * - 2M pixel
> + * - 1600 x 1200, max frame rate: 720P, 30fps@96MHz
> + * - Bayer RGB, RGB565, YCbCr 4:2:2
> + * - AE, AWB
> + * - PLL
> + * - AVDD 2.7-3V, DVDD 1.7-1.9V, IOVDD 1.7-3V
> + * - Power 180mW / 200uA standby
> + * - Interpolation, denoise, gamma, edge enhance
> + * I2C:
> + * - write reg8
> + * - read reg8
> + * - write reg8 multi
> + *
> + * Power on:
> + * MCLK on
> + * PWDN, RESET low
> + * IOVDD, AVDD, DVDD on in sequence
> + * RESET high
> + *
> + * Power off:
> + * PWDN, RESET low
> + * RESET high
> + * delay
> + * PWDN high
> + * RESET low
> + * IOVDD, AVDD, DVDD off
> + * PWDN low?
> + * MCLK off
> + *
> + * Init:
> + * - check chip id
> + * - setup pll
> + * - setup CSI interface / PAD drive strength
> + * - setup resolution/fps
> + * - enable postprocessing
> + *   (ISP related chapter)
> + *
> + * Stream on:
> + * - ???
> + */
> +
> +#define GC2145_FIRMWARE_PARAMETERS	"gc2145-init.bin"
> +
> +#define GC2145_SENSOR_WIDTH_MIN		88u
> +#define GC2145_SENSOR_HEIGHT_MIN	72u
> +
> +/* 1616x1232 8H/16V dummy pixels on each side */
> +#define GC2145_SENSOR_WIDTH_MAX		1600u
> +#define GC2145_SENSOR_HEIGHT_MAX	1200u
> +
> +/* {{{ Register definitions */
> +
> +/* system registers */
> +#define GC2145_REG_CHIP_ID			0xf0
> +#define GC2145_REG_CHIP_ID_VALUE		0x2145
> +
> +#define GC2145_REG_PAD_IO		0xf2
> +#define GC2145_REG_PLL_MODE1		0xf7
> +#define GC2145_REG_PLL_MODE2		0xf8
> +#define GC2145_REG_CM_MODE		0xf9
> +#define GC2145_REG_CLK_DIV_MODE		0xfa
> +#define GC2145_REG_ANALOG_PWC		0xfc
> +#define GC2145_REG_SCALER_MODE		0xfd
> +#define GC2145_REG_RESET		0xfe
> +
> +#define GC2145_P0_EXPOSURE_HI		0x03
> +#define GC2145_P0_EXPOSURE_LO		0x04
> +#define GC2145_P0_HBLANK_DELAY_HI	0x05
> +#define GC2145_P0_HBLANK_DELAY_LO	0x06
> +#define GC2145_P0_VBLANK_DELAY_HI	0x07
> +#define GC2145_P0_VBLANK_DELAY_LO	0x08
> +#define GC2145_P0_ROW_START_HI		0x09
> +#define GC2145_P0_ROW_START_LO		0x0a
> +#define GC2145_P0_COL_START_HI		0x0b
> +#define GC2145_P0_COL_START_LO		0x0c
> +#define GC2145_P0_WIN_HEIGHT_HI		0x0d
> +#define GC2145_P0_WIN_HEIGHT_LO		0x0e
> +#define GC2145_P0_WIN_WIDTH_HI		0x0f
> +#define GC2145_P0_WIN_WIDTH_LO		0x10
> +#define GC2145_P0_SH_DELAY_HI		0x11
> +#define GC2145_P0_SH_DELAY_LO		0x12
> +#define GC2145_P0_START_TIME		0x13
> +#define GC2145_P0_END_TIME		0x14
> +
> +#define GC2145_P0_ISP_BLK_ENABLE1	0x80
> +#define GC2145_P0_ISP_BLK_ENABLE2	0x81
> +#define GC2145_P0_ISP_BLK_ENABLE3	0x82
> +#define GC2145_P0_ISP_SPECIAL_EFFECT	0x83
> +#define GC2145_P0_ISP_OUT_FORMAT	0x84
> +#define GC2145_P0_FRAME_START		0x85
> +#define GC2145_P0_SYNC_MODE		0x86
> +#define GC2145_P0_ISP_BLK_ENABLE4	0x87
> +#define GC2145_P0_ISP_MODULE_GATING	0x88
> +#define GC2145_P0_ISP_BYPASS_MODE	0x89
> +#define GC2145_P0_DEBUG_MODE2		0x8c
> +#define GC2145_P0_DEBUG_MODE3		0x8d
> +
> +#define GC2145_P0_CROP_ENABLE		0x90
> +#define GC2145_P0_CROP_Y1_HI		0x91
> +#define GC2145_P0_CROP_Y1_LO		0x92
> +#define GC2145_P0_CROP_X1_HI		0x93
> +#define GC2145_P0_CROP_X1_LO		0x94
> +#define GC2145_P0_CROP_WIN_HEIGHT_HI	0x95
> +#define GC2145_P0_CROP_WIN_HEIGHT_LO	0x96
> +#define GC2145_P0_CROP_WIN_WIDTH_HI	0x97
> +#define GC2145_P0_CROP_WIN_WIDTH_LO	0x98
> +
> +#define GC2145_P0_SUBSAMPLE_RATIO	0x99
> +#define GC2145_P0_SUBSAMPLE_MODE	0x9a
> +#define GC2145_P0_SUB_ROW_N1		0x9b
> +#define GC2145_P0_SUB_ROW_N2		0x9c
> +#define GC2145_P0_SUB_ROW_N3		0x9d
> +#define GC2145_P0_SUB_ROW_N4		0x9e
> +#define GC2145_P0_SUB_COL_N1		0x9f
> +#define GC2145_P0_SUB_COL_N2		0xa0
> +#define GC2145_P0_SUB_COL_N3		0xa1
> +#define GC2145_P0_SUB_COL_N4		0xa2
> +#define GC2145_P0_OUT_BUF_ENABLE	0xc2
> +
> +/* }}} */
> +
> +struct gc2145_pixfmt {
> +	u32 code;
> +	u32 colorspace;
> +	u8 fmt_setup;
> +};
> +
> +static const struct gc2145_pixfmt gc2145_formats[] = {
> +	{
> +		.code              = MEDIA_BUS_FMT_UYVY8_2X8,
> +		.colorspace        = V4L2_COLORSPACE_SRGB,
> +		.fmt_setup         = 0x00,
> +	},
> +	{
> +		.code              = MEDIA_BUS_FMT_VYUY8_2X8,
> +		.colorspace        = V4L2_COLORSPACE_SRGB,
> +		.fmt_setup         = 0x01,
> +	},
> +	{
> +		.code              = MEDIA_BUS_FMT_YUYV8_2X8,
> +		.colorspace        = V4L2_COLORSPACE_SRGB,
> +		.fmt_setup         = 0x02,
> +	},
> +	{
> +		.code              = MEDIA_BUS_FMT_YVYU8_2X8,
> +		.colorspace        = V4L2_COLORSPACE_SRGB,
> +		.fmt_setup         = 0x03,
> +	},
> +	{
> +		.code              = MEDIA_BUS_FMT_RGB565_2X8_LE,
> +		.colorspace        = V4L2_COLORSPACE_SRGB,
> +		.fmt_setup         = 0x06,
> +	},
> +	{
> +		.code              = MEDIA_BUS_FMT_SBGGR8_1X8,
> +		.colorspace        = V4L2_COLORSPACE_RAW,
> +		.fmt_setup         = 0x17,
> +	},
> +};
> +
> +static const struct gc2145_pixfmt *gc2145_find_format(u32 code)
> +{
> +	int i;
> +
> +	for (i = 0; i < ARRAY_SIZE(gc2145_formats); i++)
> +		if (gc2145_formats[i].code == code)
> +			return &gc2145_formats[i];
> +
> +	return NULL;
> +}
> +
> +/* regulator supplies */
> +static const char * const gc2145_supply_name[] = {
> +	"IOVDD", /* Digital I/O (1.7-3V) suppply */
> +	"AVDD",  /* Analog (2.7-3V) supply */
> +	"DVDD",  /* Digital Core (1.7-1.9V) supply */
> +};
> +
> +#define GC2145_NUM_SUPPLIES ARRAY_SIZE(gc2145_supply_name)
> +
> +struct gc2145_ctrls {
> +	struct v4l2_ctrl_handler handler;
> +	struct {
> +		struct v4l2_ctrl *auto_exposure;
> +		struct v4l2_ctrl *exposure;
> +		struct v4l2_ctrl *d_gain;
> +		struct v4l2_ctrl *a_gain;
> +	};
> +	struct v4l2_ctrl *metering;
> +	struct v4l2_ctrl *exposure_bias;
> +	struct {
> +		struct v4l2_ctrl *wb;
> +		struct v4l2_ctrl *blue_balance;
> +		struct v4l2_ctrl *red_balance;
> +	};
> +	struct v4l2_ctrl *aaa_lock;
> +	struct v4l2_ctrl *hflip;
> +	struct v4l2_ctrl *vflip;
> +	struct v4l2_ctrl *pl_freq;
> +	struct v4l2_ctrl *colorfx;
> +	struct v4l2_ctrl *brightness;
> +	struct v4l2_ctrl *saturation;
> +	struct v4l2_ctrl *contrast;
> +	struct v4l2_ctrl *gamma;
> +	struct v4l2_ctrl *test_pattern;
> +	struct v4l2_ctrl *test_data[4];
> +};
> +
> +enum {
> +	TX_WRITE = 1,
> +	TX_WRITE16,
> +	TX_UPDATE_BITS,
> +};
> +
> +#define GC2145_MAX_OPS 64
> +
> +struct gc2145_tx_op {
> +	int op;
> +	u16 reg;
> +	u16 val;
> +	u16 mask;
> +};
> +
> +struct gc2145_dev {
> +	struct i2c_client *i2c_client;
> +	struct v4l2_subdev sd;
> +	struct media_pad pad;
> +	struct v4l2_fwnode_endpoint ep; /* the parsed DT endpoint info */
> +	struct clk *xclk; /* external clock for GC2145 */
> +
> +	struct regulator_bulk_data supplies[GC2145_NUM_SUPPLIES];
> +	struct gpio_desc *reset_gpio; /* nrst pin */
> +	struct gpio_desc *enable_gpio; /* ce pin */
> +
> +	/* lock to protect all members below */
> +	struct mutex lock;
> +
> +	struct v4l2_mbus_framefmt fmt;
> +	struct v4l2_fract frame_interval;
> +	struct gc2145_ctrls ctrls;
> +
> +	bool pending_mode_change;
> +	bool powered;
> +	bool streaming;
> +
> +	u8 current_bank;
> +
> +	struct gc2145_tx_op ops[GC2145_MAX_OPS];
> +	int n_ops;
> +	int tx_started;
> +};
> +
> +static inline struct gc2145_dev *to_gc2145_dev(struct v4l2_subdev *sd)
> +{
> +	return container_of(sd, struct gc2145_dev, sd);
> +}
> +
> +/* {{{ Register access helpers */
> +
> +static int gc2145_write_regs(struct gc2145_dev *sensor, u8 addr,
> +			     u8 *data, int data_size)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct i2c_msg msg;
> +	u8 buf[128 + 1];
> +	int ret;
> +
> +	if (data_size > sizeof(buf) - 1) {
> +		v4l2_err(&sensor->sd, "%s: oversized transfer (size=%d)\n",
> +			 __func__, data_size);
> +		return -EINVAL;
> +	}
> +
> +	buf[0] = addr;
> +	memcpy(buf + 1, data, data_size);
> +
> +	msg.addr = client->addr;
> +	msg.flags = client->flags;
> +	msg.buf = buf;
> +	msg.len = data_size + 1;
> +
> +	dev_dbg(&sensor->i2c_client->dev, "[wr %02x] <= %*ph\n",
> +		(u32)addr, data_size, data);
> +
> +	ret = i2c_transfer(client->adapter, &msg, 1);
> +	if (ret < 0) {
> +		v4l2_err(&sensor->sd,
> +			 "%s: error %d: addr=%x, data=%*ph\n",
> +			 __func__, ret, (u32)addr, data_size, data);
> +		return ret;
> +	}
> +
> +	return 0;
> +}
> +
> +static int gc2145_read_regs(struct gc2145_dev *sensor, u8 addr,
> +			    u8 *data, int data_size)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	struct i2c_msg msg[2];
> +	int ret;
> +
> +	msg[0].addr = client->addr;
> +	msg[0].flags = client->flags;
> +	msg[0].buf = &addr;
> +	msg[0].len = 1;
> +
> +	msg[1].addr = client->addr;
> +	msg[1].flags = client->flags | I2C_M_RD;
> +	msg[1].buf = data;
> +	msg[1].len = data_size;
> +
> +	ret = i2c_transfer(client->adapter, msg, 2);
> +	if (ret < 0) {
> +		v4l2_err(&sensor->sd,
> +			 "%s: error %d: start_index=%x, data_size=%d\n",
> +			 __func__, ret, (u32)addr, data_size);
> +		return ret;
> +	}
> +
> +	dev_dbg(&sensor->i2c_client->dev, "[rd %02x] => %*ph\n",
> +		(u32)addr, data_size, data);
> +
> +	return 0;
> +}
> +
> +static int gc2145_switch_bank(struct gc2145_dev *sensor, u16 reg)
> +{
> +	int ret;
> +	u8 bank = reg >> 8;
> +
> +	if (bank & ~3u)
> +		return -ERANGE;
> +
> +	if (sensor->current_bank != bank) {
> +		ret = gc2145_write_regs(sensor, GC2145_REG_RESET, &bank, 1);
> +		if (ret)
> +			return ret;
> +
> +		sensor->current_bank = bank;
> +		dev_info(&sensor->i2c_client->dev, "bank switch: 0x%02x\n",
> +				(unsigned int)sensor->current_bank);
> +	}
> +
> +	return 0;
> +}
> +
> +static int gc2145_read(struct gc2145_dev *sensor, u16 reg, u8 *val)
> +{
> +	int ret;
> +
> +	ret = gc2145_switch_bank(sensor, reg);
> +	if (ret)
> +		return ret;
> +
> +	return gc2145_read_regs(sensor, reg, val, 1);
> +}
> +
> +static int gc2145_write(struct gc2145_dev *sensor, u16 reg, u8 val)
> +{
> +	int ret;
> +
> +	ret = gc2145_switch_bank(sensor, reg);
> +	if (ret)
> +		return ret;
> +
> +	if ((reg & 0xffu) == GC2145_REG_RESET)
> +		sensor->current_bank = val & 3;
> +
> +	return gc2145_write_regs(sensor, reg, &val, 1);
> +}
> +
> +static int gc2145_update_bits(struct gc2145_dev *sensor, u16 reg, u8 mask, u8 val)
> +{
> +	int ret;
> +	u8 tmp;
> +
> +	ret = gc2145_read(sensor, reg, &tmp);
> +	if (ret)
> +		return ret;
> +
> +	tmp &= ~mask;
> +	tmp |= val & mask;
> +
> +	return gc2145_write(sensor, reg, tmp);
> +}
> +
> +static int gc2145_read16(struct gc2145_dev *sensor, u16 reg, u16 *val)
> +{
> +	int ret;
> +
> +	ret = gc2145_switch_bank(sensor, reg);
> +	if (ret)
> +		return ret;
> +
> +	ret = gc2145_read_regs(sensor, reg, (u8 *)val, sizeof(*val));
> +	if (ret)
> +		return ret;
> +
> +	*val = be16_to_cpu(*val);
> +	return 0;
> +}
> +
> +static int gc2145_write16(struct gc2145_dev *sensor, u16 reg, u16 val)
> +{
> +	u16 tmp = cpu_to_be16(val);
> +	int ret;
> +
> +	ret = gc2145_switch_bank(sensor, reg);
> +	if (ret)
> +		return ret;
> +
> +	return gc2145_write_regs(sensor, reg, (u8 *)&tmp, sizeof(tmp));
> +}
> +
> +static void gc2145_tx_start(struct gc2145_dev *sensor)
> +{
> +	if (sensor->tx_started++)
> +		dev_err(&sensor->i2c_client->dev,
> +				"tx_start called multiple times\n");
> +
> +	sensor->n_ops = 0;
> +}
> +
> +static void gc2145_tx_add(struct gc2145_dev *sensor, int kind,
> +			  u16 reg, u16 val, u16 mask)
> +{
> +	struct gc2145_tx_op *op;
> +
> +	if (!sensor->tx_started) {
> +		dev_err(&sensor->i2c_client->dev,
> +				"op added without calling tx_start\n");
> +		return;
> +	}
> +
> +	if (sensor->n_ops >= ARRAY_SIZE(sensor->ops)) {
> +		dev_err(&sensor->i2c_client->dev,
> +				"ops overflow, increase GC2145_MAX_OPS\n");
> +		return;
> +	}
> +
> +	op = &sensor->ops[sensor->n_ops++];
> +	op->op = kind;
> +	op->reg = reg;
> +	op->val = val;
> +	op->mask = mask;
> +}
> +
> +static void gc2145_tx_write8(struct gc2145_dev *sensor, u16 reg, u8 val)
> +{
> +	return gc2145_tx_add(sensor, TX_WRITE, reg, val, 0);
> +}
> +
> +static void gc2145_tx_write16(struct gc2145_dev *sensor, u16 reg, u16 val)
> +{
> +	return gc2145_tx_add(sensor, TX_WRITE16, reg, val, 0);
> +}
> +
> +static void gc2145_tx_update_bits(struct gc2145_dev *sensor, u16 reg,
> +				  u8 mask, u8 val)
> +{
> +	return gc2145_tx_add(sensor, TX_UPDATE_BITS, reg, val, mask);
> +}
> +
> +static int gc2145_tx_commit(struct gc2145_dev *sensor)
> +{
> +	struct gc2145_tx_op* op;
> +	int i, ret, n_ops;
> +
> +	if (!sensor->tx_started) {
> +		dev_err(&sensor->i2c_client->dev,
> +				"tx_commit called without tx_start\n");
> +		return 0;
> +	}
> +
> +	n_ops = sensor->n_ops;
> +	sensor->tx_started = 0;
> +	sensor->n_ops = 0;
> +
> +	for (i = 0; i < n_ops; i++) {
> +		op = &sensor->ops[i];
> +
> +		switch (op->op) {
> +		case TX_WRITE:
> +			ret = gc2145_write(sensor, op->reg, op->val);
> +			break;
> +		case TX_WRITE16:
> +			ret = gc2145_write16(sensor, op->reg, op->val);
> +			break;
> +		case TX_UPDATE_BITS:
> +			ret = gc2145_update_bits(sensor, op->reg, op->mask, op->val);
> +			break;
> +		default:
> +			dev_err(&sensor->i2c_client->dev, "invalid op at %d\n", i);
> +			ret = -EINVAL;
> +		}
> +
> +		if (ret)
> +			return ret;
> +	}
> +
> +	return 0;
> +}
> +
> +/*
> + * Efficiently write to a set of registers, using auto-increment
> + * when possible. User must not use address 0xff. To switch banks,
> + * use sequence: 0xfe, bank_no.
> + */
> +static int gc2145_set_registers(struct gc2145_dev *sensor,
> +				const uint8_t* data, size_t data_len)
> +{
> +	int ret = 0, i = 0;
> +	u16 start, len;
> +	u8 buf[128];
> +
> +	if (data_len % 2 != 0) {
> +		v4l2_err(&sensor->sd, "Register map has invalid size\n");
> +		return -EINVAL;
> +	}
> +
> +	/* we speed up communication by using auto-increment functionality */
> +	while (i < data_len) {
> +		start = data[i];
> +		len = 0;
> +
> +		while (i < data_len && data[i] == (start + len) &&
> +		       len < sizeof(buf)) {
> +			buf[len++] = data[i + 1];
> +			i += 2;
> +		}
> +
> +		ret = gc2145_write_regs(sensor, start, buf, len);
> +		if (ret)
> +			return ret;
> +	}
> +
> +	sensor->current_bank = 0xff;
> +	return 0;
> +}
> +
> +/*
> + * The firmware format:
> + * <record 0>, ..., <record N - 1>
> + * "record" is a 1-byte register address followed by 1-byte data
> + */
> +static int gc2145_load_firmware(struct gc2145_dev *sensor, const char *name)
> +{
> +	const struct firmware *fw;
> +	int ret;
> +
> +	ret = request_firmware(&fw, name, sensor->sd.v4l2_dev->dev);
> +	if (ret) {
> +		v4l2_warn(&sensor->sd,
> +			  "Failed to read firmware %s, continuing anyway...\n",
> +			  name);
> +		return 1;
> +	}
> +
> +	if (fw->size == 0)
> +		return 1;
> +
> +	ret = gc2145_set_registers(sensor, fw->data, fw->size);
> +
> +	release_firmware(fw);
> +	return ret;
> +}
> +
> +/* }}} */
> +/* {{{ Controls */
> +
> +static inline struct v4l2_subdev *ctrl_to_sd(struct v4l2_ctrl *ctrl)
> +{
> +	return &container_of(ctrl->handler, struct gc2145_dev,
> +			     ctrls.handler)->sd;
> +}
> +
> +/* Exposure */
> +
> +static int gc2145_get_exposure(struct gc2145_dev *sensor)
> +{
> +	struct gc2145_ctrls *ctrls = &sensor->ctrls;
> +	u8 again, dgain;
> +	u16 exp;
> +	int ret;
> +
> +	ret = gc2145_read(sensor, 0xb1, &again);
> +	if (ret)
> +		return ret;
> +
> +	ret = gc2145_read(sensor, 0xb2, &dgain);
> +	if (ret)
> +		return ret;
> +
> +	ret = gc2145_read16(sensor, 0x03, &exp);
> +	if (ret)
> +		return ret;
> +
> +	ctrls->exposure->val = exp;
> +	ctrls->d_gain->val = dgain;
> +	ctrls->a_gain->val = again;
> +
> +	return 0;
> +}
> +
> +#define AE_BIAS_MENU_DEFAULT_VALUE_INDEX 4
> +static const s64 ae_bias_menu_values[] = {
> +	-4000, -3000, -2000, -1000, 0, 1000, 2000, 3000, 4000
> +};
> +
> +static const s8 ae_bias_menu_reg_values[] = {
> +	0x55, 0x60, 0x65, 0x70, 0x7b, 0x85, 0x90, 0x95, 0xa0
> +};
> +
> +static int gc2145_set_exposure(struct gc2145_dev *sensor)
> +{
> +	struct gc2145_ctrls *ctrls = &sensor->ctrls;
> +	bool is_auto = (ctrls->auto_exposure->val != V4L2_EXPOSURE_MANUAL);
> +
> +	gc2145_tx_start(sensor);
> +
> +	if (ctrls->auto_exposure->is_new) {
> +		gc2145_tx_write8(sensor, 0xb6, is_auto ? 1 : 0);
> +	}
> +
> +	if (!is_auto && ctrls->exposure->is_new)
> +		gc2145_tx_write16(sensor, 0x03, ctrls->exposure->val);
> +
> +	if (!is_auto && ctrls->d_gain->is_new)
> +		gc2145_tx_write8(sensor, 0xb2, ctrls->d_gain->val);
> +
> +	if (!is_auto && ctrls->a_gain->is_new)
> +		gc2145_tx_write8(sensor, 0xb1, ctrls->a_gain->val);
> +
> +	return gc2145_tx_commit(sensor);;
> +}
> +
> +/* Test patterns */
> +
> +enum {
> +	GC2145_TEST_PATTERN_DISABLED,
> +	GC2145_TEST_PATTERN_VGA_COLOR_BARS,
> +	GC2145_TEST_PATTERN_UXGA_COLOR_BARS,
> +	GC2145_TEST_PATTERN_SKIN_MAP,
> +	GC2145_TEST_PATTERN_SOLID_COLOR,
> +};
> +
> +static const char * const test_pattern_menu[] = {
> +	"Disabled",
> +	"VGA color bars",
> +	"UXGA color bars",
> +	"Skin map",
> +	"Solid black color",
> +	"Solid light gray color",
> +	"Solid gray color",
> +	"Solid dark gray color",
> +	"Solid white color",
> +	"Solid red color",
> +	"Solid green color",
> +	"Solid blue color",
> +	"Solid yellow color",
> +	"Solid cyan color",
> +	"Solid magenta color",
> +};
> +
> +static int gc2145_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	int ret;
> +
> +	/* v4l2_ctrl_lock() locks our own mutex */
> +
> +	if (!sensor->powered)
> +		return -EIO;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		ret = gc2145_get_exposure(sensor);
> +		if (ret)
> +			return ret;
> +		break;
> +	default:
> +		dev_err(&sensor->i2c_client->dev, "getting unknown control %d\n", ctrl->id);
> +		return -EINVAL;
> +	}
> +
> +	return 0;
> +}
> +
> +static int gc2145_s_ctrl(struct v4l2_ctrl *ctrl)
> +{
> +	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	struct gc2145_ctrls *ctrls = &sensor->ctrls;
> +	s32 val = ctrl->val;
> +	unsigned int i;
> +	int ret;
> +	u8 test1, test2;
> +
> +	/* v4l2_ctrl_lock() locks our own mutex */
> +
> +	/*
> +	 * If the device is not powered up by the host driver do
> +	 * not apply any controls to H/W at this time. Instead
> +	 * the controls will be restored right after power-up.
> +	 */
> +	if (!sensor->powered)
> +		return 0;
> +
> +	switch (ctrl->id) {
> +	case V4L2_CID_EXPOSURE_AUTO:
> +		return gc2145_set_exposure(sensor);
> +
> +	case V4L2_CID_AUTO_EXPOSURE_BIAS:
> +		if (val < 0 || val >= ARRAY_SIZE(ae_bias_menu_reg_values)) {
> +			dev_err(&sensor->i2c_client->dev, "ae bias out of range\n");
> +			return -EINVAL;
> +		}
> +
> +		return gc2145_write(sensor, 0x113,
> +				    (u8)ae_bias_menu_reg_values[val]);
> +
> +	case V4L2_CID_VFLIP:
> +		return gc2145_update_bits(sensor, 0x17, BIT(1), val ? BIT(1) : 0);
> +
> +	case V4L2_CID_HFLIP:
> +		return gc2145_update_bits(sensor, 0x17, BIT(0), val ? BIT(0) : 0);
> +
> +	case V4L2_CID_TEST_PATTERN:
> +		for (i = 0; i < ARRAY_SIZE(ctrls->test_data); i++)
> +			v4l2_ctrl_activate(ctrls->test_data[i],
> +					   val == 6); /* solid color */
> +
> +		test1 = 0;
> +		test2 = 0x01;
> +
> +		if (val == GC2145_TEST_PATTERN_VGA_COLOR_BARS)
> +			test1 = 0x04;
> +		else if (val == GC2145_TEST_PATTERN_UXGA_COLOR_BARS)
> +			test1 = 0x44;
> +		else if (val == GC2145_TEST_PATTERN_SKIN_MAP)
> +			test1 = 0x10;
> +		else if (val >= GC2145_TEST_PATTERN_SOLID_COLOR) {
> +			test1 = 0x04;
> +			test2 = ((val - GC2145_TEST_PATTERN_SOLID_COLOR) << 4) | 0x8;
> +		} else if (val != GC2145_TEST_PATTERN_DISABLED) {
> +			dev_err(&sensor->i2c_client->dev, "test pattern out of range\n");
> +			return -EINVAL;
> +		}
> +
> +		ret = gc2145_write(sensor, 0x8c, test1);
> +		if (ret)
> +			return ret;
> +
> +		return gc2145_write(sensor, 0x8d, test2);
> +
> +	default:
> +		dev_err(&sensor->i2c_client->dev, "setting unknown control %d\n", ctrl->id);
> +		return -EINVAL;
> +	}
> +}
> +
> +static const struct v4l2_ctrl_ops gc2145_ctrl_ops = {
> +	.g_volatile_ctrl = gc2145_g_volatile_ctrl,
> +	.s_ctrl = gc2145_s_ctrl,
> +};
> +
> +static int gc2145_init_controls(struct gc2145_dev *sensor)
> +{
> +	const struct v4l2_ctrl_ops *ops = &gc2145_ctrl_ops;
> +	struct gc2145_ctrls *ctrls = &sensor->ctrls;
> +	struct v4l2_ctrl_handler *hdl = &ctrls->handler;
> +	int ret;
> +
> +	v4l2_ctrl_handler_init(hdl, 32);
> +
> +	/* we can use our own mutex for the ctrl lock */
> +	hdl->lock = &sensor->lock;
> +
> +	/* Exposure controls */
> +	ctrls->auto_exposure = v4l2_ctrl_new_std_menu(hdl, ops,
> +						      V4L2_CID_EXPOSURE_AUTO,
> +						      V4L2_EXPOSURE_MANUAL, 0,
> +						      V4L2_EXPOSURE_AUTO);
> +	ctrls->exposure = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_EXPOSURE,
> +					    1, 0x1fff, 1, 0x80);
> +	ctrls->a_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_ANALOGUE_GAIN,
> +					  0, 255, 1, 0x20);
> +	ctrls->d_gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_DIGITAL_GAIN,
> +					  0, 255, 1, 0x40);
> +	ctrls->exposure_bias =
> +		v4l2_ctrl_new_int_menu(hdl, ops, V4L2_CID_AUTO_EXPOSURE_BIAS,
> +				       ARRAY_SIZE(ae_bias_menu_values) - 1,
> +				       AE_BIAS_MENU_DEFAULT_VALUE_INDEX,
> +				       ae_bias_menu_values);
> +
> +	/* V/H flips */
> +	ctrls->hflip = v4l2_ctrl_new_std(hdl, ops,
> +					 V4L2_CID_HFLIP, 0, 1, 1, 0);
> +	ctrls->vflip = v4l2_ctrl_new_std(hdl, ops,
> +					 V4L2_CID_VFLIP, 0, 1, 1, 0);
> +
> +
> +	/* Test patterns */
> +	ctrls->test_pattern =
> +		v4l2_ctrl_new_std_menu_items(hdl, ops, V4L2_CID_TEST_PATTERN,
> +					     ARRAY_SIZE(test_pattern_menu) - 1,
> +					     0, 0, test_pattern_menu);
> +
> +	v4l2_ctrl_auto_cluster(4, &ctrls->auto_exposure, V4L2_EXPOSURE_MANUAL,
> +			       true);
> +
> +	if (hdl->error) {
> +		ret = hdl->error;
> +		goto free_ctrls;
> +	}
> +
> +	sensor->sd.ctrl_handler = hdl;
> +	return 0;
> +
> +free_ctrls:
> +	v4l2_ctrl_handler_free(hdl);
> +	return ret;
> +}
> +
> +/* }}} */
> +/* {{{ Video ops */
> +
> +static int gc2145_g_frame_interval(struct v4l2_subdev *sd,
> +				   struct v4l2_subdev_frame_interval *fi)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +
> +	if (fi->pad != 0)
> +		return -EINVAL;
> +
> +	mutex_lock(&sensor->lock);
> +	fi->interval = sensor->frame_interval;
> +	mutex_unlock(&sensor->lock);
> +
> +	return 0;
> +}
> +
> +static int gc2145_s_frame_interval(struct v4l2_subdev *sd,
> +				   struct v4l2_subdev_frame_interval *fi)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	int ret = 0, fps;
> +
> +	if (fi->pad != 0)
> +		return -EINVAL;
> +
> +	mutex_lock(&sensor->lock);
> +
> +	/* user requested infinite frame rate */
> +	if (fi->interval.numerator == 0)
> +		fps = 60;
> +	else
> +		fps = DIV_ROUND_CLOSEST(fi->interval.denominator,
> +					fi->interval.numerator);
> +
> +	fps = clamp(fps, 1, 60);
> +
> +	sensor->frame_interval.numerator = 1;
> +	sensor->frame_interval.denominator = fps;
> +	fi->interval = sensor->frame_interval;
> +
> +	mutex_unlock(&sensor->lock);
> +	return ret;
> +}
> +
> +static int gc2145_set_2pclk(struct gc2145_dev *sensor,
> +			    unsigned long *freq, bool apply)
> +{
> +	unsigned long pll_mult, pll_mult_max, pclk_div, pclk2,
> +		      mclk;
> +	unsigned long pll_mult_best = 0, pclk_div_best = 0, diff_best = ULONG_MAX, diff,
> +		      pclk2_best = 0;
> +	int mclk_div2_en;
> +	int mclk_div2_en_best = 0;
> +
> +	mclk = clk_get_rate(sensor->xclk);
> +	if (mclk == 0)
> +		return -EINVAL;
> +
> +        for (mclk_div2_en = 0; mclk_div2_en <= 1; mclk_div2_en++) {
> +		pll_mult_max = 768000000 / 4 / (mclk / (mclk_div2_en ? 2 : 1));
> +		if (pll_mult_max > 32)
> +			pll_mult_max = 32;
> +
> +		for (pll_mult = 2; pll_mult <= pll_mult_max; pll_mult++) {
> +			for (pclk_div = 1; pclk_div <= 8; pclk_div++) {
> +				pclk2 = mclk / (mclk_div2_en ? 2 : 1) * pll_mult / pclk_div;
> +
> +				if (pclk2 > *freq)
> +					continue;
> +
> +				diff = *freq - pclk2;
> +
> +				if (diff < diff_best) {
> +					diff_best = diff;
> +					pclk2_best = pclk2;
> +
> +					pll_mult_best = pll_mult;
> +					pclk_div_best = pclk_div;
> +					mclk_div2_en_best = mclk_div2_en;
> +				}
> +
> +				if (diff == 0)
> +					goto found;
> +			}
> +		}
> +	}
> +
> +	if (diff_best == ULONG_MAX)
> +		return -1;
> +
> +found:
> +	*freq = pclk2_best;
> +	if (!apply)
> +		return 0;
> +
> +	gc2145_tx_start(sensor);
> +
> +	gc2145_tx_write8(sensor, 0xf7,
> +			 ((pclk_div_best - 1)) << 4 |
> +			 (mclk_div2_en_best << 1) | BIT(0) /* pll_en */);
> +	gc2145_tx_write8(sensor, 0xf8, BIT(7) | (pll_mult_best - 1));
> +	gc2145_tx_write8(sensor, 0xfa,
> +			 (pclk_div_best - 1) << 4 |
> +			 (((pclk_div_best - 1) / 2) & 0xf));
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +static int gc2145_setup_awb(struct gc2145_dev *sensor,
> +			     u16 x1, u16 y1, u16 x2, u16 y2)
> +{
> +	int ratio = 8;
> +
> +	gc2145_tx_start(sensor);
> +
> +	/* disable awb */
> +	gc2145_tx_update_bits(sensor, 0x82, BIT(1), 0);
> +
> +	/* reset white balance RGB gains */
> +	gc2145_tx_write8(sensor, 0xb3, 0x40);
> +	gc2145_tx_write8(sensor, 0xb4, 0x40);
> +	gc2145_tx_write8(sensor, 0xb5, 0x40);
> +
> +	/* awb window */
> +	gc2145_tx_write8(sensor, 0x1ec, x1 / ratio);
> +	gc2145_tx_write8(sensor, 0x1ed, y1 / ratio);
> +	gc2145_tx_write8(sensor, 0x1ee, x2 / ratio);
> +	gc2145_tx_write8(sensor, 0x1ef, y2 / ratio);
> +
> +	/* eanble awb */
> +	gc2145_tx_update_bits(sensor, 0x82, BIT(1), BIT(1));
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +static int gc2145_setup_aec(struct gc2145_dev *sensor,
> +			     u16 x1, u16 y1, u16 x2, u16 y2,
> +			     u16 cx1, u16 cy1, u16 cx2, u16 cy2)
> +{
> +        u16 x_ratio = 8;
> +
> +	gc2145_tx_start(sensor);
> +
> +	/* disable AEC */
> +	gc2145_tx_write8(sensor, 0xb6, 0);
> +
> +	/* set reasonable initial exposure and gains */
> +	gc2145_tx_write16(sensor, 0x03, 1200);
> +	gc2145_tx_write8(sensor, 0xb1, 0x20);
> +	gc2145_tx_write8(sensor, 0xb2, 0xe0);
> +
> +	/* setup measure window */
> +	gc2145_tx_write8(sensor, 0x101, x1 / x_ratio);
> +	gc2145_tx_write8(sensor, 0x102, x2 / x_ratio);
> +	gc2145_tx_write8(sensor, 0x103, y1 / 8);
> +	gc2145_tx_write8(sensor, 0x104, y2 / 8);
> +
> +	/* setup center */
> +	gc2145_tx_write8(sensor, 0x105, cx1 / x_ratio);
> +	gc2145_tx_write8(sensor, 0x106, cx2 / x_ratio);
> +	gc2145_tx_write8(sensor, 0x107, cy1 / 8);
> +	gc2145_tx_write8(sensor, 0x108, cy2 / 8);
> +
> +	/* enable AEC again */
> +	gc2145_tx_write8(sensor, 0xb6, 1);
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +struct gc2145_sensor_params {
> +	unsigned int enable_scaler;
> +	unsigned int col_scaler_only;
> +	unsigned int row_skip;
> +	unsigned int col_skip;
> +	unsigned long sh_delay;
> +	unsigned long hb;
> +	unsigned long vb;
> +	unsigned long st;
> +	unsigned long et;
> +	unsigned long win_width;
> +	unsigned long win_height;
> +	unsigned long width;
> +	unsigned long height;
> +};
> +
> +static void gc2145_sensor_params_init(struct gc2145_sensor_params* p, int width, int height)
> +{
> +	p->win_height = height + 32;
> +	p->win_width = (width + 16);
> +	p->width = width;
> +	p->height = height;
> +	p->st = 2;
> +	p->et = 2;
> +	p->vb = 8;
> +	p->hb = 0x1f0;
> +	p->sh_delay = 30;
> +}
> +
> +/* unit is PCLK periods */
> +static unsigned long
> +gc2145_sensor_params_get_row_period(struct gc2145_sensor_params* p)
> +{
> +	return 2 * (p->win_width / 2 / (p->col_skip + 1) + p->sh_delay + p->hb + 4);
> +}
> +
> +static unsigned long
> +gc2145_sensor_params_get_frame_period(struct gc2145_sensor_params* p)
> +{
> +	unsigned long rt = gc2145_sensor_params_get_row_period(p);
> +
> +	return rt * (p->vb + p->win_height) / (p->row_skip + 1);
> +}
> +
> +static void
> +gc2145_sensor_params_fit_hb_to_power_line_period(struct gc2145_sensor_params* p,
> +					  unsigned long power_line_freq,
> +					  unsigned long pclk)
> +{
> +	unsigned long rt, power_line_ratio;
> +
> +        for (p->hb = 0x1f0; p->hb < 2047; p->hb++) {
> +		rt = gc2145_sensor_params_get_row_period(p);
> +
> +		/* power_line_ratio is row_freq / power_line_freq * 1000 */
> +                power_line_ratio = pclk / power_line_freq * 1000 / rt;
> +
> +		/* if we're close enough, stop the search */
> +                if (power_line_ratio % 1000 < 50)
> +                        break;
> +        }
> +
> +	/* finding the optimal Hb is not critical */
> +	if (p->hb == 2047)
> +		p->hb = 0x1f0;
> +}
> +
> +static void
> +gc2145_sensor_params_fit_vb_to_frame_period(struct gc2145_sensor_params* p,
> +				     unsigned long frame_period)
> +{
> +	unsigned long rt, fp;
> +
> +	p->vb = 8;
> +	rt = gc2145_sensor_params_get_row_period(p);
> +	fp = gc2145_sensor_params_get_frame_period(p);
> +
> +	if (frame_period > fp)
> +		p->vb = frame_period * (p->row_skip + 1) / rt - p->win_height;
> +
> +	if (p->vb > 4095)
> +		p->vb = 4095;
> +}
> +
> +static int gc2145_sensor_params_apply(struct gc2145_dev *sensor,
> +				      struct gc2145_sensor_params* p)
> +{
> +	u32 off_x = (GC2145_SENSOR_WIDTH_MAX - p->width) / 2;
> +	u32 off_y = (GC2145_SENSOR_HEIGHT_MAX - p->height) / 2;
> +
> +	gc2145_tx_start(sensor);
> +
> +	gc2145_tx_write8(sensor, 0xfd, (p->enable_scaler ? BIT(0) : 0)
> +			| (p->col_scaler_only ? BIT(1) : 0));
> +
> +	gc2145_tx_write8(sensor, 0x18, 0x0a
> +		       | (p->col_skip ? BIT(7) : 0)
> +		       | (p->row_skip ? BIT(6) : 0));
> +
> +	gc2145_tx_write16(sensor, 0x09, off_y);
> +	gc2145_tx_write16(sensor, 0x0b, off_x);
> +	gc2145_tx_write16(sensor, 0x0d, p->win_height);
> +	gc2145_tx_write16(sensor, 0x0f, p->win_width);
> +	gc2145_tx_write16(sensor, 0x05, p->hb);
> +	gc2145_tx_write16(sensor, 0x07, p->vb);
> +	gc2145_tx_write16(sensor, 0x11, p->sh_delay);
> +
> +	gc2145_tx_write8(sensor, 0x13, p->st);
> +	gc2145_tx_write8(sensor, 0x14, p->et);
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +static int gc2145_setup_mode(struct gc2145_dev *sensor)
> +{
> +	int scaling_desired, ret, pad, i;
> +	struct gc2145_sensor_params params = {0};
> +	unsigned long pclk2, frame_period;
> +	unsigned long power_line_freq = 50;
> +	unsigned long width = sensor->fmt.width;
> +	unsigned long height = sensor->fmt.height;
> +	unsigned long framerate = sensor->frame_interval.denominator;
> +	const struct gc2145_pixfmt *pix_fmt;
> +	unsigned long rt, ft, ft_rt;
> +
> +	pix_fmt = gc2145_find_format(sensor->fmt.code);
> +	if (!pix_fmt) {
> +		dev_err(&sensor->i2c_client->dev,
> +			"pixel format not supported %u\n", sensor->fmt.code);
> +		return -EINVAL;
> +	}
> +
> +        /*
> +	 * Equations for calculating framerate are:
> +	 *
> +	 *    ww = width + 16
> +	 *    wh = height + 32
> +	 *    Rt = (ww / 2 / (col_skip + 1) + sh_delay + Hb + 4)
> +	 *    Ft = Rt * (Vb + wh) / (row_skip + 1)
> +	 *    framerate = 2pclk / 4 / Ft
> +	 *
> +	 * Based on these equations:
> +	 *
> +	 * 1) First we need to determine what 2PCLK frequency to use. The 2PCLK
> +	 *    frequency is not arbitrarily precise, so we need to calculate the
> +	 *    actual frequency used, after setting our target frequency.
> +	 *
> +	 *    We use a simple heuristic:
> +	 *
> +	 *      If pixel_count * 2 * framerate * 1.15 is > 40MHz, we use 60MHz,
> +	 *      otherwise we use 40MHz.
> +	 *
> +	 * 2) We want to determine lowest Hb that we can use to extend row
> +	 *    period so that row time takes an integer fraction of the power
> +	 *    line frequency period. Minimum Hb is 0x1f0.
> +	 *
> +	 * 3) If the requested resolution is less than half the sensor's size,
> +	 *    we'll use scaling, or row skipping + column scaling, or row and
> +	 *    column skiping, depending on what allows us to achieve the
> +	 *    requested framerate.
> +         *
> +	 * 4) We use the selected Hb to calculate Vb value that will give
> +	 *    us the desired framerate, given the scaling/skipping option
> +	 *    selected in 3).
> +	 */
> +
> +	scaling_desired = width <= GC2145_SENSOR_WIDTH_MAX / 2
> +			&& height <= GC2145_SENSOR_HEIGHT_MAX / 2;
> +
> +	pclk2 = 60000000;
> +
> +	ret = gc2145_set_2pclk(sensor, &pclk2, false);
> +	if (ret < 0)
> +		return ret;
> +
> +	gc2145_sensor_params_init(&params, width, height);
> +
> +	/*
> +	 * if the resolution is < half the sensor size, enable the scaler
> +	 * to cover more area of the chip
> +	 */
> +	if (scaling_desired) {
> +		params.enable_scaler = 1;
> +		pclk2 *= 2;
> +		gc2145_sensor_params_init(&params, width * 2, height * 2);
> +	}
> +
> +	/* we need to call this each time pclk or power_line_freq is changed */
> +	gc2145_sensor_params_fit_hb_to_power_line_period(&params,
> +							 power_line_freq,
> +							 pclk2 / 2);
> +
> +	frame_period = gc2145_sensor_params_get_frame_period(&params);
> +	if (framerate <= pclk2 / 2 / frame_period)
> +		goto apply;
> +
> +	if (scaling_desired) {
> +		/* try using just the column scaler + row skip */
> +		params.col_scaler_only = 1;
> +		params.row_skip = 1;
> +		gc2145_sensor_params_fit_hb_to_power_line_period(&params,
> +								 power_line_freq,
> +								 pclk2 / 2);
> +
> +		frame_period = gc2145_sensor_params_get_frame_period(&params);
> +		if (framerate <= pclk2 / 2 / frame_period)
> +			goto apply;
> +	}
> +
> +apply:
> +        /* adjust vb to fit the target framerate */
> +	gc2145_sensor_params_fit_vb_to_frame_period(&params,
> +						    pclk2 / 2 / framerate);
> +
> +	gc2145_sensor_params_apply(sensor, &params);
> +
> +	ret = gc2145_set_2pclk(sensor, &pclk2, true);
> +	if (ret < 0)
> +		return ret;
> +
> +	pad = (width > 256 && height > 256) ? 32 : 16;
> +
> +	ret = gc2145_setup_awb(sensor, pad, pad, width - pad * 2, height - pad * 2);
> +	if (ret)
> +		return ret;
> +
> +	ret = gc2145_setup_aec(sensor,
> +				pad, pad, width - pad * 2, height - pad * 2,
> +				2 * pad, 2 * pad, width - pad * 4, height - pad * 4);
> +	if (ret)
> +		return ret;
> +
> +	gc2145_tx_start(sensor);
> +
> +	rt = gc2145_sensor_params_get_row_period(&params);
> +	ft = gc2145_sensor_params_get_frame_period(&params);
> +	ft_rt = ft / rt / 4;
> +
> +	for (i = 0; i < 7; i++) {
> +		/* exposure settings for exposure levels */
> +		gc2145_tx_write16(sensor, 0x127 + 2 * i, ft_rt * (i + 1));
> +		/* max dg gains */
> +		gc2145_tx_write8(sensor, 0x135 + i, 0x50);
> +	}
> +
> +	/*  max analog gain */
> +	gc2145_tx_write8(sensor, 0x11f, 0x50);
> +	/*  max digital gain */
> +	gc2145_tx_write8(sensor, 0x120, 0xe0);
> +
> +	gc2145_tx_write8(sensor, GC2145_P0_ISP_OUT_FORMAT, pix_fmt->fmt_setup);
> +
> +	/*  set gamma curve */
> +	gc2145_tx_update_bits(sensor, 0x80, BIT(6), BIT(6));
> +
> +	/*  disable denoising */
> +	gc2145_tx_update_bits(sensor, 0x80, BIT(2), 0);
> +
> +	/*  drive strength */
> +	gc2145_tx_write8(sensor, 0x24,
> +			 (pclk2 / (params.enable_scaler + 1)) > 40000000 ?
> +				0xff : 0x55);
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +static int gc2145_set_stream(struct gc2145_dev *sensor, int enable)
> +{
> +	gc2145_tx_start(sensor);
> +	gc2145_tx_write8(sensor, GC2145_REG_PAD_IO, enable ? 0x0f : 0);
> +
> +	return gc2145_tx_commit(sensor);
> +}
> +
> +static int gc2145_s_stream(struct v4l2_subdev *sd, int enable)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	int ret = 0;
> +
> +	mutex_lock(&sensor->lock);
> +
> +	if (sensor->streaming == !enable) {
> +		if (enable && sensor->pending_mode_change) {
> +			ret = gc2145_setup_mode(sensor);
> +			if (ret)
> +				goto out;
> +		}
> +
> +		ret = gc2145_set_stream(sensor, enable);
> +		if (ret)
> +			goto out;
> +
> +		sensor->streaming = !!enable;
> +	}
> +
> +out:
> +	mutex_unlock(&sensor->lock);
> +	return ret;
> +}
> +
> +/* }}} */
> +/* {{{ Pad ops */
> +
> +static int gc2145_enum_mbus_code(struct v4l2_subdev *sd,
> +				 struct v4l2_subdev_state *sd_state,
> +				 struct v4l2_subdev_mbus_code_enum *code)
> +{
> +	if (code->pad != 0 || code->index >= ARRAY_SIZE(gc2145_formats))
> +		return -EINVAL;
> +
> +	code->code = gc2145_formats[code->index].code;
> +
> +	return 0;
> +}
> +
> +static int gc2145_enum_frame_size(struct v4l2_subdev *sd,
> +				  struct v4l2_subdev_state *sd_state,
> +				  struct v4l2_subdev_frame_size_enum *fse)
> +{
> +	if (fse->pad != 0 || fse->index > 0)
> +		return -EINVAL;
> +
> +	fse->min_width = GC2145_SENSOR_WIDTH_MIN;
> +	fse->max_width = GC2145_SENSOR_WIDTH_MAX;
> +
> +	fse->min_height = GC2145_SENSOR_HEIGHT_MIN;
> +	fse->max_height = GC2145_SENSOR_HEIGHT_MAX;
> +
> +	return 0;
> +}
> +
> +static int gc2145_enum_frame_interval(
> +	struct v4l2_subdev *sd,
> +	struct v4l2_subdev_state *sd_state,
> +	struct v4l2_subdev_frame_interval_enum *fie)
> +{
> +	if (fie->pad != 0 || fie->index > 0)
> +		return -EINVAL;
> +
> +	fie->interval.numerator = 1;
> +	fie->interval.denominator = 30;
> +
> +	return 0;
> +}
> +
> +static int gc2145_get_fmt(struct v4l2_subdev *sd,
> +			  struct v4l2_subdev_state *sd_state,
> +			  struct v4l2_subdev_format *format)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	struct v4l2_mbus_framefmt *mf;
> +
> +	if (format->pad != 0)
> +		return -EINVAL;
> +
> +	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
> +		mf = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
> +		format->format = *mf;
> +		return 0;
> +	}
> +
> +	mutex_lock(&sensor->lock);
> +	format->format = sensor->fmt;
> +	mutex_unlock(&sensor->lock);
> +
> +	return 0;
> +}
> +
> +static int gc2145_set_fmt(struct v4l2_subdev *sd,
> +			  struct v4l2_subdev_state *sd_state,
> +			  struct v4l2_subdev_format *format)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	struct v4l2_mbus_framefmt *mf = &format->format;
> +	const struct gc2145_pixfmt *pixfmt;
> +	int ret = 0;
> +
> +	if (format->pad != 0)
> +		return -EINVAL;
> +
> +	/* check if we support requested mbus fmt */
> +	pixfmt = gc2145_find_format(mf->code);
> +	if (!pixfmt)
> +		pixfmt = &gc2145_formats[0];
> +
> +	mf->code = pixfmt->code;
> +	mf->colorspace = pixfmt->colorspace;
> +	mf->xfer_func = V4L2_XFER_FUNC_DEFAULT;
> +	mf->ycbcr_enc = V4L2_YCBCR_ENC_DEFAULT;
> +	mf->quantization = V4L2_QUANTIZATION_DEFAULT;
> +	mf->field = V4L2_FIELD_NONE;
> +
> +	mutex_lock(&sensor->lock);
> +
> +	mf->width = clamp(mf->width, GC2145_SENSOR_WIDTH_MIN,
> +		      GC2145_SENSOR_WIDTH_MAX);
> +	mf->height = clamp(mf->height, GC2145_SENSOR_HEIGHT_MIN,
> +		       GC2145_SENSOR_HEIGHT_MAX);
> +
> +	if (format->which == V4L2_SUBDEV_FORMAT_TRY) {
> +		struct v4l2_mbus_framefmt *try_mf;
> +
> +		try_mf = v4l2_subdev_get_try_format(sd, sd_state, format->pad);
> +		*try_mf = *mf;
> +		goto out;
> +	}
> +
> +	if (sensor->streaming) {
> +		ret = -EBUSY;
> +		goto out;
> +	}
> +
> +	sensor->fmt = *mf;
> +	sensor->pending_mode_change = true;
> +out:
> +	mutex_unlock(&sensor->lock);
> +	return ret;
> +}
> +
> +/* }}} */
> +/* {{{ Core Ops */
> +
> +static int gc2145_configure(struct gc2145_dev *sensor)
> +{
> +	struct v4l2_fwnode_bus_parallel *bus = &sensor->ep.bus.parallel;
> +	u8 sync_mode = 0;
> +	u16 chip_id;
> +	int ret;
> +
> +	ret = gc2145_read16(sensor, GC2145_REG_CHIP_ID, &chip_id);
> +	if (ret)
> +		return ret;
> +
> +	dev_info(&sensor->i2c_client->dev, "device id: 0x%04x\n",
> +		 (unsigned int)chip_id);
> +
> +	if (chip_id != GC2145_REG_CHIP_ID_VALUE) {
> +		dev_err(&sensor->i2c_client->dev,
> +			"unsupported device id: 0x%04x\n",
> +			(unsigned int)chip_id);
> +		return -EINVAL;
> +	}
> +
> +        /*  setup parallel bus */
> +
> +	if (bus->flags & V4L2_MBUS_VSYNC_ACTIVE_LOW)
> +		sync_mode |= 0x01;
> +
> +	if (bus->flags & V4L2_MBUS_HSYNC_ACTIVE_LOW)
> +		sync_mode |= 0x02;
> +
> +	if (bus->flags & V4L2_MBUS_PCLK_SAMPLE_FALLING)
> +		sync_mode |= 0x04;
> +
> +	gc2145_tx_start(sensor);
> +
> +	/* soft reset */
> +	gc2145_tx_write8(sensor, GC2145_REG_RESET, 0xf0);
> +
> +	/* enable analog/digital parts */
> +	gc2145_tx_write8(sensor, GC2145_REG_ANALOG_PWC, 0x06);
> +
> +	/* safe initial PLL setting */
> +	gc2145_tx_write8(sensor, GC2145_REG_PLL_MODE1, 0x1d);
> +	gc2145_tx_write8(sensor, GC2145_REG_PLL_MODE2, 0x84);
> +	gc2145_tx_write8(sensor, GC2145_REG_CLK_DIV_MODE, 0x00);
> +
> +	gc2145_tx_write8(sensor, GC2145_REG_CM_MODE, 0xfe);
> +
> +	/* disable pads */
> +	gc2145_tx_write8(sensor, GC2145_REG_PAD_IO, 0);
> +
> +	gc2145_tx_write8(sensor, 0x19, 0x0c); /* set AD pipe number */
> +	gc2145_tx_write8(sensor, 0x20, 0x01); /* AD clk mode */
> +
> +	/* enable defect correction, etc. */
> +	gc2145_tx_write8(sensor, 0x80, 0x0b);
> +
> +	gc2145_tx_write8(sensor, GC2145_P0_SYNC_MODE, sync_mode);
> +
> +	ret = gc2145_tx_commit(sensor);
> +	if (ret)
> +		return ret;
> +
> +	/* load default register values from the firmware file */
> +	ret = gc2145_load_firmware(sensor, GC2145_FIRMWARE_PARAMETERS);
> +	if (ret < 0)
> +		return ret;
> +
> +	return 0;
> +}
> +
> +static int gc2145_set_power(struct gc2145_dev *sensor, bool on)
> +{
> +	int ret = 0;
> +
> +	if (on) {
> +		ret = regulator_bulk_enable(GC2145_NUM_SUPPLIES,
> +					    sensor->supplies);
> +		if (ret)
> +			return ret;
> +
> +		ret = clk_set_rate(sensor->xclk, 24000000);
> +		if (ret)
> +			goto xclk_off;
> +
> +		ret = clk_prepare_enable(sensor->xclk);
> +		if (ret)
> +			goto power_off;
> +
> +		usleep_range(10000, 12000);
> +		gpiod_direction_output(sensor->reset_gpio, 1);
> +		usleep_range(10000, 12000);
> +		gpiod_direction_output(sensor->enable_gpio, 1);
> +		usleep_range(10000, 12000);
> +		gpiod_direction_output(sensor->reset_gpio, 0);
> +		usleep_range(40000, 50000);
> +
> +		ret = gc2145_configure(sensor);
> +		if (ret)
> +			goto power_off;
> +
> +		ret = gc2145_setup_mode(sensor);
> +		if (ret)
> +			goto power_off;
> +
> +		return 0;
> +	}
> +
> +xclk_off:
> +	clk_disable_unprepare(sensor->xclk);
> +power_off:
> +	gpiod_direction_input(sensor->reset_gpio);
> +	gpiod_direction_input(sensor->enable_gpio);
> +	regulator_bulk_disable(GC2145_NUM_SUPPLIES, sensor->supplies);
> +	msleep(100);
> +	return ret;
> +}
> +
> +static int gc2145_s_power(struct v4l2_subdev *sd, int on)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	bool power_up, power_down;
> +	int ret = 0;
> +
> +	mutex_lock(&sensor->lock);
> +
> +	power_up = on && !sensor->powered;
> +	power_down = !on && sensor->powered;
> +
> +	if (power_up || power_down) {
> +		ret = gc2145_set_power(sensor, power_up);
> +		if (!ret)
> +			sensor->powered = on;
> +	}
> +
> +	mutex_unlock(&sensor->lock);
> +
> +	if (!ret && power_up) {
> +		/* restore controls */
> +		ret = v4l2_ctrl_handler_setup(&sensor->ctrls.handler);
> +		if (ret)
> +			gc2145_s_power(sd, 0);
> +	}
> +
> +	return ret;
> +}
> +
> +#ifdef CONFIG_VIDEO_ADV_DEBUG
> +static int gc2145_g_register(struct v4l2_subdev *sd,
> +			     struct v4l2_dbg_register *reg)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	int ret;
> +	u8 val = 0;
> +
> +	if (reg->reg > 0xffff)
> +		return -EINVAL;
> +
> +	reg->size = 1;
> +
> +	mutex_lock(&sensor->lock);
> +	ret = gc2145_read(sensor, reg->reg, &val);
> +	mutex_unlock(&sensor->lock);
> +	if (ret)
> +		return -EIO;
> +
> +	reg->val = val;
> +	return 0;
> +}
> +
> +static int gc2145_s_register(struct v4l2_subdev *sd,
> +			     const struct v4l2_dbg_register *reg)
> +{
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +	int ret;
> +
> +	if (reg->reg > 0xffff || reg->val > 0xff)
> +		return -EINVAL;
> +
> +	mutex_lock(&sensor->lock);
> +	ret = gc2145_write(sensor, reg->reg, reg->val);
> +	mutex_unlock(&sensor->lock);
> +
> +	return ret;
> +}
> +#endif
> +
> +/* }}} */
> +
> +static const struct v4l2_subdev_core_ops gc2145_core_ops = {
> +	.s_power = gc2145_s_power,
> +#ifdef CONFIG_VIDEO_ADV_DEBUG
> +	.g_register = gc2145_g_register,
> +	.s_register = gc2145_s_register,
> +#endif
> +};
> +
> +static const struct v4l2_subdev_pad_ops gc2145_pad_ops = {
> +	.enum_mbus_code = gc2145_enum_mbus_code,
> +	.enum_frame_size = gc2145_enum_frame_size,
> +	.enum_frame_interval = gc2145_enum_frame_interval,
> +	.get_fmt = gc2145_get_fmt,
> +	.set_fmt = gc2145_set_fmt,
> +};
> +
> +static const struct v4l2_subdev_video_ops gc2145_video_ops = {
> +	.g_frame_interval = gc2145_g_frame_interval,
> +	.s_frame_interval = gc2145_s_frame_interval,
> +	.s_stream = gc2145_s_stream,
> +};
> +
> +static const struct v4l2_subdev_ops gc2145_subdev_ops = {
> +	.core = &gc2145_core_ops,
> +	.pad = &gc2145_pad_ops,
> +	.video = &gc2145_video_ops,
> +};
> +
> +static int gc2145_get_regulators(struct gc2145_dev *sensor)
> +{
> +	int i;
> +
> +	for (i = 0; i < GC2145_NUM_SUPPLIES; i++)
> +		sensor->supplies[i].supply = gc2145_supply_name[i];
> +
> +	return devm_regulator_bulk_get(&sensor->i2c_client->dev,
> +				       GC2145_NUM_SUPPLIES,
> +				       sensor->supplies);
> +}
> +
> +static int gc2145_probe(struct i2c_client *client,
> +			const struct i2c_device_id *id)
> +{
> +	struct device *dev = &client->dev;
> +	struct fwnode_handle *endpoint;
> +	struct gc2145_dev *sensor;
> +	int ret;
> +
> +	sensor = devm_kzalloc(dev, sizeof(*sensor), GFP_KERNEL);
> +	if (!sensor)
> +		return -ENOMEM;
> +
> +	sensor->i2c_client = client;
> +
> +	sensor->fmt.code = gc2145_formats[0].code;
> +	sensor->fmt.width = 1600;
> +	sensor->fmt.height = 1200;
> +	sensor->fmt.field = V4L2_FIELD_NONE;
> +	sensor->frame_interval.numerator = 1;
> +	sensor->frame_interval.denominator = 10;
> +	sensor->pending_mode_change = true;
> +	sensor->current_bank = 0xff;
> +
> +	endpoint = fwnode_graph_get_next_endpoint(
> +		of_fwnode_handle(client->dev.of_node), NULL);
> +	if (!endpoint) {
> +		dev_err(dev, "endpoint node not found\n");
> +		return -EINVAL;
> +	}
> +
> +	ret = v4l2_fwnode_endpoint_parse(endpoint, &sensor->ep);
> +	fwnode_handle_put(endpoint);
> +	if (ret) {
> +		dev_err(dev, "could not parse endpoint\n");
> +		return ret;
> +	}
> +
> +	if (sensor->ep.bus_type != V4L2_MBUS_PARALLEL) {
> +		dev_err(dev, "unsupported bus type %d\n", sensor->ep.bus_type);
> +		return -EINVAL;
> +	}
> +
> +	sensor->xclk = devm_clk_get(dev, "xclk");
> +	if (IS_ERR(sensor->xclk)) {
> +		dev_err(dev, "failed to get xclk\n");
> +		return PTR_ERR(sensor->xclk);
> +	}
> +
> +	sensor->enable_gpio = devm_gpiod_get_optional(dev, "enable", GPIOD_IN);
> +	if (IS_ERR(sensor->enable_gpio)) {
> +		dev_err(dev, "failed to get enable gpio\n");
> +		return PTR_ERR(sensor->enable_gpio);
> +	}
> +
> +	sensor->reset_gpio = devm_gpiod_get_optional(dev, "reset", GPIOD_IN);
> +	if (IS_ERR(sensor->reset_gpio)) {
> +		dev_err(dev, "failed to get reset gpio\n");
> +		return PTR_ERR(sensor->reset_gpio);
> +	}
> +
> +	if (!sensor->enable_gpio || !sensor->reset_gpio) {
> +		dev_err(dev, "enable and reset pins must be configured\n");
> +		return ret;
> +	}
> +
> +	v4l2_i2c_subdev_init(&sensor->sd, client, &gc2145_subdev_ops);
> +
> +	sensor->sd.flags = V4L2_SUBDEV_FL_HAS_DEVNODE;
> +	sensor->pad.flags = MEDIA_PAD_FL_SOURCE;
> +	sensor->sd.entity.function = MEDIA_ENT_F_CAM_SENSOR;
> +	ret = media_entity_pads_init(&sensor->sd.entity, 1, &sensor->pad);
> +	if (ret)
> +		return ret;
> +
> +	mutex_init(&sensor->lock);
> +
> +	ret = gc2145_get_regulators(sensor);
> +	if (ret)
> +		goto entity_cleanup;
> +
> +	ret = gc2145_init_controls(sensor);
> +	if (ret)
> +		goto entity_cleanup;
> +
> +	ret = v4l2_async_register_subdev(&sensor->sd);
> +	if (ret)
> +		goto free_ctrls;
> +
> +	return 0;
> +
> +free_ctrls:
> +	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
> +entity_cleanup:
> +	mutex_destroy(&sensor->lock);
> +	media_entity_cleanup(&sensor->sd.entity);
> +	return ret;
> +}
> +
> +static int gc2145_remove(struct i2c_client *client)
> +{
> +	struct v4l2_subdev *sd = i2c_get_clientdata(client);
> +	struct gc2145_dev *sensor = to_gc2145_dev(sd);
> +
> +	v4l2_async_unregister_subdev(&sensor->sd);
> +	mutex_destroy(&sensor->lock);
> +	media_entity_cleanup(&sensor->sd.entity);
> +	v4l2_ctrl_handler_free(&sensor->ctrls.handler);
> +
> +	return 0;
> +}
> +
> +static const struct i2c_device_id gc2145_id[] = {
> +	{"gc2145", 0},
> +	{},
> +};
> +MODULE_DEVICE_TABLE(i2c, gc2145_id);
> +
> +static const struct of_device_id gc2145_dt_ids[] = {
> +	{ .compatible = "galaxycore,gc2145" },
> +	{ /* sentinel */ }
> +};
> +MODULE_DEVICE_TABLE(of, gc2145_dt_ids);
> +
> +static struct i2c_driver gc2145_i2c_driver = {
> +	.driver = {
> +		.name  = "gc2145",
> +		.of_match_table	= gc2145_dt_ids,
> +	},
> +	.id_table = gc2145_id,
> +	.probe    = gc2145_probe,
> +	.remove   = gc2145_remove,
> +};
> +
> +module_i2c_driver(gc2145_i2c_driver);
> +
> +MODULE_AUTHOR("Ondrej Jirman <megi@xff.cz>");
> +MODULE_DESCRIPTION("GC2145 Camera Subdev Driver");
> +MODULE_LICENSE("GPL");
> diff --git a/drivers/media/i2c/ov5640.c b/drivers/media/i2c/ov5640.c
> index db5a19babe67..79630d25106c 100644
> --- a/drivers/media/i2c/ov5640.c
> +++ b/drivers/media/i2c/ov5640.c
> @@ -9,6 +9,7 @@
>  #include <linux/clkdev.h>
>  #include <linux/ctype.h>
>  #include <linux/delay.h>
> +#include <linux/firmware.h>
>  #include <linux/device.h>
>  #include <linux/gpio/consumer.h>
>  #include <linux/i2c.h>
> @@ -31,7 +32,11 @@
>  
>  #define OV5640_DEFAULT_SLAVE_ID 0x3c
>  
> +#define OV5640_REG_SYS_RESET00		0x3000
> +#define OV5640_REG_SYS_RESET01		0x3001
>  #define OV5640_REG_SYS_RESET02		0x3002
> +#define OV5640_REG_SYS_CLOCK_ENABLE00	0x3004
> +#define OV5640_REG_SYS_CLOCK_ENABLE01	0x3005
>  #define OV5640_REG_SYS_CLOCK_ENABLE02	0x3006
>  #define OV5640_REG_SYS_CTRL0		0x3008
>  #define OV5640_REG_SYS_CTRL0_SW_PWDN	0x42
> @@ -41,6 +46,14 @@
>  #define OV5640_REG_PAD_OUTPUT_ENABLE01	0x3017
>  #define OV5640_REG_PAD_OUTPUT_ENABLE02	0x3018
>  #define OV5640_REG_PAD_OUTPUT00		0x3019
> +#define OV5640_REG_FW_CMD_MAIN		0x3022
> +#define OV5640_REG_FW_CMD_ACK		0x3023
> +#define OV5640_REG_FW_CMD_PARA0		0x3024
> +#define OV5640_REG_FW_CMD_PARA1		0x3025
> +#define OV5640_REG_FW_CMD_PARA2		0x3026
> +#define OV5640_REG_FW_CMD_PARA3		0x3027
> +#define OV5640_REG_FW_CMD_PARA4		0x3028
> +#define OV5640_REG_FW_STATUS		0x3029
>  #define OV5640_REG_SYSTEM_CONTROL1	0x302e
>  #define OV5640_REG_SC_PLL_CTRL0		0x3034
>  #define OV5640_REG_SC_PLL_CTRL1		0x3035
> @@ -59,6 +72,7 @@
>  #define OV5640_REG_AEC_PK_MANUAL	0x3503
>  #define OV5640_REG_AEC_PK_REAL_GAIN	0x350a
>  #define OV5640_REG_AEC_PK_VTS		0x350c
> +#define OV5640_REG_VCM_CONTROL4		0x3606
>  #define OV5640_REG_TIMING_DVPHO		0x3808
>  #define OV5640_REG_TIMING_DVPVO		0x380a
>  #define OV5640_REG_TIMING_HTS		0x380c
> @@ -96,6 +110,20 @@
>  #define OV5640_REG_SDE_CTRL4		0x5584
>  #define OV5640_REG_SDE_CTRL5		0x5585
>  #define OV5640_REG_AVG_READOUT		0x56a1
> +#define OV5640_REG_FIRMWARE_BASE	0x8000
> +
> +#define OV5640_FW_STATUS_S_FIRMWARE	0x7f
> +#define OV5640_FW_STATUS_S_STARTUP	0x7e
> +#define OV5640_FW_STATUS_S_IDLE		0x70
> +#define OV5640_FW_STATUS_S_FOCUSING	0x00
> +#define OV5640_FW_STATUS_S_FOCUSED	0x10
> +
> +#define OV5640_FW_CMD_TRIGGER_FOCUS	0x03
> +#define OV5640_FW_CMD_CONTINUOUS_FOCUS	0x04
> +#define OV5640_FW_CMD_GET_FOCUS_RESULT	0x07
> +#define OV5640_FW_CMD_RELEASE_FOCUS	0x08
> +#define OV5640_FW_CMD_ZONE_CONFIG	0x12
> +#define OV5640_FW_CMD_DEFAULT_ZONES	0x80
>  
>  enum ov5640_mode_id {
>  	OV5640_MODE_QQVGA_160_120 = 0,
> @@ -112,7 +140,11 @@ enum ov5640_mode_id {
>  };
>  
>  enum ov5640_frame_rate {
> -	OV5640_15_FPS = 0,
> +	OV5640_2_FPS = 0,
> +	OV5640_3_FPS,
> +	OV5640_5_FPS,
> +	OV5640_7_FPS,
> +	OV5640_15_FPS,
>  	OV5640_30_FPS,
>  	OV5640_60_FPS,
>  	OV5640_NUM_FRAMERATES,
> @@ -156,6 +188,10 @@ MODULE_PARM_DESC(virtual_channel,
>  		 "MIPI CSI-2 virtual channel (0..3), default 0");
>  
>  static const int ov5640_framerates[] = {
> +	[OV5640_2_FPS] = 2,
> +	[OV5640_3_FPS] = 3,
> +	[OV5640_5_FPS] = 5,
> +	[OV5640_7_FPS] = 7,
>  	[OV5640_15_FPS] = 15,
>  	[OV5640_30_FPS] = 30,
>  	[OV5640_60_FPS] = 60,
> @@ -214,6 +250,12 @@ struct ov5640_ctrls {
>  		struct v4l2_ctrl *auto_gain;
>  		struct v4l2_ctrl *gain;
>  	};
> +	struct {
> +		struct v4l2_ctrl *focus_auto;
> +		struct v4l2_ctrl *af_start;
> +		struct v4l2_ctrl *af_stop;
> +		struct v4l2_ctrl *af_status;
> +	};
>  	struct v4l2_ctrl *brightness;
>  	struct v4l2_ctrl *light_freq;
>  	struct v4l2_ctrl *saturation;
> @@ -257,6 +299,8 @@ struct ov5640_dev {
>  
>  	bool pending_mode_change;
>  	bool streaming;
> +
> +	bool af_initialized;
>  };
>  
>  static inline struct ov5640_dev *to_ov5640_dev(struct v4l2_subdev *sd)
> @@ -1764,6 +1808,7 @@ static int ov5640_set_mode(struct ov5640_dev *sensor)
>  	bool auto_exp =  sensor->ctrls.auto_exp->val == V4L2_EXPOSURE_AUTO;
>  	unsigned long rate;
>  	int ret;
> +	u8 tmp;
>  
>  	dn_mode = mode->dn_mode;
>  	orig_dn_mode = orig_mode->dn_mode;
> @@ -1836,6 +1881,22 @@ static int ov5640_set_mode(struct ov5640_dev *sensor)
>  	if (ret < 0)
>  		return ret;
>  
> +	ret = ov5640_read_reg(sensor, 0x5308, &tmp);
> +	if (ret)
> +		return ret;
> +
> +	ret = ov5640_write_reg(sensor, 0x5308, tmp | 0x10 | 0x40);
> +	if (ret)
> +		return ret;
> +
> +	ret = ov5640_write_reg(sensor, 0x5306, 0);
> +	if (ret)
> +		return ret;
> +
> +	ret = ov5640_write_reg(sensor, 0x5302, 0);
> +	if (ret)
> +		return ret;
> +
>  	sensor->pending_mode_change = false;
>  	sensor->last_mode = mode;
>  
> @@ -1904,9 +1965,122 @@ static void ov5640_reset(struct ov5640_dev *sensor)
>  	usleep_range(20000, 25000);
>  }
>  
> +static int ov5640_copy_fw_to_device(struct ov5640_dev *sensor,
> +					const struct firmware *fw)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	const u8 *data = (const u8 *)fw->data;
> +	u8 fw_status;
> +	int i;
> +	int ret;
> +
> +	// Putting MCU in reset state
> +	ret = ov5640_write_reg(sensor, OV5640_REG_SYS_RESET00, 0x20);
> +	if (ret)
> +		return ret;
> +
> +	// Write firmware
> +	for (i = 0; i < fw->size / sizeof(u8); i++)
> +		ov5640_write_reg(sensor,
> +				OV5640_REG_FIRMWARE_BASE + i, 
> +				data[i]);
> +
> +	// Reset MCU state
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_MAIN, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_ACK, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA0, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA1, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA2, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA3, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_CMD_PARA4, 0x00);
> +	ov5640_write_reg(sensor, OV5640_REG_FW_STATUS, 0x7f);
> +
> +	// Start AF MCU
> +	ret = ov5640_write_reg(sensor, OV5640_REG_SYS_RESET00, 0x00);
> +	if (ret)
> +		return ret;
> +
> +	dev_info(&client->dev, "firmware upload success\n");
> +
> +	// Wait for firmware to be ready
> +	for (i = 0; i < 5; i++) {
> +		ret = ov5640_read_reg(sensor, OV5640_REG_FW_STATUS, &fw_status);
> +		if (fw_status == OV5640_FW_STATUS_S_IDLE) {
> +			dev_info(&client->dev, "fw started after %d ms\n", i * 50);
> +			return ret;
> +		}
> +		msleep(50);
> +	}
> +	dev_err(&client->dev, "uploaded firmware didn't start, got to 0x%x, retrying...\n", fw_status);
> +
> +	// Putting MCU in reset state
> +	ret = ov5640_write_reg(sensor, OV5640_REG_SYS_RESET00, 0x20);
> +	if (ret)
> +		return ret;
> +	// Start AF MCU
> +	ret = ov5640_write_reg(sensor, OV5640_REG_SYS_RESET00, 0x00);
> +	if (ret)
> +		return ret;
> +	// Wait for firmware to be ready
> +	for (i = 0; i < 5; i++) {
> +		ret = ov5640_read_reg(sensor, OV5640_REG_FW_STATUS, &fw_status);
> +		if (fw_status == OV5640_FW_STATUS_S_IDLE) {
> +			dev_info(&client->dev, "fw started after %d ms\n", i * 50);
> +			return ret;
> +		}
> +		msleep(50);
> +	}
> +	dev_err(&client->dev, "uploaded firmware didn't start, got to 0x%x\n", fw_status);
> +	return -ETIMEDOUT;
> +}
> +
> +static int ov5640_af_init(struct ov5640_dev *sensor)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	const char* fwname = "ov5640_af.bin";
> +	const struct firmware *fw;
> +	int ret;
> +
> +	if (sensor->af_initialized) {
> +		return 0;
> +	}
> +
> +	if (firmware_request_nowarn(&fw, fwname, &client->dev) == 0) {
> +		ret = ov5640_copy_fw_to_device(sensor, fw);
> +		if (ret == 0)
> +			sensor->af_initialized = 1;
> +	} else {
> +		dev_warn(&client->dev, "%s: no autofocus firmware available (%s)\n",
> +			__func__, fwname);
> +		ret = -1;
> +	}
> +	release_firmware(fw);
> +
> +	if (ret)
> +		return ret;
> +
> +	// Enable AF systems
> +	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE00,
> +			     (BIT(6) | BIT(5)), (BIT(6) | BIT(5)));
> +	if (ret)
> +		return ret;
> +	ret = ov5640_mod_reg(sensor, OV5640_REG_SYS_CLOCK_ENABLE01,
> +			     BIT(6), BIT(6));
> +	if (ret)
> +		return ret;
> +
> +	// Set lens focus driver on
> +	ov5640_write_reg(sensor, OV5640_REG_VCM_CONTROL4, 0x3f);
> +	if (ret)
> +		return ret;
> +
> +	return ret;
> +}
> +
>  static int ov5640_set_power_on(struct ov5640_dev *sensor)
>  {
>  	struct i2c_client *client = sensor->i2c_client;
> +	u16 chip_id;
>  	int ret;
>  
>  	ret = clk_prepare_enable(sensor->xclk);
> @@ -1924,6 +2098,8 @@ static int ov5640_set_power_on(struct ov5640_dev *sensor)
>  		goto xclk_off;
>  	}
>  
> +	sensor->af_initialized = 0;
> +
>  	ov5640_reset(sensor);
>  	ov5640_power(sensor, true);
>  
> @@ -1931,6 +2107,13 @@ static int ov5640_set_power_on(struct ov5640_dev *sensor)
>  	if (ret)
>  		goto power_off;
>  
> +	ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
> +	if (ret) {
> +		dev_err(&client->dev, "%s: failed to read chip identifier\n",
> +			__func__);
> +		goto power_off;
> +	}
> +
>  	return 0;
>  
>  power_off:
> @@ -1946,6 +2129,7 @@ static void ov5640_set_power_off(struct ov5640_dev *sensor)
>  	ov5640_power(sensor, false);
>  	regulator_bulk_disable(OV5640_NUM_SUPPLIES, sensor->supplies);
>  	clk_disable_unprepare(sensor->xclk);
> +	msleep(100);
>  }
>  
>  static int ov5640_set_power_mipi(struct ov5640_dev *sensor, bool on)
> @@ -2193,11 +2377,11 @@ static int ov5640_try_frame_interval(struct ov5640_dev *sensor,
>  				     u32 width, u32 height)
>  {
>  	const struct ov5640_mode_info *mode;
> -	enum ov5640_frame_rate rate = OV5640_15_FPS;
> +	enum ov5640_frame_rate rate = OV5640_2_FPS;
>  	int minfps, maxfps, best_fps, fps;
>  	int i;
>  
> -	minfps = ov5640_framerates[OV5640_15_FPS];
> +	minfps = ov5640_framerates[OV5640_2_FPS];
>  	maxfps = ov5640_framerates[OV5640_60_FPS];
>  
>  	if (fi->numerator == 0) {
> @@ -2433,6 +2617,35 @@ static int ov5640_set_framefmt(struct ov5640_dev *sensor,
>  			      is_jpeg ? (BIT(5) | BIT(3)) : 0);
>  }
>  
> +static int ov5640_fw_command(struct ov5640_dev *sensor, int command)
> +{
> +	u8 fw_ack;
> +	int i;
> +	int ret;
> +
> +	ret = ov5640_write_reg(sensor, OV5640_REG_FW_CMD_ACK, 0x01);
> +	if(ret)
> +		return ret;
> +	
> +	ret = ov5640_write_reg(sensor, OV5640_REG_FW_CMD_MAIN, command);
> +	if(ret)
> +		return ret;
> +
> +	for (i = 0; i < 100; i++) {
> +		ret = ov5640_read_reg(sensor, OV5640_REG_FW_CMD_ACK, &fw_ack);
> +		if (ret)
> +			return ret;
> +
> +		if (fw_ack == 0){
> +			return ret;
> +		}
> +
> +		msleep(50);
> +	}
> +	return -ETIMEDOUT;
> +}
> +
> +
>  /*
>   * Sensor Controls.
>   */
> @@ -2549,6 +2762,41 @@ static int ov5640_set_ctrl_exposure(struct ov5640_dev *sensor,
>  	return ret;
>  }
>  
> +static int ov5640_set_ctrl_focus(struct ov5640_dev *sensor, int command)
> +{
> +	struct i2c_client *client = sensor->i2c_client;
> +	int ret;
> +
> +	ret = ov5640_af_init(sensor);
> +	if (ret) {
> +		dev_err(&client->dev, "%s: no autofocus firmware loaded\n",
> +			__func__);
> +		return 0;
> +	}
> +
> +	if (command == OV5640_FW_CMD_RELEASE_FOCUS) {
> +		dev_dbg(&client->dev, "%s: Releasing autofocus\n",
> +			__func__);
> +		return ov5640_fw_command(sensor, OV5640_FW_CMD_RELEASE_FOCUS);
> +	}
> +	
> +	// Restart zone config
> +	ret = ov5640_fw_command(sensor, OV5640_FW_CMD_ZONE_CONFIG);
> +	if (ret)
> +		return ret;
> +
> +	// Set default focus zones
> +	ret = ov5640_fw_command(sensor, OV5640_FW_CMD_DEFAULT_ZONES);
> +	if (ret)
> +		return ret;
> +
> +	dev_dbg(&client->dev, "%s: Triggering autofocus\n",
> +		__func__);
> +
> +	// Start focussing
> +	return ov5640_fw_command(sensor, command);
> +}
> +
>  static int ov5640_set_ctrl_gain(struct ov5640_dev *sensor, bool auto_gain)
>  {
>  	struct ov5640_ctrls *ctrls = &sensor->ctrls;
> @@ -2655,6 +2903,32 @@ static int ov5640_set_ctrl_vflip(struct ov5640_dev *sensor, int value)
>  			      (BIT(2) | BIT(1)) : 0);
>  }
>  
> +static int ov5640_get_af_status(struct ov5640_dev *sensor)
> +{
> +	u8 fw_status;
> +	int ret;
> +
> +	ret = ov5640_read_reg(sensor, OV5640_REG_FW_STATUS, &fw_status);
> +	if (ret)
> +		return ret;
> +
> +	switch (fw_status) {
> +		case OV5640_FW_STATUS_S_FIRMWARE:
> +		case OV5640_FW_STATUS_S_STARTUP:
> +			return V4L2_AUTO_FOCUS_STATUS_FAILED;
> +			break;
> +		case OV5640_FW_STATUS_S_IDLE:
> +			return V4L2_AUTO_FOCUS_STATUS_IDLE;
> +			break;
> +		case OV5640_FW_STATUS_S_FOCUSED:
> +			return V4L2_AUTO_FOCUS_STATUS_REACHED;
> +			break;
> +		default:
> +			return V4L2_AUTO_FOCUS_STATUS_BUSY;
> +			break;
> +	}
> +}
> +
>  static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
>  {
>  	struct v4l2_subdev *sd = ctrl_to_sd(ctrl);
> @@ -2676,6 +2950,12 @@ static int ov5640_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
>  			return val;
>  		sensor->ctrls.exposure->val = val;
>  		break;
> +	case V4L2_CID_FOCUS_AUTO:
> +		val = ov5640_get_af_status(sensor);
> +		if (val < 0)
> +			return val;
> +		sensor->ctrls.af_status->val = val;
> +		break;
>  	}
>  
>  	return 0;
> @@ -2707,6 +2987,18 @@ static int ov5640_s_ctrl(struct v4l2_ctrl *ctrl)
>  	case V4L2_CID_AUTO_WHITE_BALANCE:
>  		ret = ov5640_set_ctrl_white_balance(sensor, ctrl->val);
>  		break;
> +	case V4L2_CID_FOCUS_AUTO:
> +		if (ctrl->val)
> +			ret = ov5640_set_ctrl_focus(sensor, OV5640_FW_CMD_CONTINUOUS_FOCUS);
> +		else
> +			ret = ov5640_set_ctrl_focus(sensor, OV5640_FW_CMD_RELEASE_FOCUS);
> +		break;
> +	case V4L2_CID_AUTO_FOCUS_START:
> +		ret = ov5640_set_ctrl_focus(sensor, OV5640_FW_CMD_TRIGGER_FOCUS);
> +		break;
> +	case V4L2_CID_AUTO_FOCUS_STOP:
> +		ret = ov5640_set_ctrl_focus(sensor, OV5640_FW_CMD_RELEASE_FOCUS);
> +		break;
>  	case V4L2_CID_HUE:
>  		ret = ov5640_set_ctrl_hue(sensor, ctrl->val);
>  		break;
> @@ -2779,6 +3071,20 @@ static int ov5640_init_controls(struct ov5640_dev *sensor)
>  	ctrls->gain = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_GAIN,
>  					0, 1023, 1, 0);
>  
> +	/* Autofocus */
> +	ctrls->focus_auto = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_FOCUS_AUTO,
> +					    0, 1, 1, 0);
> +	ctrls->af_start = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_FOCUS_START,
> +					    0, 1, 1, 0);
> +	ctrls->af_stop = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_AUTO_FOCUS_STOP,
> +					   0, 1, 1, 0);
> +	ctrls->af_status = v4l2_ctrl_new_std(hdl, ops,
> +					     V4L2_CID_AUTO_FOCUS_STATUS, 0,
> +					     (V4L2_AUTO_FOCUS_STATUS_BUSY |
> +					      V4L2_AUTO_FOCUS_STATUS_REACHED |
> +					      V4L2_AUTO_FOCUS_STATUS_FAILED),
> +					     0, V4L2_AUTO_FOCUS_STATUS_IDLE);
> +
>  	ctrls->saturation = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_SATURATION,
>  					      0, 255, 1, 64);
>  	ctrls->hue = v4l2_ctrl_new_std(hdl, ops, V4L2_CID_HUE,
> @@ -2812,6 +3118,7 @@ static int ov5640_init_controls(struct ov5640_dev *sensor)
>  	v4l2_ctrl_auto_cluster(3, &ctrls->auto_wb, 0, false);
>  	v4l2_ctrl_auto_cluster(2, &ctrls->auto_gain, 0, true);
>  	v4l2_ctrl_auto_cluster(2, &ctrls->auto_exp, 1, true);
> +	v4l2_ctrl_cluster(4, &ctrls->focus_auto);
>  
>  	sensor->sd.ctrl_handler = hdl;
>  	return 0;
> @@ -3013,34 +3320,6 @@ static int ov5640_get_regulators(struct ov5640_dev *sensor)
>  				       sensor->supplies);
>  }
>  
> -static int ov5640_check_chip_id(struct ov5640_dev *sensor)
> -{
> -	struct i2c_client *client = sensor->i2c_client;
> -	int ret = 0;
> -	u16 chip_id;
> -
> -	ret = ov5640_set_power_on(sensor);
> -	if (ret)
> -		return ret;
> -
> -	ret = ov5640_read_reg16(sensor, OV5640_REG_CHIP_ID, &chip_id);
> -	if (ret) {
> -		dev_err(&client->dev, "%s: failed to read chip identifier\n",
> -			__func__);
> -		goto power_off;
> -	}
> -
> -	if (chip_id != 0x5640) {
> -		dev_err(&client->dev, "%s: wrong chip identifier, expected 0x5640, got 0x%x\n",
> -			__func__, chip_id);
> -		ret = -ENXIO;
> -	}
> -
> -power_off:
> -	ov5640_set_power_off(sensor);
> -	return ret;
> -}
> -
>  static int ov5640_probe(struct i2c_client *client)
>  {
>  	struct device *dev = &client->dev;
> @@ -3076,7 +3355,7 @@ static int ov5640_probe(struct i2c_client *client)
>  		&ov5640_mode_data[OV5640_MODE_VGA_640_480];
>  	sensor->last_mode = sensor->current_mode;
>  
> -	sensor->ae_target = 52;
> +	sensor->ae_target = 28;
>  
>  	/* optional indication of physical rotation of sensor */
>  	ret = fwnode_property_read_u32(dev_fwnode(&client->dev), "rotation",
> @@ -3158,10 +3437,6 @@ static int ov5640_probe(struct i2c_client *client)
>  
>  	mutex_init(&sensor->lock);
>  
> -	ret = ov5640_check_chip_id(sensor);
> -	if (ret)
> -		goto entity_cleanup;
> -
>  	ret = ov5640_init_controls(sensor);
>  	if (ret)
>  		goto entity_cleanup;
> 
> -- 
> People of Russia, stop Putin before his war on Ukraine escalates.