/*
 * imx290.c - imx290 sensor driver
 *
 * Copyright (c) 2016-2017, NVIDIA CORPORATION.  All rights reserved.
 *
 * This program is free software; you can redistribute it and/or modify it
 * under the terms and conditions of the GNU General Public License,
 * version 2, as published by the Free Software Foundation.
 *
 * This program is distributed in the hope it will be useful, but WITHOUT
 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
 * more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <dt-bindings/gpio/tegra-gpio.h>
#include <linux/slab.h>
#include <linux/uaccess.h>
#include <linux/gpio.h>
#include <linux/module.h>

#include <linux/seq_file.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/of_gpio.h>

#include <media/tegra_v4l2_camera.h>
#include <media/tegra-v4l2-camera.h>
#include <media/camera_common.h>
#include <media/soc_camera.h>
#include "imx290_mode_tbls.h"


#define IMX290_DEFAULT_MODE	IMX290_MODE_1920X1080_30FPS
#define IMX290_DEFAULT_DATAFMT	MEDIA_BUS_FMT_SRGGB12_1X12

#define IMX290_MIN_FRAME_LENGTH	(1125)
#define IMX290_MAX_FRAME_LENGTH	(0x1FFFF)
#define IMX290_MIN_SHS1_1080P_HDR	(5)
#define IMX290_MIN_SHS2_1080P_HDR	(82)
#define IMX290_MAX_SHS2_1080P_HDR	(IMX290_MAX_FRAME_LENGTH - 5)
#define IMX290_MAX_SHS1_1080P_HDR	(IMX290_MAX_SHS2_1080P_HDR / 16)

#define IMX290_FRAME_LENGTH_ADDR_MSB		0x301A
#define IMX290_FRAME_LENGTH_ADDR_MID		0x3019
#define IMX290_FRAME_LENGTH_ADDR_LSB		0x3018
#define IMX290_COARSE_TIME_SHS1_ADDR_MSB	0x3022
#define IMX290_COARSE_TIME_SHS1_ADDR_MID	0x3021
#define IMX290_COARSE_TIME_SHS1_ADDR_LSB	0x3020
#define IMX290_COARSE_TIME_SHS2_ADDR_MSB	0x3025
#define IMX290_COARSE_TIME_SHS2_ADDR_MID	0x3024
#define IMX290_COARSE_TIME_SHS2_ADDR_LSB	0x3023
#define IMX290_GAIN_ADDR					0x3014
#define IMX290_GROUP_HOLD_ADDR				0x3001
#define IMX290_SW_RESET_ADDR			0x3003

/* There is no fuse id? */

/* #define IMX290_FUSE_ID_ADDR	0x3382
#define IMX290_FUSE_ID_SIZE	6
#define IMX290_FUSE_ID_STR_SIZE	(IMX290_FUSE_ID_SIZE * 2) */

#define IMX290_DEFAULT_WIDTH	1920
#define IMX290_DEFAULT_HEIGHT	1080
#define IMX290_DEFAULT_CLK_FREQ	74250000

struct imx290 {
	struct camera_common_power_rail	power;
	int	numctrls;
	struct v4l2_ctrl_handler	ctrl_handler;
	struct i2c_client	*i2c_client;
	struct v4l2_subdev	*subdev;
	struct media_pad	pad;
	u32				frame_length;
	s32	group_hold_prev;
	bool	group_hold_en;
	s64 last_wdr_et_val;
	struct regmap	*regmap;
	struct camera_common_data	*s_data;
	struct camera_common_pdata	*pdata;
	struct v4l2_ctrl		*ctrls[];
};

static const struct regmap_config sensor_regmap_config = {
	.reg_bits = 16,
	.val_bits = 8,
	.cache_type = REGCACHE_RBTREE,
	.use_single_rw = true,
};

static int imx290_g_volatile_ctrl(struct v4l2_ctrl *ctrl);
static int imx290_s_ctrl(struct v4l2_ctrl *ctrl);

static const struct v4l2_ctrl_ops imx290_ctrl_ops = {
	.g_volatile_ctrl = imx290_g_volatile_ctrl,
	.s_ctrl = imx290_s_ctrl,
};

