/**
* m5mols_read - I2C read function
* @reg: combination of size, category and command for the I2C packet
* @size: desired size of I2C packet
* @val: read value
*
* Returns 0 on success, or else negative errno.
*/
static int m5mols_read(struct v4l2_subdev *sd, u32 size, u32 reg, u32 *val)
{
struct i2c_client *client = v4l2_get_subdevdata(sd);
struct m5mols_info *info = to_m5mols(sd);
u8 rbuf[M5MOLS_I2C_MAX_SIZE + 1];
u8 category = I2C_CATEGORY(reg);
u8 cmd = I2C_COMMAND(reg);
struct i2c_msg msg[2];
u8 wbuf[5];
int ret;
if (!client->adapter)
return -ENODEV;
msg[0].addr = client->addr;
msg[0].flags = 0;
msg[0].len = 5;
msg[0].buf = wbuf;
wbuf[0] = 5;
wbuf[1] = M5MOLS_BYTE_READ;
wbuf[2] = category;
wbuf[3] = cmd;
wbuf[4] = size;
msg[1].addr = client->addr;
msg[1].flags = I2C_M_RD;
msg[1].len = size + 1;
msg[1].buf = rbuf;
/* minimum stabilization time */
usleep_range(200, 200);
ret = i2c_transfer(client->adapter, msg, 2);
if (ret == 2) {
*val = m5mols_swap_byte(&rbuf[1], size);
return 0;
}
if (info->isp_ready)
v4l2_err(sd, "read failed: size:%d cat:%02x cmd:%02x. %d\n",
size, category, cmd, ret);
return ret < 0 ? ret : -EIO;
}
/**
* m5mols_write - I2C command write function
* @reg: combination of size, category and command for the I2C packet
* @val: value to write
*
* Returns 0 on success, or else negative errno.
*/
int m5mols_write(struct v4l2_subdev *sd, u32 reg, u32 val)
{
struct i2c_client *client = v4l2_get_subdevdata(sd);
struct m5mols_info *info = to_m5mols(sd);
u8 wbuf[M5MOLS_I2C_MAX_SIZE + 4];
u8 category = I2C_CATEGORY(reg);
u8 cmd = I2C_COMMAND(reg);
u8 size = I2C_SIZE(reg);
u32 *buf = (u32 *)&wbuf[4];
struct i2c_msg msg[1];
int ret;
if (!client->adapter)
return -ENODEV;
if (size != 1 && size != 2 && size != 4) {
v4l2_err(sd, "Wrong data size\n");
return -EINVAL;
}
msg->addr = client->addr;
msg->flags = 0;
msg->len = (u16)size + 4;
msg->buf = wbuf;
wbuf[0] = size + 4;
wbuf[1] = M5MOLS_BYTE_WRITE;
wbuf[2] = category;
wbuf[3] = cmd;
*buf = m5mols_swap_byte((u8 *)&val, size);
usleep_range(200, 200);
ret = i2c_transfer(client->adapter, msg, 1);
if (ret == 1)
return 0;
if (info->isp_ready)
v4l2_err(sd, "write failed: cat:%02x cmd:%02x ret:%d\n",
category, cmd, ret);
return ret < 0 ? ret : -EIO;
}
