//#include <linux/init.h>
//#include <linux/module.h>
#include <linux/i2c.h>           /* For of_device_id, i2c_driver,
				  * of_match_ptr, module_i2c_driver
				  */
#include <linux/module.h>        // For module_i2c_driver
#include <media/tegracam_core.h> // For tegracam_device
#include <linux/of_device.h>     // For of_match_device
#include <linux/of_gpio.h>       // For of_get_named_gpio

// For define arguments type
#define INPUT_ARGUMENT
#define MODIFIED_ARGUMENT
#define OUTPUT_ARGUMENT

/* imx219 - sensor parameter limits */
#define IMX219_MIN_GAIN				0x0000
#define IMX219_MAX_GAIN				0x00e8
#define IMX219_MIN_FRAME_LENGTH			0x0100
#define IMX219_MAX_FRAME_LENGTH			0xffff
#define IMX219_MIN_COARSE_EXPOSURE		0x0001
#define IMX219_MAX_COARSE_DIFF			0x0004

/* imx219 sensor register address */
#define IMX219_MODEL_ID_ADDR_MSB		0x0000
#define IMX219_MODEL_ID_ADDR_LSB		0x0001
#define IMX219_GAIN_ADDR			0x0157
#define IMX219_FRAME_LENGTH_ADDR_MSB		0x0160
#define IMX219_FRAME_LENGTH_ADDR_LSB		0x0161
#define IMX219_COARSE_INTEG_TIME_ADDR_MSB	0x015a
#define IMX219_COARSE_INTEG_TIME_ADDR_LSB	0x015b
#define IMX219_FINE_INTEG_TIME_ADDR_MSB		0x0388
#define IMX219_FINE_INTEG_TIME_ADDR_LSB		0x0389


//--- from imx219_mode_tbls.h ---
#define IMX219_TABLE_WAIT_MS	0
#define IMX219_TABLE_END	1

#define imx219_reg struct reg_8

static imx219_reg imx219_start_stream[] = {
	{0x0100, 0x01},
	{IMX219_TABLE_WAIT_MS, 3},
	{IMX219_TABLE_END, 0x00}
};

static imx219_reg imx219_stop_stream[] = {
	{0x0100, 0x00},
	{IMX219_TABLE_END, 0x00}
};

static imx219_reg imx219_mode_common[] = {
	/* software reset */
	{0x0103, 0x01},
	{IMX219_TABLE_WAIT_MS, 10},
	/* sensor config */
	{0x0114, 0x01}, /* D-Phy, 2-lane */
	{0x0128, 0x00},
	{0x012A, 0x18}, /* 24 MHz INCK */
	{0x012B, 0x00},
	/* access code - vendor addr. ranges */
	{0x30EB, 0x05},
	{0x30EB, 0x0C},
	{0x300A, 0xFF},
	{0x300B, 0xFF},
	{0x30EB, 0x05},
	{0x30EB, 0x09},
	/* cis tuning */
	{0x455E, 0x00},
	{0x471E, 0x4B},
	{0x4767, 0x0F},
	{0x4750, 0x14},
	{0x4540, 0x00},
	{0x47B4, 0x14},
	{0x4713, 0x30},
	{0x478B, 0x10},
	{0x478F, 0x10},
	{0x4793, 0x10},
	{0x4797, 0x0E},
	{0x479B, 0x0E},
	{IMX219_TABLE_END, 0x00}
};

static imx219_reg imx219_mode_1920x1080_30fps[] = {
	/* capture settings */
	{0x0157, 0x00}, /* ANALOG_GAIN_GLOBAL[7:0] */
	{0x015A, 0x06}, /* COARSE_INTEG_TIME[15:8] */
	{0x015B, 0xde}, /* COARSE_INTEG_TIME[7:0] */
	/* format settings */
	{0x0160, 0x06}, /* FRM_LENGTH[15:8] */
	{0x0161, 0xE2}, /* FRM_LENGTH[7:0] */
	{0x0162, 0x0D}, /* LINE_LENGTH[15:8] */
	{0x0163, 0x78}, /* LINE_LENGTH[7:0] */
	{0x0164, 0x02},
	{0x0165, 0xA8},
	{0x0166, 0x0A},
	{0x0167, 0x27},
	{0x0168, 0x02},
	{0x0169, 0xB4},
	{0x016A, 0x06},
	{0x016B, 0xEB},
	{0x016C, 0x07},
	{0x016D, 0x80},
	{0x016E, 0x04},
	{0x016F, 0x38},
	{0x0170, 0x01},
	{0x0171, 0x01},
	{0x0174, 0x00},
	{0x0175, 0x00},
	{0x018C, 0x0A},
	{0x018D, 0x0A},
	/* clocks dividers */
	{0x0301, 0x05},
	{0x0303, 0x01},
	{0x0304, 0x03},
	{0x0305, 0x03},
	{0x0306, 0x00},
	{0x0307, 0x39},
	{0x0309, 0x0A},
	{0x030B, 0x01},
	{0x030C, 0x00},
	{0x030D, 0x72},
	{IMX219_TABLE_END, 0x00}
};