static struct v4l2_ctrl_config ctrl_config_list[] = {
/* Do not change the name field for the controls! */
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_GAIN,
		.name = "Gain",
		.type = V4L2_CTRL_TYPE_INTEGER64,
		.flags = V4L2_CTRL_FLAG_SLIDER,
		.min = 0 * FIXED_POINT_SCALING_FACTOR,
		.max = 48 * FIXED_POINT_SCALING_FACTOR,
		.def = 0 * FIXED_POINT_SCALING_FACTOR,
		.step = 3 * FIXED_POINT_SCALING_FACTOR / 10, /* 0.3 db */
	},
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_EXPOSURE,
		.name = "Exposure",
		.type = V4L2_CTRL_TYPE_INTEGER64,
		.flags = V4L2_CTRL_FLAG_SLIDER,
		.min = 30 * FIXED_POINT_SCALING_FACTOR / 1000000,
		.max = 1000000LL * FIXED_POINT_SCALING_FACTOR / 1000000,
		.def = 30 * FIXED_POINT_SCALING_FACTOR / 1000000,
		.step = 1 * FIXED_POINT_SCALING_FACTOR / 1000000,
	},
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_FRAME_RATE,
		.name = "Frame Rate",
		.type = V4L2_CTRL_TYPE_INTEGER64,
		.flags = V4L2_CTRL_FLAG_SLIDER,
		.min = 1 * FIXED_POINT_SCALING_FACTOR,
		.max = 60 * FIXED_POINT_SCALING_FACTOR,
		.def = 30 * FIXED_POINT_SCALING_FACTOR,
		.step = 1 * FIXED_POINT_SCALING_FACTOR,
	},
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_GROUP_HOLD,
		.name = "Group Hold",
		.type = V4L2_CTRL_TYPE_INTEGER_MENU,
		.min = 0,
		.max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
		.menu_skip_mask = 0,
		.def = 0,
		.qmenu_int = switch_ctrl_qmenu,
	},
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_HDR_EN,
		.name = "HDR enable",
		.type = V4L2_CTRL_TYPE_INTEGER_MENU,
		.min = 0,
		.max = ARRAY_SIZE(switch_ctrl_qmenu) - 1,
		.menu_skip_mask = 0,
		.def = 0,
		.qmenu_int = switch_ctrl_qmenu,
	},
	/* There is no fuse id? */
	/*
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_FUSE_ID,
		.name = "Fuse ID",
		.type = V4L2_CTRL_TYPE_STRING,
		.flags = V4L2_CTRL_FLAG_READ_ONLY,
		.min = 0,
		.max = IMX290_FUSE_ID_STR_SIZE,
		.step = 2,
	},
	*/
	{
		.ops = &imx290_ctrl_ops,
		.id = TEGRA_CAMERA_CID_SENSOR_MODE_ID,
		.name = "Sensor Mode",
		.type = V4L2_CTRL_TYPE_INTEGER64,
		.flags = V4L2_CTRL_FLAG_SLIDER,
		.min = 0,
		.max = 0xFF,
		.def = 0xFE,
		.step = 1,
	},
};

static inline void imx290_get_frame_length_regs(imx290_reg *regs,
				u32 frame_length)
{
	regs->addr = IMX290_FRAME_LENGTH_ADDR_MSB;
	//regs->val = (frame_length >> 16) & 0x01;			
	regs->val = (frame_length >> 16) & 0x03;			// Falco: There are two msb bits in 0x301A

	(regs + 1)->addr = IMX290_FRAME_LENGTH_ADDR_MID;
	(regs + 1)->val = (frame_length >> 8) & 0xff;

	(regs + 2)->addr = IMX290_FRAME_LENGTH_ADDR_LSB;
	(regs + 2)->val = (frame_length) & 0xff;
}

static inline void imx290_get_coarse_time_regs_shs1(imx290_reg *regs,
				u32 coarse_time)
{
	regs->addr = IMX290_COARSE_TIME_SHS1_ADDR_MSB;
	regs->val = (coarse_time >> 16) & 0x01;

	(regs + 1)->addr = IMX290_COARSE_TIME_SHS1_ADDR_MID;
	(regs + 1)->val = (coarse_time >> 8) & 0xff;

	(regs + 2)->addr = IMX290_COARSE_TIME_SHS1_ADDR_LSB;
	(regs + 2)->val = (coarse_time) & 0xff;

}

static inline void imx290_get_coarse_time_regs_shs2(imx290_reg *regs,
				u32 coarse_time)
{
	/*regs->addr = IMX290_COARSE_TIME_SHS2_ADDR_MSB;
	regs->val = (coarse_time >> 16) & 0x01;

	(regs + 1)->addr = IMX290_COARSE_TIME_SHS2_ADDR_MID;
	(regs + 1)->val = (coarse_time >> 8) & 0xff;

	(regs + 2)->addr = IMX290_COARSE_TIME_SHS2_ADDR_LSB;
	(regs + 2)->val = (coarse_time) & 0xff;*/
	
	/* Falco: There is no SHS2 in an IMX290 */
}

static inline void imx290_get_gain_reg(imx290_reg *regs,
				u8 gain)
{
	regs->addr = IMX290_GAIN_ADDR;
	regs->val = (gain) & 0xff;
}

static int test_mode;
module_param(test_mode, int, 0644);

static inline int imx290_read_reg(struct camera_common_data *s_data,
				u16 addr, u8 *val)
{
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	int err = 0;
	u32 reg_val = 0;

	err = regmap_read(priv->regmap, addr, &reg_val);
	*val = reg_val & 0xFF;

	return err;
}

static int imx290_write_reg(struct camera_common_data *s_data,
				u16 addr, u8 val)
{
	int err;
	struct imx290 *priv = (struct imx290 *)s_data->priv;

	err = regmap_write(priv->regmap, addr, val);
	if (err)
		pr_err("%s:i2c write failed, 0x%x = %x\n",
			__func__, addr, val);

	return err;
}

