summaryrefslogtreecommitdiff
path: root/drivers/media/i2c/smiapp/smiapp-regs.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/media/i2c/smiapp/smiapp-regs.c')
-rw-r--r--drivers/media/i2c/smiapp/smiapp-regs.c71
1 files changed, 20 insertions, 51 deletions
diff --git a/drivers/media/i2c/smiapp/smiapp-regs.c b/drivers/media/i2c/smiapp/smiapp-regs.c
index ce8c1d47fbf0..1b58b7c6c839 100644
--- a/drivers/media/i2c/smiapp/smiapp-regs.c
+++ b/drivers/media/i2c/smiapp/smiapp-regs.c
@@ -8,6 +8,8 @@
* Contact: Sakari Ailus <sakari.ailus@iki.fi>
*/
+#include <asm/unaligned.h>
+
#include <linux/delay.h>
#include <linux/i2c.h>
@@ -69,18 +71,19 @@ static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
{
struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
struct i2c_msg msg;
- unsigned char data[4];
- u16 offset = reg;
+ unsigned char data_buf[sizeof(u32)] = { 0 };
+ unsigned char offset_buf[sizeof(u16)];
int r;
+ if (len > sizeof(data_buf))
+ return -EINVAL;
+
msg.addr = client->addr;
msg.flags = 0;
- msg.len = 2;
- msg.buf = data;
+ msg.len = sizeof(offset_buf);
+ msg.buf = offset_buf;
+ put_unaligned_be16(reg, offset_buf);
- /* high byte goes out first */
- data[0] = (u8) (offset >> 8);
- data[1] = (u8) offset;
r = i2c_transfer(client->adapter, &msg, 1);
if (r != 1) {
if (r >= 0)
@@ -90,6 +93,8 @@ static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
msg.len = len;
msg.flags = I2C_M_RD;
+ msg.buf = &data_buf[sizeof(data_buf) - len];
+
r = i2c_transfer(client->adapter, &msg, 1);
if (r != 1) {
if (r >= 0)
@@ -97,27 +102,12 @@ static int ____smiapp_read(struct smiapp_sensor *sensor, u16 reg,
goto err;
}
- *val = 0;
- /* high byte comes first */
- switch (len) {
- case SMIAPP_REG_32BIT:
- *val = (data[0] << 24) + (data[1] << 16) + (data[2] << 8) +
- data[3];
- break;
- case SMIAPP_REG_16BIT:
- *val = (data[0] << 8) + data[1];
- break;
- case SMIAPP_REG_8BIT:
- *val = data[0];
- break;
- default:
- BUG();
- }
+ *val = get_unaligned_be32(data_buf);
return 0;
err:
- dev_err(&client->dev, "read from offset 0x%x error %d\n", offset, r);
+ dev_err(&client->dev, "read from offset 0x%x error %d\n", reg, r);
return r;
}
@@ -158,7 +148,7 @@ static int __smiapp_read(struct smiapp_sensor *sensor, u32 reg, u32 *val,
&& len != SMIAPP_REG_32BIT)
return -EINVAL;
- if (len == SMIAPP_REG_8BIT || !only8)
+ if (!only8)
rval = ____smiapp_read(sensor, SMIAPP_REG_ADDR(reg), len, val);
else
rval = ____smiapp_read_8only(sensor, SMIAPP_REG_ADDR(reg), len,
@@ -214,13 +204,10 @@ int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
struct i2c_msg msg;
unsigned char data[6];
unsigned int retries;
- u8 flags = SMIAPP_REG_FLAGS(reg);
u8 len = SMIAPP_REG_WIDTH(reg);
- u16 offset = SMIAPP_REG_ADDR(reg);
int r;
- if ((len != SMIAPP_REG_8BIT && len != SMIAPP_REG_16BIT &&
- len != SMIAPP_REG_32BIT) || flags)
+ if (len > sizeof(data) - 2)
return -EINVAL;
msg.addr = client->addr;
@@ -228,27 +215,8 @@ int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
msg.len = 2 + len;
msg.buf = data;
- /* high byte goes out first */
- data[0] = (u8) (reg >> 8);
- data[1] = (u8) (reg & 0xff);
-
- switch (len) {
- case SMIAPP_REG_8BIT:
- data[2] = val;
- break;
- case SMIAPP_REG_16BIT:
- data[2] = val >> 8;
- data[3] = val;
- break;
- case SMIAPP_REG_32BIT:
- data[2] = val >> 24;
- data[3] = val >> 16;
- data[4] = val >> 8;
- data[5] = val;
- break;
- default:
- BUG();
- }
+ put_unaligned_be16(SMIAPP_REG_ADDR(reg), data);
+ put_unaligned_be32(val << (8 * (sizeof(val) - len)), data + 2);
for (retries = 0; retries < 5; retries++) {
/*
@@ -269,7 +237,8 @@ int smiapp_write_no_quirk(struct smiapp_sensor *sensor, u32 reg, u32 val)
}
dev_err(&client->dev,
- "wrote 0x%x to offset 0x%x error %d\n", val, offset, r);
+ "wrote 0x%x to offset 0x%x error %d\n", val,
+ SMIAPP_REG_ADDR(reg), r);
return r;
}