static imx219_reg imx219_mode_1640x1232_30fps[] = {
	/* capture settings */
	{0x0157, 0x00}, /* ANALOG_GAIN_GLOBAL[7:0] */
	{0x015A, 0x06}, /* COARSE_INTEG_TIME[15:8] */
	{0x015B, 0xA8}, /* COARSE_INTEG_TIME[7:0] */
	/* format settings */
	{0x0160, 0x06}, /* FRM_LENGTH[15:8] */
	{0x0161, 0xE2}, /* FRM_LENGTH[7:0] */
	{0x0162, 0x0D}, /* LINE_LENGTH[15:8] */
	{0x0163, 0x78}, /* LINE_LENGTH[7:0] */
	{0x0164, 0x00},
	{0x0165, 0x00},
	{0x0166, 0x0C},
	{0x0167, 0xCF},
	{0x0168, 0x00},
	{0x0169, 0x00},
	{0x016A, 0x09},
	{0x016B, 0x9F},
	{0x016C, 0x06},
	{0x016D, 0x68},
	{0x016E, 0x04},
	{0x016F, 0xD0},
	{0x0170, 0x01},
	{0x0171, 0x01},
	{0x0174, 0x01},
	{0x0175, 0x01},
	{0x018C, 0x0A},
	{0x018D, 0x0A},
	/* clocks dividers */
	{0x0301, 0x05},
	{0x0303, 0x01},
	{0x0304, 0x03},
	{0x0305, 0x03},
	{0x0306, 0x00},
	{0x0307, 0x39},
	{0x0309, 0x0A},
	{0x030B, 0x01},
	{0x030C, 0x00},
	{0x030D, 0x72},
	{IMX219_TABLE_END, 0x00}
};

static imx219_reg imx219_mode_1280x720_60fps[] = {
	/* capture settings */
	{0x0157, 0x00}, /* ANALOG_GAIN_GLOBAL[7:0] */
	{0x015A, 0x03}, /* COARSE_INTEG_TIME[15:8] */
	{0x015B, 0x6c}, /* COARSE_INTEG_TIME[7:0] */
	/* format settings */
	{0x0160, 0x03}, /* FRM_LENGTH[15:8] */
	{0x0161, 0x70}, /* FRM_LENGTH[7:0] */
	{0x0162, 0x0D}, /* LINE_LENGTH[15:8] */
	{0x0163, 0x78}, /* LINE_LENGTH[7:0] */
	{0x0164, 0x01},
	{0x0165, 0x68},
	{0x0166, 0x0B},
	{0x0167, 0x67},
	{0x0168, 0x02},
	{0x0169, 0x00},
	{0x016A, 0x07},
	{0x016B, 0x9F},
	{0x016C, 0x05},
	{0x016D, 0x00},
	{0x016E, 0x02},
	{0x016F, 0xD0},
	{0x0170, 0x01},
	{0x0171, 0x01},
	{0x0174, 0x01},
	{0x0175, 0x01},
	{0x018C, 0x0A},
	{0x018D, 0x0A},
	/* clocks dividers */
	{0x0301, 0x05},
	{0x0303, 0x01},
	{0x0304, 0x03},
	{0x0305, 0x03},
	{0x0306, 0x00},
	{0x0307, 0x39},
	{0x0309, 0x0A},
	{0x030B, 0x01},
	{0x030C, 0x00},
	{0x030D, 0x72},
	{IMX219_TABLE_END, 0x00}
};

enum {
  IMX219_MODE_1920x1080_30FPS,
  IMX219_MODE_1640x1232_30FPS,
  IMX219_MODE_1280x720_60FPS,
  
  IMX219_MODE_COMMON,
  IMX219_START_STREAM,
  IMX219_STOP_STREAM,
};

static imx219_reg *mode_table[] = {
  [IMX219_MODE_1920x1080_30FPS] = imx219_mode_1920x1080_30fps,
  [IMX219_MODE_1640x1232_30FPS] = imx219_mode_1640x1232_30fps,
  [IMX219_MODE_1280x720_60FPS] = imx219_mode_1280x720_60fps,
  
  [IMX219_MODE_COMMON]  = imx219_mode_common,
  [IMX219_START_STREAM]  = imx219_start_stream,
  [IMX219_STOP_STREAM]  = imx219_stop_stream,
};

static const int imx219_21fps[] = {
	21,
};

static const int imx219_28fps[] = {
	28,
};

static const int imx219_30fps[] = {
	30,
};

static const int imx219_60fps[] = {
	60,
};

/*
 * WARNING: frmfmt ordering need to match mode definition in
 * device tree!
 */
static const struct camera_common_frmfmt imx219_frmfmt[] = {
  {{1920, 1080},	imx219_30fps, 1, 0, IMX219_MODE_1920x1080_30FPS},
  {{1640, 1232},	imx219_30fps, 1, 0, IMX219_MODE_1640x1232_30FPS},
  {{1280, 720},	imx219_60fps, 1, 0, IMX219_MODE_1280x720_60FPS},
};

