@@ -179,6 +179,34 @@ static int fti2c010_probe(struct i2c_adapter *adap, u8 dev)
return ret;
}
+static void addr_to_i2c_data(u8 *buf, uint32_t val, int len)
+{
+ if (!buf || len <= 0)
+ return;
+
+ /* MSB first */
+ switch (len) {
+ case 1:
+ buf[0] = (u8)(val & 0xff);
+ break;
+ case 2:
+ buf[0] = (u8)((val >> 8) & 0xff);
+ buf[1] = (u8)(val & 0xff);
+ break;
+ case 3:
+ buf[0] = (u8)((val >> 16) & 0xff);
+ buf[1] = (u8)((val >> 8) & 0xff);
+ buf[2] = (u8)(val & 0xff);
+ break;
+ default:
+ buf[0] = (u8)((val >> 24) & 0xff);
+ buf[1] = (u8)((val >> 16) & 0xff);
+ buf[2] = (u8)((val >> 8) & 0xff);
+ buf[3] = (u8)(val & 0xff);
+ break;
+ }
+}
+
static int fti2c010_read(struct i2c_adapter *adap,
u8 dev, uint addr, int alen, uchar *buf, int len)
{
@@ -187,10 +215,7 @@ static int fti2c010_read(struct i2c_adapter *adap,
int ret, pos;
uchar paddr[4];
- paddr[0] = (addr >> 0) & 0xFF;
- paddr[1] = (addr >> 8) & 0xFF;
- paddr[2] = (addr >> 16) & 0xFF;
- paddr[3] = (addr >> 24) & 0xFF;
+ addr_to_i2c_data(paddr, addr, alen);
/*
* Phase A. Set register address
@@ -252,10 +277,7 @@ static int fti2c010_write(struct i2c_adapter *adap,
int ret, pos;
uchar paddr[4];
- paddr[0] = (addr >> 0) & 0xFF;
- paddr[1] = (addr >> 8) & 0xFF;
- paddr[2] = (addr >> 16) & 0xFF;
- paddr[3] = (addr >> 24) & 0xFF;
+ addr_to_i2c_data(paddr, addr, alen);
/*
* Phase A. Set register address