static int imx290_write_table(struct imx290 *priv,
				const imx290_reg table[])
{
	return regmap_util_write_table_8(priv->regmap,
					 table,
					 NULL, 0,
					 IMX290_TABLE_WAIT_MS,
					 IMX290_TABLE_END);
}

/* Falco: testing to handle GPIO actions according to ov example, but didnt work 

static void imx290_gpio_set(struct imx290 *priv,
			    unsigned int gpio, int val)
{
	if (1){
		printk("Accessing GPIO... trying to put gpio: %i and val: %i\n",gpio,val);
		cam_gpio_ctrl(&priv->i2c_client->dev, gpio, val, 1);
		printk("GPIO accessed.\n");
	}
	else {
		printk("\n\nThis should never be reached! Exit now!\n\n");
	}
} */

static int imx290_power_on(struct camera_common_data *s_data)
{
	int err = 0;
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	struct camera_common_power_rail *pw = &priv->power;

	dev_dbg(&priv->i2c_client->dev, "%s: power on\n", __func__);
	if (priv->pdata && priv->pdata->power_on) {
		err = priv->pdata->power_on(pw);
		if (err)
			pr_err("%s failed.\n", __func__);
		else
			pw->state = SWITCH_ON;
		return err;
	}

	/*exit reset mode: XCLR */
	/* Falco: This seems to switch the reset input of the sensor, needs editing for using i2c io buffer on sensor board */
	if (pw->reset_gpio) {
		printk("Power on function is starting\n");
		gpio_set_value(pw->reset_gpio, 0);
		//imx290_gpio_set(priv, pw->reset_gpio, 0);
		usleep_range(300, 500);			/* Falco: Prolonged delays */
		gpio_set_value(pw->reset_gpio, 1);
		//imx290_gpio_set(priv, pw->reset_gpio, 1);
		usleep_range(3000, 5000);		/* Falco: Prolonged delays */
		printk("Delays after power-on has succeeded\n"); 
	}

	pw->state = SWITCH_ON;
	return 0;

}

static int imx290_power_off(struct camera_common_data *s_data)
{
	int err = 0;
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	struct camera_common_power_rail *pw = &priv->power;

	dev_dbg(&priv->i2c_client->dev, "%s: power off\n", __func__);

	if (priv->pdata && priv->pdata->power_off) {
		err = priv->pdata->power_off(pw);
		if (!err)
			goto power_off_done;
		else
			pr_err("%s failed.\n", __func__);
		return err;
	}
	/* enter reset mode: XCLR */
	usleep_range(1, 2);
	if (pw->reset_gpio)
		gpio_set_value(pw->reset_gpio, 0);
		//imx290_gpio_set(priv, pw->reset_gpio, 0);

power_off_done:
	pw->state = SWITCH_OFF;

	return 0;
}

static int imx290_power_get(struct imx290 *priv)
{
	struct camera_common_power_rail *pw = &priv->power;
	struct camera_common_pdata *pdata = priv->pdata;
	const char *mclk_name;
	struct clk *parent;
	int err = 0;

	mclk_name = priv->pdata->mclk_name ?
		    priv->pdata->mclk_name : "extperiph1";
	pw->mclk = devm_clk_get(&priv->i2c_client->dev, mclk_name);
	if (IS_ERR(pw->mclk)) {
		dev_err(&priv->i2c_client->dev,
			"unable to get clock %s\n", mclk_name);
		return PTR_ERR(pw->mclk);
	}

	parent = devm_clk_get(&priv->i2c_client->dev, "pllp_grtba");
	if (IS_ERR(parent))
		dev_err(&priv->i2c_client->dev, "devm_clk_get failed for pllp_grtba");
	else
		clk_set_parent(pw->mclk, parent);

	pw->reset_gpio = pdata->reset_gpio;

	/* Falco : Register GPIO? Didnt work
	err = cam_gpio_register(&priv->i2c_client->dev, pw->reset_gpio);
	if (err){
		dev_err(&priv->i2c_client->dev,
			"%s ERR can't register cam gpio %u!\n",
			__func__, pw->reset_gpio);
	} */

	pw->state = SWITCH_OFF;
	return err;
}

static int imx290_set_coarse_time(struct imx290 *priv, s64 val);
static int imx290_set_coarse_time_hdr(struct imx290 *priv, s64 val);
static int imx290_set_gain(struct imx290 *priv, s64 val);
static int imx290_set_frame_rate(struct imx290 *priv, s64 val);
static int imx290_set_exposure(struct imx290 *priv, s64 val);