//--- from imx219_mode_tbls.h ---

static const struct of_device_id imx219_of_match[] = {
	{ .compatible = "nvidia,imx219", },
	{ },
};
MODULE_DEVICE_TABLE(of, imx219_of_match);

static const u32 ctrl_cid_list[] = {
	TEGRA_CAMERA_CID_GAIN,
	TEGRA_CAMERA_CID_EXPOSURE,
	TEGRA_CAMERA_CID_FRAME_RATE,
	TEGRA_CAMERA_CID_SENSOR_MODE_ID,
};

struct imx219 {
	struct i2c_client		*i2c_client;
	struct v4l2_subdev		*subdev;
	u16				fine_integ_time;
	u32				frame_length;
	struct camera_common_data	*s_data;
	struct tegracam_device		*tc_dev;
};

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

static inline void imx219_get_frame_length_regs(imx219_reg *regs,
	u32 frame_length)
{
  printk("#PUSA : imx219_get_frame_length_regs");
  regs->addr = IMX219_FRAME_LENGTH_ADDR_MSB;
  regs->val = (frame_length >> 8) & 0xff;
  (regs + 1)->addr = IMX219_FRAME_LENGTH_ADDR_LSB;
  (regs + 1)->val = (frame_length) & 0xff;
}

static inline void imx219_get_coarse_integ_time_regs(imx219_reg *regs,
	u32 coarse_time)
{

	regs->addr = IMX219_COARSE_INTEG_TIME_ADDR_MSB;
	regs->val = (coarse_time >> 8) & 0xff;
	(regs + 1)->addr = IMX219_COARSE_INTEG_TIME_ADDR_LSB;
	(regs + 1)->val = (coarse_time) & 0xff;
	printk("#IMX219 : imx219_get_coarse_integ_time_regs");
}

static inline void imx219_get_gain_reg(imx219_reg *reg, u8 gain)
{

	reg->addr = IMX219_GAIN_ADDR;
	reg->val = gain & 0xff;
	printk("#IMX219 : imx219_get_gain_reg");
}

static inline int imx219_read_reg(struct camera_common_data *s_data,
	u16 addr, u8 *val)
{

	int err = 0;
	u32 reg_val = 0;

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

	printk("#IMX219 : imx219_read_reg");
	return err;
}

static inline int imx219_write_reg(struct camera_common_data *s_data,
				   u16 addr, u8 val)
{
  int err = 0;

  printk("#PUSA219 : imx219_write_reg\n");
  printk("#PUSA219 : imx219_write_reg : addr= %d, val= %d\n", addr, val);

  // To deal with i2c errors and warnings
  if ((addr != 352 && val != 1) && 
      (addr != 353 && val != 0)) {
    err = regmap_write(s_data->regmap, addr, val);
  } else {
    err = 0;
  }
  
  if (err)
    dev_err(s_data->dev, "%s: i2c write failed, 0x%x = %x",
	    __func__, addr, val);
  
  return err;
}

static int imx219_write_table(struct imx219 *priv, const imx219_reg table[])
{
  int tmp = 0;
  int index = -1;
  printk("#PUSA : imx219_write_table");
  //  printk("#IMX219 : imx219_write_table : priv->s_data->regmap->name = %d",
  //	 priv->s_data->regmap->async_ret);
  do {
    index++;
    printk("#PUSA : imx219_write_table : mode_table[IMX219_MODE_COMMON][%d] : addr = 0x%x, val = 0x%x",
	   index,
	   table[index].addr,
	   table[index].val);
  } while ((table[index].addr != IMX219_TABLE_END) ||
	   (table[index].val != 0));
  
  tmp = regmap_util_write_table_8(priv->s_data->regmap, table, NULL, 0,
  				  IMX219_TABLE_WAIT_MS, IMX219_TABLE_END);
  printk("#IMX219 : imx219_write_table : result = %d", tmp);
  return tmp;
}

static int imx219_set_group_hold(struct tegracam_device *tc_dev, bool val)
{
  printk("#IMX219 : imx219_set_group_hold");
	/* imx219 does not support group hold */
	return 0;
}

static int imx219_get_fine_integ_time(struct imx219 *priv, u16 *fine_time)
{

	struct camera_common_data *s_data = priv->s_data;
	int err = 0;
	u8 reg_val[2];

	printk("#PUSA : imx219_get_fine_integ_time\n");
	goto done;
	
	err = imx219_read_reg(s_data, IMX219_FINE_INTEG_TIME_ADDR_MSB,
		&reg_val[0]);
	if (err){
	  printk("#PUSA : imx219_get_fine_integ_time : result = %d. Error!\n", err);
	  goto done;
	}
	
	err = imx219_read_reg(s_data, IMX219_FINE_INTEG_TIME_ADDR_LSB,
		&reg_val[1]);
	if (err)
		goto done;

	*fine_time = (reg_val[0] << 8) | reg_val[1];

	/* For IMX219 fine_time = 500 */
	//printk("#IMX219 : imx219_get_fine_integ_time time = %d", *fine_time);

	
done:
	/* Just workaround */
	*fine_time = 500;
	err = 0;
	
	return err;
}

