drivers/media/platform/msm/camera/cam_sensor_module/cam_sensor_io/cam_sensor_qup_i2c.c - kernel/msm-4.9 - Gitiles

 /* Copyright (c) 2017, The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
  * only version 2 as published by the Free Software Foundation.
  *
  * This program is distributed in the hope that 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.
  */

 #include "cam_sensor_cmn_header.h"
 #include "cam_sensor_i2c.h"
 #include "cam_sensor_io.h"

 #define I2C_REG_DATA_MAX       (8*1024)
 #define I2C_REG_MAX_BUF_SIZE   8

 static int32_t cam_qup_i2c_rxdata(
 	struct i2c_client *dev_client, unsigned char *rxdata,
 	enum camera_sensor_i2c_type addr_type,
 	int data_length)
 {
 	int32_t rc = 0;
 	uint16_t saddr = dev_client->addr >> 1;
 	struct i2c_msg msgs[] = {
 		{
 			.addr  = saddr,
 			.flags = 0,
 			.len   = addr_type,
 			.buf   = rxdata,
 		},
 		{
 			.addr  = saddr,
 			.flags = I2C_M_RD,
 			.len   = data_length,
 			.buf   = rxdata,
 		},
 	};
 	rc = i2c_transfer(dev_client->adapter, msgs, 2);
 	if (rc < 0)
 		pr_err("%s:failed 0x%x\n", __func__, saddr);
 	return rc;
 }


 static int32_t cam_qup_i2c_txdata(
 	struct camera_io_master *dev_client, unsigned char *txdata,
 	int length)
 {
 	int32_t rc = 0;
 	uint16_t saddr = dev_client->client->addr >> 1;
 	struct i2c_msg msg[] = {
 		{
 			.addr = saddr,
 			.flags = 0,
 			.len = length,
 			.buf = txdata,
 		 },
 	};
 	rc = i2c_transfer(dev_client->client->adapter, msg, 1);
 	if (rc < 0)
 		pr_err("%s: failed 0x%x\n", __func__, saddr);
 	return rc;
 }

 int32_t cam_qup_i2c_read(struct i2c_client *client,
 	uint32_t addr, uint32_t *data,
 	enum camera_sensor_i2c_type addr_type,
 	enum camera_sensor_i2c_type data_type)
 {
 	int32_t rc = -EINVAL;
 	unsigned char *buf = NULL;

 	if (addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
 		|| data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| data_type >= CAMERA_SENSOR_I2C_TYPE_MAX) {
 		pr_err("ERR: %s Failed with addr/data_type verfication\n",
 			__func__);
 		return rc;
 	}

 	buf = kzalloc(addr_type + data_type, GFP_KERNEL);

 	if (!buf)
 		return -ENOMEM;

 	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
 		buf[0] = addr;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
 		buf[0] = addr >> 8;
 		buf[1] = addr;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
 		buf[0] = addr >> 16;
 		buf[1] = addr >> 8;
 		buf[2] = addr;
 	} else {
 		buf[0] = addr >> 24;
 		buf[1] = addr >> 16;
 		buf[2] = addr >> 8;
 		buf[3] = addr;
 	}

 	rc = cam_qup_i2c_rxdata(client, buf, addr_type, data_type);
 	if (rc < 0) {
 		pr_err("%s fail\n", __func__);
 		goto read_fail;
 	}

 	if (data_type == CAMERA_SENSOR_I2C_TYPE_BYTE)
 		*data = buf[0];
 	else if (data_type == CAMERA_SENSOR_I2C_TYPE_WORD)
 		*data = buf[0] << 8 | buf[1];
 	else if (data_type == CAMERA_SENSOR_I2C_TYPE_3B)
 		*data = buf[0] << 16 | buf[1] << 8 | buf[2];
 	else
 		*data = buf[0] << 24 | buf[1] << 16 |
 			buf[2] << 8 | buf[3];

 	CDBG("%s addr = 0x%x data: 0x%x\n", __func__, addr, *data);
 read_fail:
 	kfree(buf);
 	buf = NULL;
 	return rc;
 }

 int32_t cam_qup_i2c_read_seq(struct i2c_client *client,
 	uint32_t addr, uint8_t *data,
 	enum camera_sensor_i2c_type addr_type,
 	uint32_t num_byte)
 {
 	int32_t rc = -EFAULT;
 	unsigned char *buf = NULL;
 	int i;

 	if (addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX) {
 		pr_err("ERR: %s Failed with addr_type verification\n",
 			__func__);
 		return rc;
 	}

 	if ((num_byte == 0) || (num_byte > I2C_REG_DATA_MAX)) {
 		pr_err("%s: Error num_byte:0x%x max supported:0x%x\n",
 			__func__, num_byte, I2C_REG_DATA_MAX);
 		return rc;
 	}

 	buf = kzalloc(addr_type + num_byte, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;

 	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
 		buf[0] = addr;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
 		buf[0] = addr >> BITS_PER_BYTE;
 		buf[1] = addr;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
 		buf[0] = addr >> 16;
 		buf[1] = addr >> 8;
 		buf[2] = addr;
 	} else {
 		buf[0] = addr >> 24;
 		buf[1] = addr >> 16;
 		buf[2] = addr >> 8;
 		buf[3] = addr;
 	}

 	rc = cam_qup_i2c_rxdata(client, buf, addr_type, num_byte);
 	if (rc < 0) {
 		pr_err("%s fail\n", __func__);
 		goto read_seq_fail;
 	}

 	for (i = 0; i < num_byte; i++)
 		data[i] = buf[i];

 read_seq_fail:
 	kfree(buf);
 	buf = NULL;
 	return rc;
 }

 static int32_t cam_qup_i2c_compare(struct i2c_client *client,
 	uint32_t addr, uint32_t data, uint16_t data_mask,
 	enum camera_sensor_i2c_type data_type,
 	enum camera_sensor_i2c_type addr_type)
 {
 	int32_t rc;
 	uint32_t reg_data = 0;

 	rc = cam_qup_i2c_read(client, addr, &reg_data,
 		addr_type, data_type);
 	if (rc < 0)
 		return rc;

 	reg_data = reg_data & 0xFFFF;
 	if (data != (reg_data & ~data_mask))
 		return I2C_COMPARE_MISMATCH;

 	return I2C_COMPARE_MATCH;
 }

 int32_t cam_qup_i2c_poll(struct i2c_client *client,
 	uint32_t addr, uint16_t data, uint16_t data_mask,
 	enum camera_sensor_i2c_type addr_type,
 	enum camera_sensor_i2c_type data_type,
 	uint32_t delay_ms)
 {
 	int32_t rc = 0;
 	int i = 0;

 	if ((delay_ms > MAX_POLL_DELAY_MS) || (delay_ms == 0)) {
 		pr_err("%s:%d invalid delay = %d max_delay = %d\n",
 			__func__, __LINE__, delay_ms, MAX_POLL_DELAY_MS);
 		return -EINVAL;
 	}

 	if ((addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
 		|| data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| data_type >= CAMERA_SENSOR_I2C_TYPE_MAX))
 		return -EINVAL;

 	for (i = 0; i < delay_ms; i++) {
 		rc = cam_qup_i2c_compare(client,
 			addr, data, data_mask, data_type, addr_type);
 		if (rc == I2C_COMPARE_MATCH)
 			return rc;

 		usleep_range(1000, 1010);
 	}
 	/* If rc is MISMATCH then read is successful but poll is failure */
 	if (rc == I2C_COMPARE_MISMATCH)
 		pr_err("%s:%d poll failed rc=%d(non-fatal)\n",
 			__func__, __LINE__, rc);
 	if (rc < 0)
 		pr_err("%s:%d poll failed rc=%d\n", __func__, __LINE__, rc);

 	return rc;
 }

 static int32_t cam_qup_i2c_write(struct camera_io_master *client,
 	struct cam_sensor_i2c_reg_array *reg_setting,
 	enum camera_sensor_i2c_type addr_type,
 	enum camera_sensor_i2c_type data_type)
 {
 	int32_t rc = 0;
 	unsigned char buf[I2C_REG_MAX_BUF_SIZE];
 	uint8_t len = 0;

 	CDBG("%s reg addr = 0x%x data type: %d\n",
 			  __func__, reg_setting->reg_addr, data_type);
 	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
 		buf[0] = reg_setting->reg_addr;
 		CDBG("%s byte %d: 0x%x\n", __func__,
 			len, buf[len]);
 		len = 1;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
 		buf[0] = reg_setting->reg_addr >> 8;
 		buf[1] = reg_setting->reg_addr;
 		CDBG("%s byte %d: 0x%x\n", __func__,
 			len, buf[len]);
 		CDBG("%s byte %d: 0x%x\n", __func__,
 			len+1, buf[len+1]);
 		len = 2;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
 		buf[0] = reg_setting->reg_addr >> 16;
 		buf[1] = reg_setting->reg_addr >> 8;
 		buf[2] = reg_setting->reg_addr;
 		len = 3;
 	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_DWORD) {
 		buf[0] = reg_setting->reg_addr >> 24;
 		buf[1] = reg_setting->reg_addr >> 16;
 		buf[2] = reg_setting->reg_addr >> 8;
 		buf[3] = reg_setting->reg_addr;
 		len = 4;
 	} else {
 		pr_err("%s: Invalid I2C addr type\n", __func__);
 		return -EINVAL;
 	}

 	CDBG("Data: 0x%x\n", reg_setting->reg_data);
 	if (data_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
 		buf[len] = reg_setting->reg_data;
 		CDBG("Byte %d: 0x%x\n", len, buf[len]);
 		len += 1;
 	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
 		buf[len] = reg_setting->reg_data >> 8;
 		buf[len+1] = reg_setting->reg_data;
 		CDBG("Byte %d: 0x%x\n", len, buf[len]);
 		CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
 		len += 2;
 	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_3B) {
 		buf[len] = reg_setting->reg_data >> 16;
 		buf[len + 1] = reg_setting->reg_data >> 8;
 		buf[len + 2] = reg_setting->reg_data;
 		CDBG("Byte %d: 0x%x\n", len, buf[len]);
 		CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
 		CDBG("Byte %d: 0x%x\n", len+2, buf[len+2]);
 		len += 3;
 	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_DWORD) {
 		buf[len] = reg_setting->reg_data >> 24;
 		buf[len + 1] = reg_setting->reg_data >> 16;
 		buf[len + 2] = reg_setting->reg_data >> 8;
 		buf[len + 3] = reg_setting->reg_data;
 		CDBG("Byte %d: 0x%x\n", len, buf[len]);
 		CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
 		CDBG("Byte %d: 0x%x\n", len+2, buf[len+2]);
 		CDBG("Byte %d: 0x%x\n", len+3, buf[len+3]);
 		len += 4;
 	} else {
 		pr_err("%s: Invalid Data Type\n", __func__);
 		return -EINVAL;
 	}

 	rc = cam_qup_i2c_txdata(client, buf, len);
 	if (rc < 0)
 		pr_err("%s fail\n", __func__);
 	return rc;
 }

 int32_t cam_qup_i2c_write_table(struct camera_io_master *client,
 	struct cam_sensor_i2c_reg_setting *write_setting)
 {
 	int i;
 	int32_t rc = -EINVAL;
 	struct cam_sensor_i2c_reg_array *reg_setting;

 	if (!client || !write_setting)
 		return rc;

 	if ((write_setting->addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| write_setting->addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
 		|| (write_setting->data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
 		|| write_setting->data_type >= CAMERA_SENSOR_I2C_TYPE_MAX)))
 		return rc;

 	reg_setting = write_setting->reg_setting;

 	for (i = 0; i < write_setting->size; i++) {
 		CDBG("%s addr 0x%x data 0x%x\n", __func__,
 			reg_setting->reg_addr, reg_setting->reg_data);

 		rc = cam_qup_i2c_write(client, reg_setting,
 			write_setting->addr_type, write_setting->data_type);
 		if (rc < 0)
 			break;
 		reg_setting++;
 	}

 	if (write_setting->delay > 20)
 		msleep(write_setting->delay);
 	else if (write_setting->delay)
 		usleep_range(write_setting->delay * 1000, (write_setting->delay
 			* 1000) + 1000);

 	return rc;
 }
	/* Copyright (c) 2017, The Linux Foundation. All rights reserved.
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License version 2 and
	* only version 2 as published by the Free Software Foundation.
	*
	* This program is distributed in the hope that 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.
	*/

	#include "cam_sensor_cmn_header.h"
	#include "cam_sensor_i2c.h"
	#include "cam_sensor_io.h"

	#define I2C_REG_DATA_MAX (8*1024)
	#define I2C_REG_MAX_BUF_SIZE 8

	static int32_t cam_qup_i2c_rxdata(
	struct i2c_client dev_client, unsigned char rxdata,
	enum camera_sensor_i2c_type addr_type,
	int data_length)
	{
	int32_t rc = 0;
	uint16_t saddr = dev_client->addr >> 1;
	struct i2c_msg msgs[] = {
	{
	.addr = saddr,
	.flags = 0,
	.len = addr_type,
	.buf = rxdata,
	},
	{
	.addr = saddr,
	.flags = I2C_M_RD,
	.len = data_length,
	.buf = rxdata,
	},
	};
	rc = i2c_transfer(dev_client->adapter, msgs, 2);
	if (rc < 0)
	pr_err("%s:failed 0x%x\n", __func__, saddr);
	return rc;
	}


	static int32_t cam_qup_i2c_txdata(
	struct camera_io_master dev_client, unsigned char txdata,
	int length)
	{
	int32_t rc = 0;
	uint16_t saddr = dev_client->client->addr >> 1;
	struct i2c_msg msg[] = {
	{
	.addr = saddr,
	.flags = 0,
	.len = length,
	.buf = txdata,
	},
	};
	rc = i2c_transfer(dev_client->client->adapter, msg, 1);
	if (rc < 0)
	pr_err("%s: failed 0x%x\n", __func__, saddr);
	return rc;
	}

	int32_t cam_qup_i2c_read(struct i2c_client *client,
	uint32_t addr, uint32_t *data,
	enum camera_sensor_i2c_type addr_type,
	enum camera_sensor_i2c_type data_type)
	{
	int32_t rc = -EINVAL;
	unsigned char *buf = NULL;

	if (addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
	\|\| data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| data_type >= CAMERA_SENSOR_I2C_TYPE_MAX) {
	pr_err("ERR: %s Failed with addr/data_type verfication\n",
	__func__);
	return rc;
	}

	buf = kzalloc(addr_type + data_type, GFP_KERNEL);

	if (!buf)
	return -ENOMEM;

	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
	buf[0] = addr;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
	buf[0] = addr >> 8;
	buf[1] = addr;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
	buf[0] = addr >> 16;
	buf[1] = addr >> 8;
	buf[2] = addr;
	} else {
	buf[0] = addr >> 24;
	buf[1] = addr >> 16;
	buf[2] = addr >> 8;
	buf[3] = addr;
	}

	rc = cam_qup_i2c_rxdata(client, buf, addr_type, data_type);
	if (rc < 0) {
	pr_err("%s fail\n", __func__);
	goto read_fail;
	}

	if (data_type == CAMERA_SENSOR_I2C_TYPE_BYTE)
	*data = buf[0];
	else if (data_type == CAMERA_SENSOR_I2C_TYPE_WORD)
	*data = buf[0] << 8 \| buf[1];
	else if (data_type == CAMERA_SENSOR_I2C_TYPE_3B)
	*data = buf[0] << 16 \| buf[1] << 8 \| buf[2];
	else
	*data = buf[0] << 24 \| buf[1] << 16 \|
	buf[2] << 8 \| buf[3];

	CDBG("%s addr = 0x%x data: 0x%x\n", __func__, addr, *data);
	read_fail:
	kfree(buf);
	buf = NULL;
	return rc;
	}

	int32_t cam_qup_i2c_read_seq(struct i2c_client *client,
	uint32_t addr, uint8_t *data,
	enum camera_sensor_i2c_type addr_type,
	uint32_t num_byte)
	{
	int32_t rc = -EFAULT;
	unsigned char *buf = NULL;
	int i;

	if (addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX) {
	pr_err("ERR: %s Failed with addr_type verification\n",
	__func__);
	return rc;
	}

	if ((num_byte == 0) \|\| (num_byte > I2C_REG_DATA_MAX)) {
	pr_err("%s: Error num_byte:0x%x max supported:0x%x\n",
	__func__, num_byte, I2C_REG_DATA_MAX);
	return rc;
	}

	buf = kzalloc(addr_type + num_byte, GFP_KERNEL);
	if (!buf)
	return -ENOMEM;

	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
	buf[0] = addr;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
	buf[0] = addr >> BITS_PER_BYTE;
	buf[1] = addr;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
	buf[0] = addr >> 16;
	buf[1] = addr >> 8;
	buf[2] = addr;
	} else {
	buf[0] = addr >> 24;
	buf[1] = addr >> 16;
	buf[2] = addr >> 8;
	buf[3] = addr;
	}

	rc = cam_qup_i2c_rxdata(client, buf, addr_type, num_byte);
	if (rc < 0) {
	pr_err("%s fail\n", __func__);
	goto read_seq_fail;
	}

	for (i = 0; i < num_byte; i++)
	data[i] = buf[i];

	read_seq_fail:
	kfree(buf);
	buf = NULL;
	return rc;
	}

	static int32_t cam_qup_i2c_compare(struct i2c_client *client,
	uint32_t addr, uint32_t data, uint16_t data_mask,
	enum camera_sensor_i2c_type data_type,
	enum camera_sensor_i2c_type addr_type)
	{
	int32_t rc;
	uint32_t reg_data = 0;

	rc = cam_qup_i2c_read(client, addr, &reg_data,
	addr_type, data_type);
	if (rc < 0)
	return rc;

	reg_data = reg_data & 0xFFFF;
	if (data != (reg_data & ~data_mask))
	return I2C_COMPARE_MISMATCH;

	return I2C_COMPARE_MATCH;
	}

	int32_t cam_qup_i2c_poll(struct i2c_client *client,
	uint32_t addr, uint16_t data, uint16_t data_mask,
	enum camera_sensor_i2c_type addr_type,
	enum camera_sensor_i2c_type data_type,
	uint32_t delay_ms)
	{
	int32_t rc = 0;
	int i = 0;

	if ((delay_ms > MAX_POLL_DELAY_MS) \|\| (delay_ms == 0)) {
	pr_err("%s:%d invalid delay = %d max_delay = %d\n",
	__func__, __LINE__, delay_ms, MAX_POLL_DELAY_MS);
	return -EINVAL;
	}

	if ((addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
	\|\| data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| data_type >= CAMERA_SENSOR_I2C_TYPE_MAX))
	return -EINVAL;

	for (i = 0; i < delay_ms; i++) {
	rc = cam_qup_i2c_compare(client,
	addr, data, data_mask, data_type, addr_type);
	if (rc == I2C_COMPARE_MATCH)
	return rc;

	usleep_range(1000, 1010);
	}
	/* If rc is MISMATCH then read is successful but poll is failure */
	if (rc == I2C_COMPARE_MISMATCH)
	pr_err("%s:%d poll failed rc=%d(non-fatal)\n",
	__func__, __LINE__, rc);
	if (rc < 0)
	pr_err("%s:%d poll failed rc=%d\n", __func__, __LINE__, rc);

	return rc;
	}

	static int32_t cam_qup_i2c_write(struct camera_io_master *client,
	struct cam_sensor_i2c_reg_array *reg_setting,
	enum camera_sensor_i2c_type addr_type,
	enum camera_sensor_i2c_type data_type)
	{
	int32_t rc = 0;
	unsigned char buf[I2C_REG_MAX_BUF_SIZE];
	uint8_t len = 0;

	CDBG("%s reg addr = 0x%x data type: %d\n",
	__func__, reg_setting->reg_addr, data_type);
	if (addr_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
	buf[0] = reg_setting->reg_addr;
	CDBG("%s byte %d: 0x%x\n", __func__,
	len, buf[len]);
	len = 1;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
	buf[0] = reg_setting->reg_addr >> 8;
	buf[1] = reg_setting->reg_addr;
	CDBG("%s byte %d: 0x%x\n", __func__,
	len, buf[len]);
	CDBG("%s byte %d: 0x%x\n", __func__,
	len+1, buf[len+1]);
	len = 2;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_3B) {
	buf[0] = reg_setting->reg_addr >> 16;
	buf[1] = reg_setting->reg_addr >> 8;
	buf[2] = reg_setting->reg_addr;
	len = 3;
	} else if (addr_type == CAMERA_SENSOR_I2C_TYPE_DWORD) {
	buf[0] = reg_setting->reg_addr >> 24;
	buf[1] = reg_setting->reg_addr >> 16;
	buf[2] = reg_setting->reg_addr >> 8;
	buf[3] = reg_setting->reg_addr;
	len = 4;
	} else {
	pr_err("%s: Invalid I2C addr type\n", __func__);
	return -EINVAL;
	}

	CDBG("Data: 0x%x\n", reg_setting->reg_data);
	if (data_type == CAMERA_SENSOR_I2C_TYPE_BYTE) {
	buf[len] = reg_setting->reg_data;
	CDBG("Byte %d: 0x%x\n", len, buf[len]);
	len += 1;
	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_WORD) {
	buf[len] = reg_setting->reg_data >> 8;
	buf[len+1] = reg_setting->reg_data;
	CDBG("Byte %d: 0x%x\n", len, buf[len]);
	CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
	len += 2;
	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_3B) {
	buf[len] = reg_setting->reg_data >> 16;
	buf[len + 1] = reg_setting->reg_data >> 8;
	buf[len + 2] = reg_setting->reg_data;
	CDBG("Byte %d: 0x%x\n", len, buf[len]);
	CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
	CDBG("Byte %d: 0x%x\n", len+2, buf[len+2]);
	len += 3;
	} else if (data_type == CAMERA_SENSOR_I2C_TYPE_DWORD) {
	buf[len] = reg_setting->reg_data >> 24;
	buf[len + 1] = reg_setting->reg_data >> 16;
	buf[len + 2] = reg_setting->reg_data >> 8;
	buf[len + 3] = reg_setting->reg_data;
	CDBG("Byte %d: 0x%x\n", len, buf[len]);
	CDBG("Byte %d: 0x%x\n", len+1, buf[len+1]);
	CDBG("Byte %d: 0x%x\n", len+2, buf[len+2]);
	CDBG("Byte %d: 0x%x\n", len+3, buf[len+3]);
	len += 4;
	} else {
	pr_err("%s: Invalid Data Type\n", __func__);
	return -EINVAL;
	}

	rc = cam_qup_i2c_txdata(client, buf, len);
	if (rc < 0)
	pr_err("%s fail\n", __func__);
	return rc;
	}

	int32_t cam_qup_i2c_write_table(struct camera_io_master *client,
	struct cam_sensor_i2c_reg_setting *write_setting)
	{
	int i;
	int32_t rc = -EINVAL;
	struct cam_sensor_i2c_reg_array *reg_setting;

	if (!client \|\| !write_setting)
	return rc;

	if ((write_setting->addr_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| write_setting->addr_type >= CAMERA_SENSOR_I2C_TYPE_MAX
	\|\| (write_setting->data_type <= CAMERA_SENSOR_I2C_TYPE_INVALID
	\|\| write_setting->data_type >= CAMERA_SENSOR_I2C_TYPE_MAX)))
	return rc;

	reg_setting = write_setting->reg_setting;

	for (i = 0; i < write_setting->size; i++) {
	CDBG("%s addr 0x%x data 0x%x\n", __func__,
	reg_setting->reg_addr, reg_setting->reg_data);

	rc = cam_qup_i2c_write(client, reg_setting,
	write_setting->addr_type, write_setting->data_type);
	if (rc < 0)
	break;
	reg_setting++;
	}

	if (write_setting->delay > 20)
	msleep(write_setting->delay);
	else if (write_setting->delay)
	usleep_range(write_setting->delay * 1000, (write_setting->delay
	* 1000) + 1000);

	return rc;
	}