@@ -16,9 +16,45 @@
#include <i2c.h>
#include <compiler.h>
+static int pmic_select(struct pmic *p)
+{
+ int ret, old_bus;
+
+ old_bus = i2c_get_bus_num();
+ if (old_bus != p->bus) {
+ debug("%s: Select bus %d\n", __func__, p->bus);
+ ret = i2c_set_bus_num(p->bus);
+ if (ret) {
+ debug("%s: Cannot select pmic %s, err %d\n",
+ __func__, p->name, ret);
+ return -1;
+ }
+ }
+
+ return old_bus;
+}
+
+static int pmic_deselect(int old_bus)
+{
+ int ret;
+
+ if (old_bus != i2c_get_bus_num()) {
+ ret = i2c_set_bus_num(old_bus);
+ debug("%s: Select bus %d\n", __func__, old_bus);
+ if (ret) {
+ debug("%s: Cannot restore i2c bus, err %d\n",
+ __func__, ret);
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
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 +88,33 @@ 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);
+ if (old_bus < 0)
return -1;
- return 0;
+ ret = i2c_write(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num);
+ pmic_deselect(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);
+ if (old_bus < 0)
return -1;
+ ret = i2c_read(pmic_i2c_addr, reg, 1, buf, pmic_i2c_tx_num);
+ pmic_deselect(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 +144,15 @@ 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);
+ 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_deselect(old_bus);
+ if (ret) {
printf("Can't find PMIC:%s\n", p->name);
return -1;
}