static int imx290_s_stream(struct v4l2_subdev *sd, int enable)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct camera_common_data *s_data = to_camera_common_data(&client->dev);
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	struct v4l2_ext_controls ctrls;
	struct v4l2_ext_control control[3];
	int err;

	dev_dbg(&client->dev, "%s++ enable %d\n", __func__, enable);

	if (!enable) {									// Falco: if stream needs to be stopped
		err =  imx290_write_table(priv,
			mode_table[IMX290_MODE_STOP_STREAM]);

		if (err)
			return err;

		/* SW_RESET will have no ACK */
		regmap_write(priv->regmap, IMX290_SW_RESET_ADDR, 0x01);

		/* Wait for one frame to make sure sensor is set to
		 * software standby in V-blank
		 *
		 * delay = frame length rows * Tline (10 us)
		 */
		usleep_range(priv->frame_length * 10,
			priv->frame_length * 10 + 1000);
		return 0;
	}
	err = imx290_write_table(priv, mode_table[s_data->mode]);	// Falco: Write selected mode from table
	if (err)
		goto exit;

	if (s_data->override_enable) {
		/* write list of override regs for the asking gain, */
		/* frame rate and exposure time    */
		memset(&ctrls, 0, sizeof(ctrls));
		ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(TEGRA_CAMERA_CID_GAIN);
		ctrls.count = 3;
		ctrls.controls = control;

		control[0].id = TEGRA_CAMERA_CID_GAIN;
		control[1].id = TEGRA_CAMERA_CID_FRAME_RATE;
		control[2].id = TEGRA_CAMERA_CID_EXPOSURE;

		err = v4l2_g_ext_ctrls(&priv->ctrl_handler, &ctrls);
		if (err == 0) {
			err |= imx290_set_gain(priv, control[0].value64);
			if (err)
				dev_err(&client->dev,
					"%s: error gain override\n", __func__);

			err |= imx290_set_frame_rate(priv, control[1].value64);
			if (err)
				dev_err(&client->dev,
					"%s: error frame length override\n",
					__func__);

			err |= imx290_set_exposure(priv, control[2].value64);
			if (err)
				dev_err(&client->dev,
					"%s: error exposure override\n",
					__func__);

		} else {
			dev_err(&client->dev, "%s: faile to get overrides\n",
				__func__);
		}
	}

	if (test_mode) {
		err = imx290_write_table(priv,
			mode_table[IMX290_MODE_TEST_PATTERN]);
		if (err)
			goto exit;
	}

	err = imx290_write_table(priv, mode_table[IMX290_MODE_START_STREAM]);		// Falco: Start the selected mode 
	if (err)
		goto exit;

	return 0;
exit:
	dev_err(&client->dev, "%s: error setting stream\n", __func__);
	return err;
}

static int imx290_g_input_status(struct v4l2_subdev *sd, u32 *status)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);
	struct camera_common_data *s_data = to_camera_common_data(&client->dev);
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	struct camera_common_power_rail *pw = &priv->power;

	*status = pw->state == SWITCH_ON;
	return 0;
}

static struct v4l2_subdev_video_ops imx290_subdev_video_ops = {
	.s_stream	= imx290_s_stream,
	.g_mbus_config	= camera_common_g_mbus_config,
	.g_input_status = imx290_g_input_status,
};

static struct v4l2_subdev_core_ops imx290_subdev_core_ops = {
	.s_power	= camera_common_s_power,
};

static int imx290_get_fmt(struct v4l2_subdev *sd,
		struct v4l2_subdev_pad_config *cfg,
		struct v4l2_subdev_format *format)
{
	return camera_common_g_fmt(sd, &format->format);
}

static int imx290_set_fmt(struct v4l2_subdev *sd,
		struct v4l2_subdev_pad_config *cfg,
	struct v4l2_subdev_format *format)
{
	int ret;

	if (format->which == V4L2_SUBDEV_FORMAT_TRY)
		ret = camera_common_try_fmt(sd, &format->format);
	else
		ret = camera_common_s_fmt(sd, &format->format);

	return ret;
}

static struct v4l2_subdev_pad_ops imx290_subdev_pad_ops = {
	.set_fmt = imx290_set_fmt,
	.get_fmt = imx290_get_fmt,
	.enum_mbus_code = camera_common_enum_mbus_code,
	.enum_frame_size	= camera_common_enum_framesizes,
	.enum_frame_interval	= camera_common_enum_frameintervals,
};

static struct v4l2_subdev_ops imx290_subdev_ops = {
	.core	= &imx290_subdev_core_ops,
	.video	= &imx290_subdev_video_ops,
	.pad = &imx290_subdev_pad_ops,
};

const struct of_device_id imx290_of_match[] = {
	{ .compatible = "nvidia,imx290",},
	{ },
};

static struct camera_common_sensor_ops imx290_common_ops = {
	.power_on = imx290_power_on,
	.power_off = imx290_power_off,
	.write_reg = imx290_write_reg,
	.read_reg = imx290_read_reg,
};

static int imx290_set_group_hold(struct imx290 *priv, s32 val)
{
	int err;
	int gh_en = switch_ctrl_qmenu[val];

	priv->group_hold_prev = val;
	if (gh_en == SWITCH_ON) {

		err = imx290_write_reg(priv->s_data,
				       IMX290_GROUP_HOLD_ADDR, 0x1);
		if (err)
			goto fail;
	} else if (gh_en == SWITCH_OFF) {
		err = imx290_write_reg(priv->s_data,
				       IMX290_GROUP_HOLD_ADDR, 0x0);
		if (err)
			goto fail;
	}
	return 0;
fail:
	dev_dbg(&priv->i2c_client->dev,
		 "%s: Group hold control error\n", __func__);
	return err;
}