static int imx219_set_gain(struct tegracam_device *tc_dev, s64 val)
{

	struct camera_common_data *s_data = tc_dev->s_data;
	struct device *dev = s_data->dev;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode_prop_idx];

	int err = 0;
	imx219_reg gain_reg;
	s16 gain;

	goto finish;
	
	if (val < mode->control_properties.min_gain_val)
		val = mode->control_properties.min_gain_val;
	else if (val > mode->control_properties.max_gain_val)
		val = mode->control_properties.max_gain_val;

	/* translate value (from normalized analog gain) */
	gain = (s16)((256 * mode->control_properties.gain_factor) / val);
	gain = 256 - gain;

	if (gain < IMX219_MIN_GAIN)
		gain = IMX219_MAX_GAIN;
	else if (gain > IMX219_MAX_GAIN)
		gain = IMX219_MAX_GAIN;

	dev_dbg(dev, "%s: val: %lld (/%d) [times], gain: %u\n",
		__func__, val, mode->control_properties.gain_factor, gain);

	imx219_get_gain_reg(&gain_reg, (u8)gain);
	err = imx219_write_reg(s_data, gain_reg.addr, gain_reg.val);
	if (err)
		dev_dbg(dev, "%s: gain control error\n", __func__);

	printk("#IMX219 : imx219_set_gain");
	
 finish:
	return 0;
}

static int imx219_set_frame_rate(struct tegracam_device *tc_dev, s64 val)
{

	struct camera_common_data *s_data = tc_dev->s_data;
	struct imx219 *priv = (struct imx219 *)tc_dev->priv;
	struct device *dev = tc_dev->dev;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode_prop_idx];

	int err = 0;
	imx219_reg fl_regs[2];
	u32 frame_length;
	int i;

	frame_length = (u32)(mode->signal_properties.pixel_clock.val *
		(u64)mode->control_properties.framerate_factor /
		mode->image_properties.line_length / val);

	if (frame_length < IMX219_MIN_FRAME_LENGTH)
		frame_length = IMX219_MIN_FRAME_LENGTH;
	else if (frame_length > IMX219_MAX_FRAME_LENGTH)
		frame_length = IMX219_MAX_FRAME_LENGTH;

	dev_dbg(dev,
		"%s: val: %llde-6 [fps], frame_length: %u [lines]\n",
		__func__, val, frame_length);

	imx219_get_frame_length_regs(fl_regs, frame_length);
	for (i = 0; i < 2; i++) {
		err = imx219_write_reg(s_data, fl_regs[i].addr, fl_regs[i].val);
		if (err) {
			dev_dbg(dev,
				"%s: frame_length control error\n", __func__);
			return err;
		}
	}

	priv->frame_length = frame_length;
	printk("#IMX219 : imx219_set_frame_rate");
	
	return 0;
}

static int imx219_set_exposure(struct tegracam_device *tc_dev, s64 val)
{

	struct camera_common_data *s_data = tc_dev->s_data;
	struct imx219 *priv = (struct imx219 *)tc_dev->priv;
	struct device *dev = tc_dev->dev;
	const struct sensor_mode_properties *mode =
		&s_data->sensor_props.sensor_modes[s_data->mode_prop_idx];

	int err = 0;
	imx219_reg ct_regs[2];
	const s32 max_coarse_time = priv->frame_length - IMX219_MAX_COARSE_DIFF;
	const s32 fine_integ_time_factor = priv->fine_integ_time *
		mode->control_properties.exposure_factor /
		mode->signal_properties.pixel_clock.val;
	u32 coarse_time;
	int i;

	return 0;
	coarse_time = (val - fine_integ_time_factor)
		* mode->signal_properties.pixel_clock.val
		/ mode->control_properties.exposure_factor
		/ mode->image_properties.line_length;

	if (coarse_time < IMX219_MIN_COARSE_EXPOSURE)
		coarse_time = IMX219_MIN_COARSE_EXPOSURE;
	else if (coarse_time > max_coarse_time) {
		coarse_time = max_coarse_time;
		dev_dbg(dev,
			"%s: exposure limited by frame_length: %d [lines]\n",
			__func__, max_coarse_time);
	}

	dev_dbg(dev, "%s: val: %lld [us], coarse_time: %d [lines]\n",
		__func__, val, coarse_time);

	imx219_get_coarse_integ_time_regs(ct_regs, coarse_time);

	for (i = 0; i < 2; i++) {
		err = imx219_write_reg(s_data, ct_regs[i].addr, ct_regs[i].val);
		if (err) {
			dev_dbg(dev,
				"%s: coarse_time control error\n", __func__);
			return err;
		}
	}
	printk("#IMX219 : imx219_set_exposure");
	
	return 0;
}

