[RESEND 2/3] i2c: at91: keep the controller disabled when it is not used

[Codrin Ciubotariu]

The controller (master mode) should be disabled when it is not used, to
avoid potential glitches on the bus. The controller must be disabled
only when the controller is not running.
This fixes an issue on Microchip's SAMA5D4 platform when pinctrl changes
the I2C's pin states between GPIO and I2C, done when GPIO recovery is used.

Fixes: 813e30e9ab91 ("i2c: New Atmel AT91 bus driver")
Signed-off-by: Codrin Ciubotariu <codrin.ciubotariu at microchip.com>
---
 drivers/i2c/busses/i2c-at91-master.c | 34 ++++++++++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/drivers/i2c/busses/i2c-at91-master.c b/drivers/i2c/busses/i2c-at91-master.c
index 34df1614bc37..14c2a4856978 100644
--- a/drivers/i2c/busses/i2c-at91-master.c
+++ b/drivers/i2c/busses/i2c-at91-master.c
@@ -38,7 +38,7 @@ void at91_init_twi_bus_master(struct at91_twi_dev *dev)
 	/* FIFO should be enabled immediately after the software reset */
 	if (dev->fifo_size)
 		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_FIFOEN);
-	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSEN);
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSDIS);
 	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_SVDIS);
 	at91_twi_write(dev, AT91_TWI_CWGR, dev->twi_cwgr_reg);
 
@@ -593,7 +593,6 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 	if (time_left == 0) {
 		dev->transfer_status |= at91_twi_read(dev, AT91_TWI_SR);
 		dev_err(dev->dev, "controller timed out\n");
-		at91_init_twi_bus(dev);
 		ret = -ETIMEDOUT;
 		goto error;
 	}
@@ -642,6 +641,26 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 	return ret;
 }
 
+static void at91_twi_disable(struct at91_twi_dev *dev)
+{
+	/* return if previous operation is completed */
+	if (!(at91_twi_read(dev, AT91_TWI_SR) & AT91_TWI_TXCOMP)) {
+		/* wait for previous command to complete before disabling the controller */
+		dev_dbg(dev->dev, "wait for command to complete...\n");
+		reinit_completion(&dev->cmd_complete);
+		dev->transfer_status = 0;
+		at91_twi_write(dev, AT91_TWI_IER, AT91_TWI_TXCOMP);
+		wait_for_completion_timeout(&dev->cmd_complete, dev->adapter.timeout);
+		if (!(dev->transfer_status & AT91_TWI_TXCOMP)) {
+			dev_dbg(dev->dev, "IP still busy, resetting the controller...\n");
+			at91_init_twi_bus(dev);
+			return;
+		}
+	}
+
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSDIS);
+}
+
 static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 {
 	struct at91_twi_dev *dev = i2c_get_adapdata(adap);
@@ -657,6 +676,8 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	if (ret < 0)
 		goto out;
 
+	at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_MSEN);
+
 	if (num == 2) {
 		int internal_address = 0;
 		int i;
@@ -710,13 +731,22 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	i2c_put_dma_safe_msg_buf(dma_buf, m_start, !ret);
 
 	if (ret < 0) {
+		/* disable controller before using GPIO recovery */
+		if (!dev->pdata->has_clear_cmd)
+			at91_twi_disable(dev);
+
 		/*
 		 * some faulty I2C slave devices might hold SDA down;
 		 * we can send a bus clear command, hoping that the pins will be
 		 * released
 		 */
 		i2c_recover_bus(&dev->adapter);
+
+		/* disable controller if not disabled before */
+		if (dev->pdata->has_clear_cmd)
+			at91_twi_disable(dev);
 	} else {
+		at91_twi_disable(dev);
 		ret = num;
 	}
 
-- 
2.34.1