static int imx290_set_gain(struct imx290 *priv, s64 val)
{
	imx290_reg reg_list[1];
	int err;
	u8 gain;

	/* translate value */
	gain = (u8) (val * 160 / (48 * FIXED_POINT_SCALING_FACTOR));
	dev_dbg(&priv->i2c_client->dev,
		"%s:  gain reg: %d\n",  __func__, gain);

	imx290_get_gain_reg(reg_list, gain);

	err = imx290_write_reg(priv->s_data, reg_list[0].addr,
		 reg_list[0].val);
	if (err)
		goto fail;

	return 0;

fail:
	dev_dbg(&priv->i2c_client->dev,
		 "%s: GAIN control error\n", __func__);
	return err;
}

static int imx290_set_frame_rate(struct imx290 *priv, s64 val)
{
	imx290_reg reg_list[3];
	int err;
	u32 frame_length;
	struct camera_common_data *s_data = priv->s_data;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode];
	struct v4l2_control control;
	int hdr_en;
	int i = 0;

	frame_length = mode->signal_properties.pixel_clock.val *
		FIXED_POINT_SCALING_FACTOR /
		mode->image_properties.line_length / val;

	priv->frame_length = frame_length;
	if (priv->frame_length > IMX290_MAX_FRAME_LENGTH)
		priv->frame_length = IMX290_MAX_FRAME_LENGTH;

	dev_dbg(&priv->i2c_client->dev,
		"%s: val: %lld, , frame_length: %d\n", __func__,
		val, priv->frame_length);

	imx290_get_frame_length_regs(reg_list, priv->frame_length);

	for (i = 0; i < 3; i++) {
		err = imx290_write_reg(priv->s_data, reg_list[i].addr,
			 reg_list[i].val);
		if (err)
			goto fail;
	}

	/* check hdr enable ctrl */
	control.id = TEGRA_CAMERA_CID_HDR_EN;
	err = camera_common_g_ctrl(priv->s_data, &control);
	if (err < 0) {
		dev_err(&priv->i2c_client->dev,
			"could not find device ctrl.\n");
		return err;
	}

	hdr_en = switch_ctrl_qmenu[control.value];
	if ((hdr_en == SWITCH_ON) && (priv->last_wdr_et_val != 0)) {
		err = imx290_set_coarse_time_hdr(priv, priv->last_wdr_et_val);
		if (err)
			dev_dbg(&priv->i2c_client->dev,
			"%s: error coarse time SHS1 SHS2 override\n", __func__);
	}

	return 0;

fail:
	dev_dbg(&priv->i2c_client->dev,
		 "%s: FRAME_LENGTH control error\n", __func__);
	return err;
}

static int imx290_set_exposure(struct imx290 *priv, s64 val)
{
	int err;
	struct v4l2_control control;
	int hdr_en;

	dev_dbg(&priv->i2c_client->dev,
		 "%s: val: %lld\n", __func__, val);

	/* check hdr enable ctrl */
	control.id = TEGRA_CAMERA_CID_HDR_EN;
	err = camera_common_g_ctrl(priv->s_data, &control);
	if (err < 0) {
		dev_err(&priv->i2c_client->dev,
			"could not find device ctrl.\n");
		return err;
	}

	hdr_en = switch_ctrl_qmenu[control.value];
	if (hdr_en == SWITCH_ON) {
		err = imx290_set_coarse_time_hdr(priv, val);
		if (err)
			dev_dbg(&priv->i2c_client->dev,
			"%s: error coarse time SHS1 SHS2 override\n", __func__);
	} else {
		err = imx290_set_coarse_time(priv, val);
		if (err)
			dev_dbg(&priv->i2c_client->dev,
			"%s: error coarse time SHS1 override\n", __func__);
	}
	return err;
}

static int imx290_set_coarse_time(struct imx290 *priv, s64 val)
{
	struct camera_common_data *s_data = priv->s_data;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode];
	imx290_reg reg_list[3];
	int err;
	u32 coarse_time_shs1;
	u32 reg_shs1;
	int i = 0;

	coarse_time_shs1 = mode->signal_properties.pixel_clock.val *
		val / mode->image_properties.line_length /
		FIXED_POINT_SCALING_FACTOR;

	if (priv->frame_length == 0)
		priv->frame_length = IMX290_MIN_FRAME_LENGTH;

	reg_shs1 = priv->frame_length - coarse_time_shs1 - 1;

	dev_dbg(&priv->i2c_client->dev,
		 "%s: coarse1:%d, shs1:%d, FL:%d\n", __func__,
		 coarse_time_shs1, reg_shs1, priv->frame_length);

	imx290_get_coarse_time_regs_shs1(reg_list, reg_shs1);

	for (i = 0; i < 3; i++) {
		err = imx290_write_reg(priv->s_data, reg_list[i].addr,
			 reg_list[i].val);
		if (err)
			goto fail;
	}

	return 0;

