[PATCH] i2c: at91: Fix driver for SAMA5D2 SoC

[Alexander Shiyan <eagle.alexander923@gmail.com>]

The current driver version does not work for me on SAMA5D2 SOC.
I2C bus devices probing always returns good state, and as a result all
addresses 0x01-0x7f are always present.
After a some digging into the driver, I found that the NACK bit is not
handled correctly. I also found another SAMA5D2-related patch in the kernel
tree (1).
The result is a complex patch for correct I2C bus operation on SAMA5D2 SOC.

[1] https://squing.com/lander/squing.com/?l=linux-i2c&m=156382173106314&w=3&_=%2F%23wXPKnebvZEv31W5L%2FkOlGQ%3D%3D

Signed-off-by: Alexander Shiyan <eagle.alexander923@gmail.com>
---
 drivers/i2c/busses/i2c-at91.c | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index dbc5aa4ddf..fb0a922a62 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -113,7 +113,8 @@ static void at91_twi_write(struct at91_twi_dev *dev, unsigned reg, unsigned val)
 static void at91_disable_twi_interrupts(struct at91_twi_dev *dev)
 {
 	at91_twi_write(dev, AT91_TWI_IDR,
-		       AT91_TWI_TXCOMP | AT91_TWI_RXRDY | AT91_TWI_TXRDY);
+		       AT91_TWI_TXCOMP | AT91_TWI_RXRDY |
+		       AT91_TWI_TXRDY | AT91_TWI_NACK);
 }
 
 static void at91_init_twi_bus(struct at91_twi_dev *dev)
@@ -223,8 +224,10 @@ static void at91_twi_write_next_byte(struct at91_twi_dev *dev)
 	at91_twi_write(dev, AT91_TWI_THR, *dev->buf);
 
 	/* send stop when last byte has been written */
-	if (--dev->buf_len == 0)
+	if (--dev->buf_len == 0) {
 		at91_twi_write(dev, AT91_TWI_CR, AT91_TWI_STOP);
+		at91_twi_write(dev, AT91_TWI_IDR, AT91_TWI_TXRDY);
+	}
 
 	dev_dbg(dev->dev, "wrote 0x%x, to go %d\n", *dev->buf, dev->buf_len);
 
@@ -276,7 +279,7 @@ static int at91_twi_wait_completion(struct at91_twi_dev *dev)
 
 		dev->transfer_status |= status;
 
-	} while (!(at91_twi_read(dev, AT91_TWI_SR) & AT91_TWI_TXCOMP));
+	} while (!(status & AT91_TWI_TXCOMP));
 
 	at91_disable_twi_interrupts(dev);
 
@@ -311,11 +314,12 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		at91_twi_write(dev, AT91_TWI_CR, start_flags);
 
 		at91_twi_write(dev, AT91_TWI_IER,
-			    AT91_TWI_TXCOMP | AT91_TWI_RXRDY);
+			    AT91_TWI_TXCOMP | AT91_TWI_NACK | AT91_TWI_RXRDY);
 	} else {
 		at91_twi_write_next_byte(dev);
 		at91_twi_write(dev, AT91_TWI_IER,
-			    AT91_TWI_TXCOMP | AT91_TWI_TXRDY);
+			       AT91_TWI_TXCOMP | AT91_TWI_NACK |
+			       (dev->buf_len ? AT91_TWI_TXRDY : 0));
 	}
 
 	ret = at91_twi_wait_completion(dev);
-- 
2.39.1