ENGR00163704: MX5X: add i2c recovery function in board_lateinit.

This patch add a i2c bus recovery function, the i2c bus busy
because some device pull down the I2C SDA line. This happens
when Host is reading some byte from slave, and then host is
reset/reboot.

Since in this case, device is controlling i2c SDA line, the
only thing host can do this give the clock on SCL and sending
NAK, and STOP to finish this transaction.

To fix this issue:
when we found SDA is low, we generate 8 clock to let device
send data, then send a NAK, and STOP to finish this I2C
transaction , after this the clock will be clean.

Signed-off-by: Zhang Jiejing <jiejing.zhang@freescale.com>

2 files changed, 562 insertions, 0 deletions

diff --git a/board/freescale/mx53_loco/mx53_loco.c b/board/freescale/mx53_loco/mx53_loco.c
index b5e4480..6da3c1e 100644
--- a/board/freescale/mx53_loco/mx53_loco.c
+++ b/board/freescale/mx53_loco/mx53_loco.c
@@ -803,12 +803,297 @@ int check_recovery_cmd_file(void)
 }
 #endif
 
+#ifdef CONFIG_I2C_MXC
+/* Note: udelay() is not accurate for i2c timing */
+static void __udelay(int time)
+{
+       int i, j;
+
+       for (i = 0; i < time; i++) {
+	       for (j = 0; j < 200; j++) {
+		       asm("nop");
+		       asm("nop");
+	       }
+       }
+}
+#define I2C1_SDA_GPIO5_26_BIT_MASK  (1 << 26)
+#define I2C1_SCL_GPIO5_27_BIT_MASK  (1 << 27)
+#define I2C2_SCL_GPIO4_12_BIT_MASK  (1 << 12)
+#define I2C2_SDA_GPIO4_13_BIT_MASK  (1 << 13)
+#define I2C3_SCL_GPIO1_3_BIT_MASK   (1 << 3)
+#define I2C3_SDA_GPIO1_6_BIT_MASK   (1 << 6)
+static void mx53_i2c_gpio_scl_direction(int bus, int output)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		mxc_request_iomux(MX53_PIN_CSI0_D9, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO5_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C1_SCL_GPIO5_27_BIT_MASK;
+		else
+			reg &= ~I2C1_SCL_GPIO5_27_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 2:
+		mxc_request_iomux(MX53_PIN_KEY_COL3, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO4_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C2_SCL_GPIO4_12_BIT_MASK;
+		else
+			reg &= ~I2C2_SCL_GPIO4_12_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 3:
+		mxc_request_iomux(MX53_PIN_GPIO_3, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO1_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C3_SCL_GPIO1_3_BIT_MASK;
+		else
+			reg &= I2C3_SCL_GPIO1_3_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_GDIR);
+		break;
+	}
+}
+
+/* set 1 to output, sent 0 to input */
+static void mx53_i2c_gpio_sda_direction(int bus, int output)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		mxc_request_iomux(MX53_PIN_CSI0_D8, IOMUX_CONFIG_ALT1);
+
+		reg = readl(GPIO5_BASE_ADDR + GPIO_GDIR);
+		if (output) {
+			mxc_iomux_set_pad(MX53_PIN_CSI0_D8,
+					  PAD_CTL_ODE_OPENDRAIN_ENABLE |
+					  PAD_CTL_DRV_HIGH | PAD_CTL_100K_PU);
+			reg |= I2C1_SDA_GPIO5_26_BIT_MASK;
+		} else
+			reg &= ~I2C1_SDA_GPIO5_26_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 2:
+		mxc_request_iomux(MX53_PIN_KEY_ROW3, IOMUX_CONFIG_ALT1);
+
+		mxc_iomux_set_pad(MX53_PIN_KEY_ROW3,
+				  PAD_CTL_SRE_FAST |
+				  PAD_CTL_ODE_OPENDRAIN_ENABLE |
+				  PAD_CTL_DRV_HIGH | PAD_CTL_100K_PU |
+				  PAD_CTL_HYS_ENABLE);
+
+		reg = readl(GPIO4_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C2_SDA_GPIO4_13_BIT_MASK;
+		else
+			reg &= ~I2C2_SDA_GPIO4_13_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_GDIR);
+	case 3:
+		mxc_request_iomux(MX53_PIN_GPIO_6, IOMUX_CONFIG_ALT1);
+		mxc_iomux_set_pad(MX53_PIN_GPIO_6,
+				  PAD_CTL_PUE_PULL | PAD_CTL_PKE_ENABLE |
+				  PAD_CTL_DRV_HIGH | PAD_CTL_360K_PD |
+				  PAD_CTL_HYS_ENABLE);
+		reg = readl(GPIO1_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C3_SDA_GPIO1_6_BIT_MASK;
+		else
+			reg &= ~I2C3_SDA_GPIO1_6_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_GDIR);
+	default:
+		break;
+	}
+}
+
+/* set 1 to high 0 to low */
+static void mx53_i2c_gpio_scl_set_level(int bus, int high)
+{
+	u32 reg;
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C1_SCL_GPIO5_27_BIT_MASK;
+		else
+			reg &= ~I2C1_SCL_GPIO5_27_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_DR);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C2_SCL_GPIO4_12_BIT_MASK;
+		else
+			reg &= ~I2C2_SCL_GPIO4_12_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_DR);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C3_SCL_GPIO1_3_BIT_MASK;
+		else
+			reg &= ~I2C3_SCL_GPIO1_3_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_DR);
+		break;
+	}
+}
+
+/* set 1 to high 0 to low */
+static void mx53_i2c_gpio_sda_set_level(int bus, int high)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C1_SDA_GPIO5_26_BIT_MASK;
+		else
+			reg &= ~I2C1_SDA_GPIO5_26_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_DR);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C2_SDA_GPIO4_13_BIT_MASK;
+		else
+			reg &= ~I2C2_SDA_GPIO4_13_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_DR);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C3_SDA_GPIO1_6_BIT_MASK;
+		else
+			reg &= ~I2C3_SDA_GPIO1_6_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_DR);
+		break;
+	}
+}
+
+static int mx53_i2c_gpio_check_sda(int bus)
+{
+	u32 reg;
+	int result = 0;
+
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C1_SDA_GPIO5_26_BIT_MASK);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C2_SDA_GPIO4_13_BIT_MASK);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C3_SDA_GPIO1_6_BIT_MASK);
+		break;
+	}
+
+	return result;
+}
+
+
+ /* Random reboot cause i2c SDA low issue:
+  * the i2c bus busy because some device pull down the I2C SDA
+  * line. This happens when Host is reading some byte from slave, and
+  * then host is reset/reboot. Since in this case, device is
+  * controlling i2c SDA line, the only thing host can do this give the
+  * clock on SCL and sending NAK, and STOP to finish this
+  * transaction.
+  *
+  * How to fix this issue:
+  * detect if the SDA was low on bus send 8 dummy clock, and 1
+  * clock + NAK, and STOP to finish i2c transaction the pending
+  * transfer.
+  */
+int i2c_bus_recovery(void)
+{
+	int i, bus, result = 0;
+
+	for (bus = 1; bus <= 3; bus++) {
+		mx53_i2c_gpio_sda_direction(bus, 0);
+
+		if (mx53_i2c_gpio_check_sda(bus) == 0) {
+			printf("i2c: I2C%d SDA is low, start i2c recovery...\n", bus);
+			mx53_i2c_gpio_scl_direction(bus, 1);
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(10000);
+
+			for (i = 0; i < 9; i++) {
+				mx53_i2c_gpio_scl_set_level(bus, 1);
+				__udelay(5);
+				mx53_i2c_gpio_scl_set_level(bus, 0);
+				__udelay(5);
+			}
+
+			/* 9th clock here, the slave should already
+			   release the SDA, we can set SDA as high to
+			   a NAK.*/
+			mx53_i2c_gpio_sda_direction(bus, 1);
+			mx53_i2c_gpio_sda_set_level(bus, 1);
+			__udelay(1); /* Pull up SDA first */
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(5); /* plus pervious 1 us */
+			mx53_i2c_gpio_scl_set_level(bus, 0);
+			__udelay(5);
+			mx53_i2c_gpio_sda_set_level(bus, 0);
+			__udelay(5);
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(5);
+			/* Here: SCL is high, and SDA from low to high, it's a
+			 * stop condition */
+			mx53_i2c_gpio_sda_set_level(bus, 1);
+			__udelay(5);
+
+			mx53_i2c_gpio_sda_direction(bus, 0);
+			if (mx53_i2c_gpio_check_sda(bus) == 1)
+				printf("I2C%d Recovery success\n", bus);
+			else {
+				printf("I2C%d Recovery failed, I2C1 SDA still low!!!\n", bus);
+				result |= 1 << bus;
+			}
+		}
+
+		/* configure back to i2c */
+		switch (bus) {
+		case 1:
+			setup_i2c(I2C1_BASE_ADDR);
+			break;
+		case 2:
+			setup_i2c(I2C2_BASE_ADDR);
+			break;
+		case 3:
+			setup_i2c(I2C3_BASE_ADDR);
+			break;
+		}
+	}
+
+	return result;
+}
+#endif
+/* restore VUSB 2V5 active after suspend */
+#define BUCKPERI_RESTORE_SW_STEP   (0x55)
+/* restore VUSB 2V5 power supply after suspend */
+#define SUPPLY_RESTORE_VPERISW_EN  (0x20)
+#define DA9052_ID1213_REG                  (35)
+#define DA9052_SUPPLY_REG                  (60)
+
 int board_late_init(void)
 {
 	uchar value;
 	unsigned char buf[4] = { 0 };
 	int retries = 10, ret = -1;
 
+#ifdef CONFIG_I2C_MXC
+	/* first recovery I2C bus in case other device in some i2c
+	 * transcation */
+	i2c_bus_recovery();
+#endif
+
 	if (!i2c_probe(0x8)) {
 		if (i2c_read(0x8, 24, 1, &buf[0], 3)) {
 			printf("%s:i2c_read:error\n", __func__);
diff --git a/board/freescale/mx53_smd/mx53_smd.c b/board/freescale/mx53_smd/mx53_smd.c
index 4c8ca4e..089b4bc 100644
--- a/board/freescale/mx53_smd/mx53_smd.c
+++ b/board/freescale/mx53_smd/mx53_smd.c
@@ -500,6 +500,280 @@ int i2c_write_check(uchar chip, uint addr, int alen, uchar *buf, int len)
 	}
 }
 
+#ifdef CONFIG_I2C_MXC
+/* Note: udelay() is not accurate for i2c timing */
+static void __udelay(int time)
+{
+       int i, j;
+
+       for (i = 0; i < time; i++) {
+	       for (j = 0; j < 200; j++) {
+		       asm("nop");
+		       asm("nop");
+	       }
+       }
+}
+
+#define I2C1_SDA_GPIO5_26_BIT_MASK  (1 << 26)
+#define I2C1_SCL_GPIO5_27_BIT_MASK  (1 << 27)
+#define I2C2_SCL_GPIO4_12_BIT_MASK  (1 << 12)
+#define I2C2_SDA_GPIO4_13_BIT_MASK  (1 << 13)
+#define I2C3_SCL_GPIO1_3_BIT_MASK   (1 << 3)
+#define I2C3_SDA_GPIO1_6_BIT_MASK   (1 << 6)
+static void mx53_i2c_gpio_scl_direction(int bus, int output)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		mxc_request_iomux(MX53_PIN_CSI0_D9, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO5_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C1_SCL_GPIO5_27_BIT_MASK;
+		else
+			reg &= ~I2C1_SCL_GPIO5_27_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 2:
+		mxc_request_iomux(MX53_PIN_KEY_COL3, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO4_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C2_SCL_GPIO4_12_BIT_MASK;
+		else
+			reg &= ~I2C2_SCL_GPIO4_12_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 3:
+		mxc_request_iomux(MX53_PIN_GPIO_3, IOMUX_CONFIG_ALT1);
+		reg = readl(GPIO1_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C3_SCL_GPIO1_3_BIT_MASK;
+		else
+			reg &= I2C3_SCL_GPIO1_3_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_GDIR);
+		break;
+	}
+}
+
+/* set 1 to output, sent 0 to input */
+static void mx53_i2c_gpio_sda_direction(int bus, int output)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		mxc_request_iomux(MX53_PIN_CSI0_D8, IOMUX_CONFIG_ALT1);
+
+		reg = readl(GPIO5_BASE_ADDR + GPIO_GDIR);
+		if (output) {
+			mxc_iomux_set_pad(MX53_PIN_CSI0_D8,
+					  PAD_CTL_ODE_OPENDRAIN_ENABLE |
+					  PAD_CTL_DRV_HIGH | PAD_CTL_100K_PU);
+			reg |= I2C1_SDA_GPIO5_26_BIT_MASK;
+		} else
+			reg &= ~I2C1_SDA_GPIO5_26_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_GDIR);
+		break;
+	case 2:
+		mxc_request_iomux(MX53_PIN_KEY_ROW3, IOMUX_CONFIG_ALT1);
+
+		mxc_iomux_set_pad(MX53_PIN_KEY_ROW3,
+				  PAD_CTL_SRE_FAST |
+				  PAD_CTL_ODE_OPENDRAIN_ENABLE |
+				  PAD_CTL_DRV_HIGH | PAD_CTL_100K_PU |
+				  PAD_CTL_HYS_ENABLE);
+
+		reg = readl(GPIO4_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C2_SDA_GPIO4_13_BIT_MASK;
+		else
+			reg &= ~I2C2_SDA_GPIO4_13_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_GDIR);
+	case 3:
+		mxc_request_iomux(MX53_PIN_GPIO_6, IOMUX_CONFIG_ALT1);
+		mxc_iomux_set_pad(MX53_PIN_GPIO_6,
+				  PAD_CTL_PUE_PULL | PAD_CTL_PKE_ENABLE |
+				  PAD_CTL_DRV_HIGH | PAD_CTL_360K_PD |
+				  PAD_CTL_HYS_ENABLE);
+		reg = readl(GPIO1_BASE_ADDR + GPIO_GDIR);
+		if (output)
+			reg |= I2C3_SDA_GPIO1_6_BIT_MASK;
+		else
+			reg &= ~I2C3_SDA_GPIO1_6_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_GDIR);
+	default:
+		break;
+	}
+}
+
+/* set 1 to high 0 to low */
+static void mx53_i2c_gpio_scl_set_level(int bus, int high)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C1_SCL_GPIO5_27_BIT_MASK;
+		else
+			reg &= ~I2C1_SCL_GPIO5_27_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_DR);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C2_SCL_GPIO4_12_BIT_MASK;
+		else
+			reg &= ~I2C2_SCL_GPIO4_12_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_DR);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C3_SCL_GPIO1_3_BIT_MASK;
+		else
+			reg &= ~I2C3_SCL_GPIO1_3_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_DR);
+		break;
+	}
+}
+
+/* set 1 to high 0 to low */
+static void mx53_i2c_gpio_sda_set_level(int bus, int high)
+{
+	u32 reg;
+
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C1_SDA_GPIO5_26_BIT_MASK;
+		else
+			reg &= ~I2C1_SDA_GPIO5_26_BIT_MASK;
+		writel(reg, GPIO5_BASE_ADDR + GPIO_DR);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C2_SDA_GPIO4_13_BIT_MASK;
+		else
+			reg &= ~I2C2_SDA_GPIO4_13_BIT_MASK;
+		writel(reg, GPIO4_BASE_ADDR + GPIO_DR);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_DR);
+		if (high)
+			reg |= I2C3_SDA_GPIO1_6_BIT_MASK;
+		else
+			reg &= ~I2C3_SDA_GPIO1_6_BIT_MASK;
+		writel(reg, GPIO1_BASE_ADDR + GPIO_DR);
+		break;
+	}
+}
+
+static int mx53_i2c_gpio_check_sda(int bus)
+{
+	u32 reg;
+	int result = 0;
+
+	switch (bus) {
+	case 1:
+		reg = readl(GPIO5_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C1_SDA_GPIO5_26_BIT_MASK);
+		break;
+	case 2:
+		reg = readl(GPIO4_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C2_SDA_GPIO4_13_BIT_MASK);
+		break;
+	case 3:
+		reg = readl(GPIO1_BASE_ADDR + GPIO_PSR);
+		result = !!(reg & I2C3_SDA_GPIO1_6_BIT_MASK);
+		break;
+	}
+
+	return result;
+}
+
+ /* Random reboot cause i2c SDA low issue:
+  * the i2c bus busy because some device pull down the I2C SDA
+  * line. This happens when Host is reading some byte from slave, and
+  * then host is reset/reboot. Since in this case, device is
+  * controlling i2c SDA line, the only thing host can do this give the
+  * clock on SCL and sending NAK, and STOP to finish this
+  * transaction.
+  *
+  * How to fix this issue:
+  * detect if the SDA was low on bus send 8 dummy clock, and 1
+  * clock + NAK, and STOP to finish i2c transaction the pending
+  * transfer.
+  */
+int i2c_bus_recovery(void)
+{
+	int i, bus, result = 0;
+
+	for (bus = 1; bus <= 3; bus++) {
+		mx53_i2c_gpio_sda_direction(bus, 0);
+
+		if (mx53_i2c_gpio_check_sda(bus) == 0) {
+			printf("i2c: I2C%d SDA is low, start i2c recovery...\n", bus);
+			mx53_i2c_gpio_scl_direction(bus, 1);
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(10000);
+
+			for (i = 0; i < 9; i++) {
+				mx53_i2c_gpio_scl_set_level(bus, 1);
+				__udelay(5);
+				mx53_i2c_gpio_scl_set_level(bus, 0);
+				__udelay(5);
+			}
+
+			/* 9th clock here, the slave should already
+			   release the SDA, we can set SDA as high to
+			   a NAK.*/
+			mx53_i2c_gpio_sda_direction(bus, 1);
+			mx53_i2c_gpio_sda_set_level(bus, 1);
+			__udelay(1); /* Pull up SDA first */
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(5); /* plus pervious 1 us */
+			mx53_i2c_gpio_scl_set_level(bus, 0);
+			__udelay(5);
+			mx53_i2c_gpio_sda_set_level(bus, 0);
+			__udelay(5);
+			mx53_i2c_gpio_scl_set_level(bus, 1);
+			__udelay(5);
+			/* Here: SCL is high, and SDA from low to high, it's a
+			 * stop condition */
+			mx53_i2c_gpio_sda_set_level(bus, 1);
+			__udelay(5);
+
+			mx53_i2c_gpio_sda_direction(bus, 0);
+			if (mx53_i2c_gpio_check_sda(bus) == 1)
+				printf("I2C%d Recovery success\n", bus);
+			else {
+				printf("I2C%d Recovery failed, I2C1 SDA still low!!!\n", bus);
+				result |= 1 << bus;
+			}
+		}
+
+		/* configure back to i2c */
+		switch (bus) {
+		case 1:
+			setup_i2c(I2C1_BASE_ADDR);
+			break;
+		case 2:
+			setup_i2c(I2C2_BASE_ADDR);
+			break;
+		case 3:
+			setup_i2c(I2C3_BASE_ADDR);
+			break;
+		}
+	}
+
+	return result;
+}
+#endif
+
 /* restore VUSB 2V5 active after suspend */
 #define BUCKPERI_RESTORE_SW_STEP   (0x55)
 /* restore VUSB 2V5 power supply after suspend */
@@ -1098,6 +1372,9 @@ int board_late_init(void)
 {
 #ifdef CONFIG_I2C_MXC
 	setup_i2c(CONFIG_SYS_I2C_PORT);
+	/* first recovery I2C bus in case other device are reset
+	 * during transcation */
+	i2c_bus_recovery();
 	/* Increase VDDGP voltage */
 	setup_pmic_voltages();
 	/* Switch to 1GHZ */