fail:
	dev_dbg(&priv->i2c_client->dev,
		 "%s: set coarse time error\n", __func__);
	return err;
}

static int imx290_set_coarse_time_hdr(struct imx290 *priv, s64 val)
{
	struct camera_common_data *s_data = priv->s_data;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode];
	imx290_reg reg_list_shs1[3];
	imx290_reg reg_list_shs2[3];
	u32 coarse_time_shs1;
	u32 coarse_time_shs2;
	u32 reg_shs1;
	u32 reg_shs2;
	int err;
	int i = 0;

	if (priv->frame_length == 0)
		priv->frame_length = IMX290_MIN_FRAME_LENGTH;

	priv->last_wdr_et_val = val;

	/*WDR, update SHS1 as short ET, and SHS2 is 16x of short*/
	coarse_time_shs1 = mode->signal_properties.pixel_clock.val *
		val / mode->image_properties.line_length /
		FIXED_POINT_SCALING_FACTOR / 16;
	if (coarse_time_shs1 < IMX290_MIN_SHS1_1080P_HDR)
		coarse_time_shs1 = IMX290_MIN_SHS1_1080P_HDR;
	if (coarse_time_shs1 > IMX290_MAX_SHS1_1080P_HDR)
		coarse_time_shs1 = IMX290_MAX_SHS1_1080P_HDR;

	coarse_time_shs2 = (coarse_time_shs1 - IMX290_MIN_SHS1_1080P_HDR) * 16 +
				IMX290_MIN_SHS2_1080P_HDR;

	reg_shs1 = priv->frame_length - coarse_time_shs1 - 1;
	reg_shs2 = priv->frame_length - coarse_time_shs2 - 1;

	imx290_get_coarse_time_regs_shs1(reg_list_shs1, reg_shs1);
	imx290_get_coarse_time_regs_shs2(reg_list_shs2, reg_shs2);

	dev_dbg(&priv->i2c_client->dev,
		"%s: coarse1:%d, shs1:%d, coarse2:%d, shs2: %d, FL:%d\n",
		__func__,
		 coarse_time_shs1, reg_shs1,
		 coarse_time_shs2, reg_shs2,
		 priv->frame_length);

	for (i = 0; i < 3; i++) {
		err = imx290_write_reg(priv->s_data, reg_list_shs1[i].addr,
			 reg_list_shs1[i].val);
		if (err)
			goto fail;

		err = imx290_write_reg(priv->s_data, reg_list_shs2[i].addr,
			 reg_list_shs2[i].val);
		if (err)
			goto fail;
	}

	return 0;

fail:
	dev_dbg(&priv->i2c_client->dev,
		 "%s: set WDR coarse time error\n", __func__);
	return err;
}

/* There is no fuse id? */

/*
static int imx290_fuse_id_setup(struct imx290 *priv)
{
	int err;
	int i;
	struct i2c_client *client = v4l2_get_subdevdata(priv->subdev);
	struct camera_common_data *s_data = to_camera_common_data(&client->dev);

	struct v4l2_ctrl *ctrl;
	u8 fuse_id[IMX290_FUSE_ID_SIZE];
	u8 bak = 0;

	err = camera_common_s_power(priv->subdev, true);
	if (err)
		return -ENODEV;

	for (i = 0; i < IMX290_FUSE_ID_SIZE; i++) {
		err |= imx290_read_reg(s_data,
			IMX290_FUSE_ID_ADDR + i, &bak);
		if (!err)
			fuse_id[i] = bak;
		else {
			pr_err("%s: can not read fuse id\n", __func__);
			return -EINVAL;
		}
	}

	ctrl = v4l2_ctrl_find(&priv->ctrl_handler, TEGRA_CAMERA_CID_FUSE_ID);
	if (!ctrl) {
		dev_err(&priv->i2c_client->dev,
			"could not find device ctrl.\n");
		return -EINVAL;
	}

	for (i = 0; i < IMX290_FUSE_ID_SIZE; i++)
		sprintf(&ctrl->p_new.p_char[i*2], "%02x",
			fuse_id[i]);
	ctrl->p_cur.p_char = ctrl->p_new.p_char;
	dev_info(&client->dev, "%s,  fuse id: %s\n", __func__,
		ctrl->p_cur.p_char);

	err = camera_common_s_power(priv->subdev, false);
	if (err)
		return -ENODEV;

	return 0;
}

*/

static int imx290_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
{
	struct imx290 *priv =
		container_of(ctrl->handler, struct imx290, ctrl_handler);
	int err = 0;

	if (priv->power.state == SWITCH_OFF)
		return 0;

	switch (ctrl->id) {

	default:
			pr_err("%s: unknown ctrl id.\n", __func__);
			return -EINVAL;
	}

	return err;
}