static struct tegracam_ctrl_ops imx219_ctrl_ops = {
	.numctrls = ARRAY_SIZE(ctrl_cid_list),
	.ctrl_cid_list = ctrl_cid_list,
	.set_gain = imx219_set_gain,
	.set_exposure = imx219_set_exposure,
	.set_frame_rate = imx219_set_frame_rate,
	.set_group_hold = imx219_set_group_hold,
};

/* camera_common_data - various camera properties structure */
static int imx219_power_on(struct camera_common_data *s_data)
{

	int err = 0;

	struct camera_common_power_rail *pw = s_data->power;

	struct camera_common_pdata *pdata = s_data->pdata;
	struct device *dev = s_data->dev;

	dev_dbg(dev, "%s: power on\n", __func__);
	printk("#IMX219 : imx219_power_on");
	printk("#IMX219 : imx219_power_on : pwdn_gpio = %d, reset_gpio = %d, af_gpio = %d",
	       s_data->pdata->pwdn_gpio, s_data->pdata->reset_gpio, s_data->pdata->af_gpio);
	printk("#IMX219 : imx219_power_on : mclk_name = %s, parentclk_name = %s",
	       pdata->mclk_name, pdata->parentclk_name);
	if (pdata && pdata->power_on) {
		err = pdata->power_on(pw);
		if (err)
			dev_err(dev, "%s failed.\n", __func__);
		else
			pw->state = SWITCH_ON;
		return err;
	}

	if (pw->reset_gpio) {
		if (gpio_cansleep(pw->reset_gpio))
			gpio_set_value_cansleep(pw->reset_gpio, 0);
		else
			gpio_set_value(pw->reset_gpio, 0);
	}

	if (unlikely(!(pw->avdd || pw->iovdd || pw->dvdd)))
		goto skip_power_seqn;

	usleep_range(10, 20);

	if (pw->avdd) {
		err = regulator_enable(pw->avdd);
		if (err)
			goto imx219_avdd_fail;
	}

	if (pw->iovdd) {
		err = regulator_enable(pw->iovdd);
		if (err)
			goto imx219_iovdd_fail;
	}

	if (pw->dvdd) {
		err = regulator_enable(pw->dvdd);
		if (err)
			goto imx219_dvdd_fail;
	}

	usleep_range(10, 20);

skip_power_seqn:
	if (pw->reset_gpio) {
		if (gpio_cansleep(pw->reset_gpio))
			gpio_set_value_cansleep(pw->reset_gpio, 1);
		else
			gpio_set_value(pw->reset_gpio, 1);
	}

	/* Need to wait for t4 + t5 + t9 time as per the data sheet */
	/* t4 - 200us, t5 - 21.2ms, t9 - 1.2ms */
	usleep_range(23000, 23100);

	pw->state = SWITCH_ON;

	return 0;

imx219_dvdd_fail:
	regulator_disable(pw->iovdd);

imx219_iovdd_fail:
	regulator_disable(pw->avdd);

imx219_avdd_fail:
	dev_err(dev, "%s failed.\n", __func__);

	return -ENODEV;
}

static int imx219_power_off(struct camera_common_data *s_data)
{

	int err = 0;
	struct camera_common_power_rail *pw = s_data->power;
	struct camera_common_pdata *pdata = s_data->pdata;
	struct device *dev = s_data->dev;

	// for debug
	return err;
	// ---- for debug ----
	
	dev_dbg(dev, "%s: power off\n", __func__);
	printk("#IMX219 : imx219_power_off");
	
	if (pdata && pdata->power_off) {
		err = pdata->power_off(pw);
		if (err) {
			dev_err(dev, "%s failed.\n", __func__);
			return err;
		}
	} else {
		if (pw->reset_gpio) {
			if (gpio_cansleep(pw->reset_gpio))
				gpio_set_value_cansleep(pw->reset_gpio, 0);
			else
				gpio_set_value(pw->reset_gpio, 0);
		}

		usleep_range(10, 10);

		if (pw->dvdd)
			regulator_disable(pw->dvdd);
		if (pw->iovdd)
			regulator_disable(pw->iovdd);
		if (pw->avdd)
			regulator_disable(pw->avdd);
	}

	pw->state = SWITCH_OFF;

	return 0;
}

static int imx219_power_put(struct tegracam_device *tc_dev)
{

	struct camera_common_data *s_data = tc_dev->s_data;
	struct camera_common_power_rail *pw = s_data->power;

	if (unlikely(!pw))
		return -EFAULT;

	if (likely(pw->dvdd))
		devm_regulator_put(pw->dvdd);

	if (likely(pw->avdd))
		devm_regulator_put(pw->avdd);

	if (likely(pw->iovdd))
		devm_regulator_put(pw->iovdd);

	pw->dvdd = NULL;
	pw->avdd = NULL;
	pw->iovdd = NULL;

	if (likely(pw->reset_gpio))
		gpio_free(pw->reset_gpio);
	printk("#IMX219 : imx219_power_put");
	  
	return 0;
}

