[PATCH v2 5/6] i2c: algo-bit: check if the bus is busy

[Jan Luebbe]

If we have a timeout while waiting, try to recover.

Signed-off-by: Jan Luebbe <jluebbe at debian.org>
---
 drivers/i2c/algos/i2c-algo-bit.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/i2c/algos/i2c-algo-bit.c b/drivers/i2c/algos/i2c-algo-bit.c
index 62040cc..2563c0d 100644
--- a/drivers/i2c/algos/i2c-algo-bit.c
+++ b/drivers/i2c/algos/i2c-algo-bit.c
@@ -117,6 +117,31 @@ done:
 	return 0;
 }
 
+static int wait_busy(struct i2c_algo_bit_data *adap)
+{
+	uint64_t start;
+
+	if (sclhi(adap) < 0)
+		return -ETIMEDOUT;
+
+	start = get_time_ns();
+	while (!getsda(adap)) {
+		if (is_timeout(start, adap->timeout_ms * MSECOND)) {
+			if (getsda(adap))
+				break;
+			return -ETIMEDOUT;
+		}
+	}
+#ifdef DEBUG
+	if ((get_time_ns() - start) < 10000)
+		pr_debug("i2c-algo-bit: needed %u usecs for SDA to go "
+			 "high\n", (unsigned int)(get_time_ns() - start) /
+			 1000);
+#endif
+
+	udelay(adap->udelay);
+	return 0;
+}
 
 /* --- other auxiliary functions --------------------------------------	*/
 static void i2c_start(struct i2c_algo_bit_data *adap)
@@ -512,6 +537,13 @@ static int bit_xfer(struct i2c_adapter *i2c_adap,
 			return ret;
 	}
 
+	if (wait_busy(adap) < 0) { /* timeout */
+		dev_warn(&i2c_adap->dev, "timeout waiting for bus ready\n");
+		ret = i2c_recover_bus(i2c_adap);
+		if (ret < 0)
+			return ret;
+	}
+
 	bit_dbg(3, &i2c_adap->dev, "emitting start condition\n");
 	i2c_start(adap);
 	for (i = 0; i < num; i++) {
-- 
2.1.4