static int imx290_s_ctrl(struct v4l2_ctrl *ctrl)
{
	struct imx290 *priv =
		container_of(ctrl->handler, struct imx290, ctrl_handler);
	struct camera_common_data	*s_data = priv->s_data;
	int err = 0;

	if (priv->power.state == SWITCH_OFF)
		return 0;

	switch (ctrl->id) {
	case TEGRA_CAMERA_CID_GAIN:
		err = imx290_set_gain(priv, *ctrl->p_new.p_s64);
		break;
	case TEGRA_CAMERA_CID_EXPOSURE:
		err = imx290_set_exposure(priv, *ctrl->p_new.p_s64);
		break;
	case TEGRA_CAMERA_CID_FRAME_RATE:
		err = imx290_set_frame_rate(priv, *ctrl->p_new.p_s64);
		break;
	case TEGRA_CAMERA_CID_GROUP_HOLD:
		err = imx290_set_group_hold(priv, ctrl->val);
		break;
	case TEGRA_CAMERA_CID_HDR_EN:
		break;
	case TEGRA_CAMERA_CID_SENSOR_MODE_ID:
		s_data->sensor_mode_id = (int) (*ctrl->p_new.p_s64);
		break;
	default:
		pr_err("%s: unknown ctrl id.\n", __func__);
		return -EINVAL;
	}

	return err;
}

static int imx290_ctrls_init(struct imx290 *priv)
{
	struct i2c_client *client = priv->i2c_client;
	struct v4l2_ctrl *ctrl;
	int num_ctrls;
	int err;
	int i;

	dev_dbg(&client->dev, "%s++\n", __func__);

	num_ctrls = ARRAY_SIZE(ctrl_config_list);
	v4l2_ctrl_handler_init(&priv->ctrl_handler, num_ctrls);

	for (i = 0; i < num_ctrls; i++) {
		ctrl = v4l2_ctrl_new_custom(&priv->ctrl_handler,
			&ctrl_config_list[i], NULL);
		if (ctrl == NULL) {
			dev_err(&client->dev, "Failed to init %s ctrl\n",
				ctrl_config_list[i].name);
			continue;
		}

		if (ctrl_config_list[i].type == V4L2_CTRL_TYPE_STRING &&
			ctrl_config_list[i].flags & V4L2_CTRL_FLAG_READ_ONLY) {
			ctrl->p_new.p_char = devm_kzalloc(&client->dev,
				ctrl_config_list[i].max + 1, GFP_KERNEL);
		}
		priv->ctrls[i] = ctrl;
	}

	priv->numctrls = num_ctrls;
	priv->subdev->ctrl_handler = &priv->ctrl_handler;
	if (priv->ctrl_handler.error) {
		dev_err(&client->dev, "Error %d adding controls\n",
			priv->ctrl_handler.error);
		err = priv->ctrl_handler.error;
		goto error;
	}

	err = v4l2_ctrl_handler_setup(&priv->ctrl_handler);
	if (err) {
		dev_err(&client->dev,
			"Error %d setting default controls\n", err);
		goto error;
	}
	
	/* There is no fuse id? */
	
	/*err = imx290_fuse_id_setup(priv);					
	if (err) {
		dev_err(&client->dev,
			"Error %d reading fuse id data\n", err);
		goto error;
	}*/

	return 0;

error:
	v4l2_ctrl_handler_free(&priv->ctrl_handler);
	return err;
}

MODULE_DEVICE_TABLE(of, imx290_of_match);

static struct camera_common_pdata *imx290_parse_dt(struct i2c_client *client,
				struct camera_common_data *s_data)
{
	struct device_node *np = client->dev.of_node;
	struct camera_common_pdata *board_priv_pdata;
	const struct of_device_id *match;
	int err;
	const char *str;

	if (!np)
		return NULL;

	match = of_match_device(imx290_of_match, &client->dev);
	if (!match) {
		dev_err(&client->dev, "Failed to find matching dt id\n");
		return NULL;
	}

	err = of_property_read_string(np, "use_sensor_mode_id", &str);
	if (!err) {
		if (!strcmp(str, "true"))
			s_data->use_sensor_mode_id = true;
		else
			s_data->use_sensor_mode_id = false;
	}
	board_priv_pdata = devm_kzalloc(&client->dev,
			   sizeof(*board_priv_pdata), GFP_KERNEL);

	err = of_property_read_string(np, "mclk",
				      &board_priv_pdata->mclk_name);
	if (err)
		dev_err(&client->dev, "mclk not in DT\n");
	
	board_priv_pdata->reset_gpio = (unsigned int)of_get_named_gpio(np, "reset-gpios", 0);
	if (err) {
		dev_err(&client->dev, "reset-gpios not found %d\n", err);
		board_priv_pdata->reset_gpio = 0;
	}

	return board_priv_pdata;
}

static int imx290_open(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)
{
	struct i2c_client *client = v4l2_get_subdevdata(sd);

	dev_dbg(&client->dev, "%s:\n", __func__);

	return 0;
}

static const struct v4l2_subdev_internal_ops imx290_subdev_internal_ops = {
	.open = imx290_open,
};

static const struct media_entity_operations imx290_media_ops = {
	.link_validate = v4l2_subdev_link_validate,
};