static int imx219_power_get(struct tegracam_device *tc_dev)
{
	struct device *dev = tc_dev->dev;
	struct camera_common_data *s_data = tc_dev->s_data;
	struct camera_common_power_rail *pw = s_data->power;
	struct camera_common_pdata *pdata = s_data->pdata;
	struct clk *parent;
	int err = 0;

	printk("#PUSA : imx219_power_get");
	if (!pdata) {
	  printk("#PUSA : imx219_power_get : pdata not found! Error!\n");
	  dev_err(dev, "pdata missing\n");
	  return -EFAULT;
	}
	printk("#PUSA : imx219_power_get : pdata found. Continue.\n");

	/* Sensor MCLK (aka. INCK) */
	if (pdata->mclk_name) {
	  printk("#PUSA : imx219_power_get : pdata->mclk_name found.\n");
		pw->mclk = devm_clk_get(dev, pdata->mclk_name);
		if (IS_ERR(pw->mclk)) {
			dev_err(dev, "unable to get clock %s\n",
				pdata->mclk_name);
			return PTR_ERR(pw->mclk);
		}

		if (pdata->parentclk_name) {
			parent = devm_clk_get(dev, pdata->parentclk_name);
			if (IS_ERR(parent)) {
				dev_err(dev, "unable to get parent clock %s",
					pdata->parentclk_name);
			} else
				clk_set_parent(pw->mclk, parent);
		}
	} else {
	  printk("#PUSA : imx219_power_get : pdata->mclk_name not found.\n");
	}

	/* analog 2.8v */
	if (pdata->regulators.avdd) {
	  printk("#PUSA : imx219_power_get : pdata->regulators.avdd found.\n");
	  err |= camera_common_regulator_get(dev,
					     &pw->avdd, pdata->regulators.avdd);
	} else {
	  printk("#PUSA : imx219_power_get : pdata->regulators.avdd not found.\n");
	}
	
	/* IO 1.8v */
	if (pdata->regulators.iovdd) {
	  printk("#PUSA : imx219_power_get : pdata->regulators.iovdd found.\n");
	  err |= camera_common_regulator_get(dev,
					     &pw->iovdd, pdata->regulators.iovdd);
	} else {
	  printk("#PUSA : imx219_power_get : pdata->regulators.iovdd not found.\n");
	}
	/* dig 1.2v */
	if (pdata->regulators.dvdd) {
	  printk("#PUSA : imx219_power_get : pdata->regulators.dvdd found.\n");
	  err |= camera_common_regulator_get(dev,
					     &pw->dvdd, pdata->regulators.dvdd);
	} else {
	  printk("#PUSA : imx219_power_get : pdata->regulators.dvdd not found.\n");
	}
	
	if (err) {
	  printk("#PUSA : imx219_power_get : error= %d\n", err);
	  dev_err(dev, "%s: unable to get regulator(s)\n", __func__);
	  goto done;
	} else {
	  printk("#PUSA : imx219_power_get : no error\n");
	}

	/* Reset or ENABLE GPIO */
	pw->reset_gpio = pdata->reset_gpio;
	//err = gpio_request(pw->reset_gpio, "cam_reset_gpio");
	err = 0;
	if (err < 0) {
	  printk("#PUSA : imx219_power_get : gpio_request error= %d\n", err);
	  dev_err(dev, "%s: unable to request reset_gpio (%d)\n",
		  __func__, err);
	  goto done;
	} else {
	  printk("#PUSA : imx219_power_get : gpio_request no error\n");
	}

done:
	pw->state = SWITCH_OFF;
	printk("#PUSA : imx219_power_get : end of function\n");
	return err;
}

static struct camera_common_pdata *imx219_parse_dt(
	struct tegracam_device *tc_dev)
{

	struct device *dev = tc_dev->dev;
	struct device_node *np = dev->of_node;
	struct camera_common_pdata *board_priv_pdata;
	const struct of_device_id *match;
	struct camera_common_pdata *ret = NULL;
	int err = 0;
	int gpio;

	printk("#IMX219 : *imx219_parse_dt");
	if (!np)
		return NULL;

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

	board_priv_pdata = devm_kzalloc(dev,
		sizeof(*board_priv_pdata), GFP_KERNEL);
	if (!board_priv_pdata)
		return NULL;

	gpio = of_get_named_gpio(np, "reset-gpios", 0);
	if (gpio < 0) {
		if (gpio == -EPROBE_DEFER)
			ret = ERR_PTR(-EPROBE_DEFER);
		dev_err(dev, "reset-gpios not found\n");
		goto error;
	}
	board_priv_pdata->reset_gpio = (unsigned int)gpio;

	err = of_property_read_string(np, "mclk", &board_priv_pdata->mclk_name);
	if (err)
		dev_dbg(dev, "mclk name not present, "
			"assume sensor driven externally\n");

