1 files changed, 81 insertions, 4 deletions

diff --git a/drivers/i2c/busses/i2c-omap.c b/drivers/i2c/busses/i2c-omap.c
index 0e894193accf..fc9bf7f30e35 100644
--- a/drivers/i2c/busses/i2c-omap.c
+++ b/drivers/i2c/busses/i2c-omap.c
@@ -38,6 +38,7 @@
 #include <linux/slab.h>
 #include <linux/i2c-omap.h>
 #include <linux/pm_runtime.h>
+#include <linux/pinctrl/consumer.h>
 
 /* I2C controller revisions */
 #define OMAP_I2C_OMAP1_REV_2            0x20
@@ -481,10 +482,8 @@ static int omap_i2c_wait_for_bb(struct omap_i2c_dev *dev)
 
         timeout = jiffies + OMAP_I2C_TIMEOUT;
         while (omap_i2c_read_reg(dev, OMAP_I2C_STAT_REG) & OMAP_I2C_STAT_BB) {
-                if (time_after(jiffies, timeout)) {
-                        dev_warn(dev->dev, "timeout waiting for bus ready\n");
-                        return -ETIMEDOUT;
-                }
+                if (time_after(jiffies, timeout))
+                        return i2c_recover_bus(&dev->adapter);
                 msleep(1);
         }
 
@@ -1209,6 +1208,79 @@ MODULE_DEVICE_TABLE(of, omap_i2c_of_match);
 #define OMAP_I2C_SCHEME_0               0
 #define OMAP_I2C_SCHEME_1               1
 
+static int omap_i2c_get_scl(struct i2c_adapter *adap)
+{
+        struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+        u32 reg;
+
+        reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+
+        return reg & OMAP_I2C_SYSTEST_SCL_I_FUNC;
+}
+
+static int omap_i2c_get_sda(struct i2c_adapter *adap)
+{
+        struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+        u32 reg;
+
+        reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+
+        return reg & OMAP_I2C_SYSTEST_SDA_I_FUNC;
+}
+
+static void omap_i2c_set_scl(struct i2c_adapter *adap, int val)
+{
+        struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+        u32 reg;
+
+        reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+        if (val)
+                reg |= OMAP_I2C_SYSTEST_SCL_O;
+        else
+                reg &= ~OMAP_I2C_SYSTEST_SCL_O;
+        omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static void omap_i2c_prepare_recovery(struct i2c_adapter *adap)
+{
+        struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+        u32 reg;
+
+        reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+        /* enable test mode */
+        reg |= OMAP_I2C_SYSTEST_ST_EN;
+        /* select SDA/SCL IO mode */
+        reg |= 3 << OMAP_I2C_SYSTEST_TMODE_SHIFT;
+        /* set SCL to high-impedance state (reset value is 0) */
+        reg |= OMAP_I2C_SYSTEST_SCL_O;
+        /* set SDA to high-impedance state (reset value is 0) */
+        reg |= OMAP_I2C_SYSTEST_SDA_O;
+        omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static void omap_i2c_unprepare_recovery(struct i2c_adapter *adap)
+{
+        struct omap_i2c_dev *dev = i2c_get_adapdata(adap);
+        u32 reg;
+
+        reg = omap_i2c_read_reg(dev, OMAP_I2C_SYSTEST_REG);
+        /* restore reset values */
+        reg &= ~OMAP_I2C_SYSTEST_ST_EN;
+        reg &= ~OMAP_I2C_SYSTEST_TMODE_MASK;
+        reg &= ~OMAP_I2C_SYSTEST_SCL_O;
+        reg &= ~OMAP_I2C_SYSTEST_SDA_O;
+        omap_i2c_write_reg(dev, OMAP_I2C_SYSTEST_REG, reg);
+}
+
+static struct i2c_bus_recovery_info omap_i2c_bus_recovery_info = {
+        .get_scl                = omap_i2c_get_scl,
+        .get_sda                = omap_i2c_get_sda,
+        .set_scl                = omap_i2c_set_scl,
+        .prepare_recovery       = omap_i2c_prepare_recovery,
+        .unprepare_recovery     = omap_i2c_unprepare_recovery,
+        .recover_bus            = i2c_generic_scl_recovery,
+};
+
 static int
 omap_i2c_probe(struct platform_device *pdev)
 {
@@ -1358,6 +1430,7 @@ omap_i2c_probe(struct platform_device *pdev)
         adap->algo = &omap_i2c_algo;
         adap->dev.parent = &pdev->dev;
         adap->dev.of_node = pdev->dev.of_node;
+        adap->bus_recovery_info = &omap_i2c_bus_recovery_info;
 
         /* i2c device drivers may be active on return from add_adapter() */
         adap->nr = pdev->id;
@@ -1423,6 +1496,8 @@ static int omap_i2c_runtime_suspend(struct device *dev)
                 omap_i2c_read_reg(_dev, OMAP_I2C_STAT_REG);
         }
 
+        pinctrl_pm_select_sleep_state(dev);
+
         return 0;
 }
 
@@ -1431,6 +1506,8 @@ static int omap_i2c_runtime_resume(struct device *dev)
         struct platform_device *pdev = to_platform_device(dev);
         struct omap_i2c_dev *_dev = platform_get_drvdata(pdev);
 
+        pinctrl_pm_select_default_state(dev);
+
         if (!_dev->regs)
                 return 0;