static int imx290_probe(struct i2c_client *client,
			const struct i2c_device_id *id)
{
	struct camera_common_data *common_data;
	struct imx290 *priv;
	int err;

	dev_info(&client->dev, "[IMX290]: probing v4l2 sensor at addr 0x%0x.\n",
		client->addr);

	if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node)
		return -EINVAL;

	common_data = devm_kzalloc(&client->dev,
			    sizeof(struct camera_common_data), GFP_KERNEL);

	priv = devm_kzalloc(&client->dev,
			    sizeof(struct imx290) + sizeof(struct v4l2_ctrl *) *
			    ARRAY_SIZE(ctrl_config_list),
			    GFP_KERNEL);
	if (!priv) {
		dev_err(&client->dev, "unable to allocate memory!\n");
		return -ENOMEM;
	}

	priv->regmap = devm_regmap_init_i2c(client, &sensor_regmap_config);
	if (IS_ERR(priv->regmap)) {
		dev_err(&client->dev,
			"regmap init failed: %ld\n", PTR_ERR(priv->regmap));
		return -ENODEV;
	}

	if (client->dev.of_node)
		priv->pdata = imx290_parse_dt(client, common_data);
	if (!priv->pdata) {
		dev_err(&client->dev, "unable to get platform data\n");
		return -EFAULT;
	}

	common_data->ops = &imx290_common_ops;
	common_data->ctrl_handler = &priv->ctrl_handler;
	common_data->dev = &client->dev;
	common_data->frmfmt = &imx290_frmfmt[0];
	common_data->colorfmt = camera_common_find_datafmt(
					  IMX290_DEFAULT_DATAFMT);
	common_data->power = &priv->power;
	common_data->ctrls = priv->ctrls;
	common_data->priv = (void *)priv;
	common_data->numctrls = ARRAY_SIZE(ctrl_config_list);
	common_data->numfmts = ARRAY_SIZE(imx290_frmfmt);
	common_data->def_mode = IMX290_DEFAULT_MODE;
	common_data->def_width = IMX290_DEFAULT_WIDTH;
	common_data->def_height = IMX290_DEFAULT_HEIGHT;
	common_data->fmt_width = common_data->def_width;
	common_data->fmt_height = common_data->def_height;
	common_data->def_clk_freq = IMX290_DEFAULT_CLK_FREQ;

	priv->i2c_client = client;
	priv->s_data = common_data;
	priv->subdev = &common_data->subdev;
	priv->subdev->dev = &client->dev;
	priv->s_data->dev = &client->dev;
	priv->last_wdr_et_val = 0;

	err = imx290_power_get(priv);
	if (err)
		return err;

	err = camera_common_initialize(common_data, "imx290");
	if (err) {
		dev_err(&client->dev, "Failed to initialize imx290.\n");
		return err;
	}

	v4l2_i2c_subdev_init(priv->subdev, client, &imx290_subdev_ops);

	err = imx290_ctrls_init(priv);
	if (err)
		return err;

	priv->subdev->internal_ops = &imx290_subdev_internal_ops;
	priv->subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |
		     V4L2_SUBDEV_FL_HAS_EVENTS;

#if defined(CONFIG_MEDIA_CONTROLLER)
	priv->pad.flags = MEDIA_PAD_FL_SOURCE;
	priv->subdev->entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR;
	priv->subdev->entity.ops = &imx290_media_ops;
	err = media_entity_init(&priv->subdev->entity, 1, &priv->pad, 0);
	if (err < 0) {
		dev_err(&client->dev, "unable to init media entity\n");
		return err;
	}
#endif

	err = v4l2_async_register_subdev(priv->subdev);
	if (err)
		return err;

	dev_info(&client->dev, "Detected IMX290 sensor\n");

	return 0;
}

static int
imx290_remove(struct i2c_client *client)
{
	struct camera_common_data *s_data = to_camera_common_data(&client->dev);
	struct imx290 *priv = (struct imx290 *)s_data->priv;
	/* struct camera_common_power_rail *pw = &priv->power; */

	v4l2_async_unregister_subdev(priv->subdev);
#if defined(CONFIG_MEDIA_CONTROLLER)
	media_entity_cleanup(&priv->subdev->entity);
#endif

	v4l2_ctrl_handler_free(&priv->ctrl_handler);
	/* cam_gpio_deregister(&priv->i2c_client->dev, pw->reset_gpio); Falco: avoid this for now*/
	camera_common_cleanup(s_data);
	return 0;
}

static const struct i2c_device_id imx290_id[] = {
	{ "imx290", 0 },
	{ }
};

MODULE_DEVICE_TABLE(i2c, imx290_id);

static struct i2c_driver imx290_i2c_driver = {
	.driver = {
		.name = "imx290",
		.owner = THIS_MODULE,
		.of_match_table = of_match_ptr(imx290_of_match),
	},
	.probe = imx290_probe,
	.remove = imx290_remove,
	.id_table = imx290_id,
};

module_i2c_driver(imx290_i2c_driver);

MODULE_DESCRIPTION("Media Controller driver for Sony IMX290");
MODULE_AUTHOR("NVIDIA Corporation example, edited by Falco van Meijeren for ARVOO");
MODULE_LICENSE("unavailable");