	err = of_property_read_string(np, "avdd-reg",
		&board_priv_pdata->regulators.avdd);
	err |= of_property_read_string(np, "iovdd-reg",
		&board_priv_pdata->regulators.iovdd);
	err |= of_property_read_string(np, "dvdd-reg",
		&board_priv_pdata->regulators.dvdd);
	if (err)
		dev_dbg(dev, "avdd, iovdd and/or dvdd reglrs. not present, "
			"assume sensor powered independently\n");

	board_priv_pdata->has_eeprom =
		of_property_read_bool(np, "has-eeprom");

	return board_priv_pdata;

error:
	devm_kfree(dev, board_priv_pdata);

	return ret;
}

static int imx219_set_mode(struct tegracam_device *tc_dev)
{
  
  int err = 0;
  int index = 0;
  printk("#PUSA : imx219_set_mode");
  
  index = -1;
  do {
    index++;
    printk("#PUSA : imx219_set_mode : mode_table[IMX219_MODE_COMMON][%d] : addr = 0x%x, val = 0x%x",
	   index,
	   mode_table[IMX219_MODE_COMMON][index].addr,
	   mode_table[IMX219_MODE_COMMON][index].val);
  } while ((mode_table[IMX219_MODE_COMMON][index].addr != IMX219_TABLE_END) ||
	   (mode_table[IMX219_MODE_COMMON][index].val != 0));

  printk("#PUSA : imx219_set_mode : err = %d", err);
  if (err)
    return err;

  printk("#PUSA : imx219_set_mode : err = %d", err);
  if (err)
    return err;
  
  return 0;
}

static int imx219_start_streaming(struct tegracam_device *tc_dev)
{
  int res = 0;

  printk("#PUSA : imx219_start_streaming");
  printk("#PUSA : imx219_start_streaming : result = %d", res);
  return res;
}

static int imx219_stop_streaming(struct tegracam_device *tc_dev)
{
	int err;

	struct imx219 *priv = (struct imx219 *)tegracam_get_privdata(tc_dev);

	/* --- debug ---*/
	return 0;
	/* --- debug ---*/

	err = imx219_write_table(priv, mode_table[IMX219_STOP_STREAM]);

	usleep_range(50000, 51000);
	printk("#IMX219 : imx219_stop_streaming");
	
	return err;
}

static struct camera_common_sensor_ops imx219_common_ops = {
	.numfrmfmts = ARRAY_SIZE(imx219_frmfmt),
	.frmfmt_table = imx219_frmfmt,
	.power_on = imx219_power_on,
	.power_off = imx219_power_off,
	.write_reg = imx219_write_reg,
	.read_reg = imx219_read_reg,
	.parse_dt = imx219_parse_dt,
	.power_get = imx219_power_get,
	.power_put = imx219_power_put,
	.set_mode = imx219_set_mode,
	.start_streaming = imx219_start_streaming,
	.stop_streaming = imx219_stop_streaming,
};

static int imx219_board_setup(struct imx219 *priv)
{

	struct camera_common_data *s_data = priv->s_data;
	struct camera_common_pdata *pdata = s_data->pdata;
	struct device *dev = s_data->dev;

	//u8 reg_val[2];
	int err = 0;

	printk("#PUSA : imx219_board_setup");
	if (pdata->mclk_name) {
		err = camera_common_mclk_enable(s_data);
		printk("#PUSA : imx219_board_setup : camera_common_mclk_enable result = %d\n", err);
		if (err) {
			dev_err(dev, "error turning on mclk (%d)\n", err);
			goto done;
		}
	} else {
	  printk("#PUSA : imx219_board_setup : mclk_name not setted\n");
	}

	err = imx219_power_on(s_data);
	if (err) {
	  printk("#PUSA : imx219_board_setup : imx219_power_on result = %d. Error!\n", err);
	  dev_err(dev, "error during power on sensor (%d)\n", err);
	  goto err_power_on;
	}
	printk("#PUSA : imx219_board_setup : imx219_power_on result = %d."
	       " Power on is ok. Going on\n", err);
	
	/* Sensor fine integration time */
	err = imx219_get_fine_integ_time(priv, &priv->fine_integ_time);
	if (err) {
	  dev_err(dev, "%s: error querying sensor fine integ. time\n",
		  __func__);
	  printk("#PUSA : imx219_board_setup : imx219_get_fine_integ_time result = %d."
		 " Error!\n", err);
	  
	}

	goto err_reg_probe;
	
err_reg_probe:
	imx219_power_off(s_data);

err_power_on:
	if (pdata->mclk_name)
		camera_common_mclk_disable(s_data);

done:
	return err;
}

static int imx219_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__);
	printk("#IMX219 : imx219_open");
	
	return 0;
}

static const struct v4l2_subdev_internal_ops imx219_subdev_internal_ops = {
	.open = imx219_open,
};

