@@ -16,9 +16,29 @@
#include <i2c.h>
#include <compiler.h>
+static int pmic_select(int bus)
+{
+ int ret, old_bus;
+
+ old_bus = i2c_get_bus_num();
+
+ if (old_bus == bus)
+ return old_bus;
+
+ debug("%s: Select bus %d\n", __func__, bus);
+ ret = i2c_set_bus_num(bus);
+ if (ret) {
+ debug("%s: Cannot select pmic, err %d\n", __func__, ret);
+ return -1;
+ }
+
+ return old_bus;
+}
+
int pmic_reg_write(struct pmic *p, u32 reg, u32 val)
{
unsigned char buf[4] = { 0 };
+ int ret, old_bus;
if (check_reg(p, reg))
return -1;
@@ -52,23 +72,34 @@ int pmic_reg_write(struct pmic *p, u32 reg, u32 val)
return -1;
}
- if (i2c_write(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num))
+ old_bus = pmic_select(p->bus);
+ if (old_bus < 0)
return -1;
- return 0;
+ ret = i2c_write(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num);
+ pmic_select(old_bus);
+
+ return ret;
}
int pmic_reg_read(struct pmic *p, u32 reg, u32 *val)
{
unsigned char buf[4] = { 0 };
u32 ret_val = 0;
+ int ret, old_bus;
if (check_reg(p, reg))
return -1;
- if (i2c_read(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num))
+ old_bus = pmic_select(p->bus);
+ if (old_bus < 0)
return -1;
+ ret = i2c_read(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num);
+ pmic_select(old_bus);
+ if (ret)
+ return ret;
+
switch (pmic_i2c_tx_num) {
case 3:
if (p->sensor_byte_order == PMIC_SENSOR_BYTE_ORDER_BIG)
@@ -98,9 +129,16 @@ int pmic_reg_read(struct pmic *p, u32 reg, u32 *val)
int pmic_probe(struct pmic *p)
{
- i2c_set_bus_num(p->bus);
+ int ret, old_bus;
+
+ old_bus = pmic_select(p->bus);
+ if (old_bus < 0)
+ return -1;
+
debug("Bus: %d PMIC:%s probed!\n", p->bus, p->name);
- if (i2c_probe(pmic_i2c_addr)) {
+ ret = i2c_probe(pmic_i2c_addr);
+ pmic_select(old_bus);
+ if (ret) {
printf("Can't find PMIC:%s\n", p->name);
return -1;
}