1 files changed, 24 insertions, 15 deletions

diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 0195d9969df..faaa0522490 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -302,15 +302,9 @@ static void __omap_i2c_init(struct omap_i2c_dev *dev)
                 omap_i2c_write_reg(dev, OMAP_I2C_IE_REG, dev->iestate);
 }
 
-static int omap_i2c_init(struct omap_i2c_dev *dev)
+static int omap_i2c_reset(struct omap_i2c_dev *dev)
 {
-        u16 psc = 0, scll = 0, sclh = 0;
-        u16 fsscll = 0, fssclh = 0, hsscll = 0, hssclh = 0;
-        unsigned long fclk_rate = 12000000;
         unsigned long timeout;
-        unsigned long internal_clk = 0;
-        struct clk *fclk;
-
         if (dev->rev >= OMAP_I2C_OMAP1_REV_2) {
                 /* Disable I2C controller before soft reset */
                 omap_i2c_write_reg(dev, OMAP_I2C_CON_REG,
@@ -348,14 +342,27 @@ static int omap_i2c_init(struct omap_i2c_dev *dev)
 
                         omap_i2c_write_reg(dev, OMAP_I2C_SYSC_REG,
                                                         dev->syscstate);
-                        /*
-                         * Enabling all wakup sources to stop I2C freezing on
-                         * WFI instruction.
-                         * REVISIT: Some wkup sources might not be needed.
-                         */
-                        dev->westate = OMAP_I2C_WE_ALL;
                 }
         }
+        return 0;
+}
+
+static int omap_i2c_init(struct omap_i2c_dev *dev)
+{
+        u16 psc = 0, scll = 0, sclh = 0;
+        u16 fsscll = 0, fssclh = 0, hsscll = 0, hssclh = 0;
+        unsigned long fclk_rate = 12000000;
+        unsigned long internal_clk = 0;
+        struct clk *fclk;
+
+        if (dev->rev >= OMAP_I2C_REV_ON_3430_3530) {
+                /*
+                 * Enabling all wakup sources to stop I2C freezing on
+                 * WFI instruction.
+                 * REVISIT: Some wkup sources might not be needed.
+                 */
+                dev->westate = OMAP_I2C_WE_ALL;
+        }
 
         if (dev->flags & OMAP_I2C_FLAG_ALWAYS_ARMXOR_CLK) {
                 /*
@@ -599,7 +606,8 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
                                                 OMAP_I2C_TIMEOUT);
         if (timeout == 0) {
                 dev_err(dev->dev, "controller timed out\n");
-                omap_i2c_init(dev);
+                omap_i2c_reset(dev);
+                __omap_i2c_init(dev);
                 return -ETIMEDOUT;
         }
 
@@ -609,7 +617,8 @@ static int omap_i2c_xfer_msg(struct i2c_adapter *adap,
         /* We have an error */
         if (dev->cmd_err & (OMAP_I2C_STAT_AL | OMAP_I2C_STAT_ROVR |
                             OMAP_I2C_STAT_XUDF)) {
-                omap_i2c_init(dev);
+                omap_i2c_reset(dev);
+                __omap_i2c_init(dev);
                 return -EIO;
         }