static int imx219_probe(struct i2c_client *client,
			const struct i2c_device_id *id)
{
  
  struct device *dev = &client->dev;
  struct tegracam_device *tc_dev;
  struct imx219 *priv;
  int err;
  
  dev_dbg(dev, "probing v4l2 sensor at addr 0x%0x\n", client->addr);
  printk("#PUSA : imx219_probe");
  printk("#PUSA : imx219_probe : probing v4l2 sensor at addr 0x%0x", client->addr);
  
  if (!IS_ENABLED(CONFIG_OF) || !client->dev.of_node) {
    printk("#PUSA : imx219_probe : not enabled CONFIG_OF or not set client->dev.of_node. Stop");
    return -EINVAL;
  }
  printk("#PUSA : imx219_probe : enabled CONFIG_OF or set client->dev.of_node. Continue.");
  
  printk("#PUSA : imx219_probe : trying to allocate size for structure imx219, variable [priv]");
  priv = devm_kzalloc(dev,
		      sizeof(struct imx219), GFP_KERNEL);
  if (!priv) {
    printk("#PUSA : imx219_probe : no such space for structure. Error!");
    return -ENOMEM;
  }
  printk("#PUSA : imx219_probe : memory allocated. Continue.");
  
  printk("#PUSA : imx219_probe : trying to allocate size for structure tegracam_device, variable [tc_dev].");
  tc_dev = devm_kzalloc(dev,
			sizeof(struct tegracam_device), GFP_KERNEL);
  if (!tc_dev) {
    printk("#PUSA : imx219_probe : no such space for structure. Error!");
    return -ENOMEM;
  }
  printk("#PUSA : imx219_probe : memory for tc_dev allocated. Continue.");
  
  priv->i2c_client = tc_dev->client = client;
  tc_dev->dev = dev;
  strncpy(tc_dev->name, "imx219", sizeof(tc_dev->name));
  tc_dev->dev_regmap_config = &sensor_regmap_config;
  tc_dev->sensor_ops = &imx219_common_ops;
  tc_dev->v4l2sd_internal_ops = &imx219_subdev_internal_ops;
  tc_dev->tcctrl_ops = &imx219_ctrl_ops;
  
  printk("#PUSA : imx219_probe : starting tegracam_device_register");
  err = tegracam_device_register(tc_dev);
  if (err) {
    printk("#PUSA : imx219_probe : tegracam_device_register error code = %d", err);
    dev_err(dev, "tegra camera driver registration failed\n");
    return err;
  } else {
    printk("#PUSA : imx219_probe : tegracam_device_register finished successfully. Continue.\n");
  }
  
  priv->tc_dev = tc_dev;
  priv->s_data = tc_dev->s_data;
  priv->subdev = &tc_dev->s_data->subdev;
  tegracam_set_privdata(tc_dev, (void *)priv);
  
  printk("#PUSA : imx219_probe : trying execute imx219_board_setup");
  err = imx219_board_setup(priv);
  printk("#PUSA : imx219_probe : imx219_board_setup error code = %d", err);
  if (err) {
    printk("#PUSA : imx219_probe : imx219_board_setup error code = %d", err);
    tegracam_device_unregister(tc_dev);
    dev_err(dev, "board setup failed\n");
    return err;
  }

  printk("#PUSA : imx219_probe : tegracam_v4l2subdev_register start\n");
  err = tegracam_v4l2subdev_register(tc_dev, true);
  if (err) {
    printk("#PUSA : imx219_probe : tegracam_v4l2subdev_register error code = %d\n", err);
    dev_err(dev, "tegra camera subdev registration failed\n");
    return err;
  } else {
    printk("#PUSA : imx219_probe : tegracam_v4l2subdev_register no error\n");
  }
  
  dev_dbg(dev, "detected imx219 sensor\n");
  printk("#PUSA : imx219_probe : detected imx219 sensor");
  return 0;
}

static int imx219_remove(struct i2c_client *client)
{

	struct camera_common_data *s_data = to_camera_common_data(&client->dev);
	struct imx219 *priv = (struct imx219 *)s_data->priv;

	tegracam_v4l2subdev_unregister(priv->tc_dev);
	tegracam_device_unregister(priv->tc_dev);
	printk("#IMX219 : imx219_remove");
	
	return 0;
}

static const struct i2c_device_id imx219_id[] = {
	{ "imx219", 0 },
	{ }
};

MODULE_DEVICE_TABLE(i2c, imx219_id);

static struct i2c_driver imx219_i2c_driver = {
	.driver = {
		.name = "pusa",
		.owner = THIS_MODULE,
		.of_match_table = of_match_ptr(imx219_of_match),
	},
	.probe = imx219_probe,
	.remove = imx219_remove,
	.id_table = imx219_id,
};

module_i2c_driver(imx219_i2c_driver);

MODULE_DESCRIPTION("Media Controller driver for Sony IMX219");
MODULE_AUTHOR("NVIDIA Corporation");
MODULE_LICENSE("GPL v2");