1 files changed, 33 insertions, 16 deletions

diff --git a/drivers/i2c/busses/i2c-nforce2.c b/drivers/i2c/busses/i2c-nforce2.c
index c48140f782d0..1c63c4f2e4b6 100644
--- a/drivers/i2c/busses/i2c-nforce2.c
+++ b/drivers/i2c/busses/i2c-nforce2.c
@@ -98,15 +98,40 @@ struct nforce2_smbus {
 #define NVIDIA_SMB_PRTCL_BLOCK_DATA             0x0a
 #define NVIDIA_SMB_PRTCL_PEC                    0x80
 
+/* Misc definitions */
+#define MAX_TIMEOUT     100
+
 static struct pci_driver nforce2_driver;
 
+static int nforce2_check_status(struct i2c_adapter *adap)
+{
+        struct nforce2_smbus *smbus = adap->algo_data;
+        int timeout = 0;
+        unsigned char temp;
+
+        do {
+                msleep(1);
+                temp = inb_p(NVIDIA_SMB_STS);
+        } while ((!temp) && (timeout++ < MAX_TIMEOUT));
+
+        if (timeout >= MAX_TIMEOUT) {
+                dev_dbg(&adap->dev, "SMBus Timeout!\n");
+                return -1;
+        }
+        if (!(temp & NVIDIA_SMB_STS_DONE) || (temp & NVIDIA_SMB_STS_STATUS)) {
+                dev_dbg(&adap->dev, "Transaction failed (0x%02x)!\n", temp);
+                return -1;
+        }
+        return 0;
+}
+
 /* Return -1 on error */
 static s32 nforce2_access(struct i2c_adapter * adap, u16 addr,
                 unsigned short flags, char read_write,
                 u8 command, int size, union i2c_smbus_data * data)
 {
         struct nforce2_smbus *smbus = adap->algo_data;
-        unsigned char protocol, pec, temp;
+        unsigned char protocol, pec;
         u8 len;
         int i;
 
@@ -170,21 +195,8 @@ static s32 nforce2_access(struct i2c_adapter * adap, u16 addr,
         outb_p((addr & 0x7f) << 1, NVIDIA_SMB_ADDR);
         outb_p(protocol, NVIDIA_SMB_PRTCL);
 
-        temp = inb_p(NVIDIA_SMB_STS);
-
-        if (~temp & NVIDIA_SMB_STS_DONE) {
-                udelay(500);
-                temp = inb_p(NVIDIA_SMB_STS);
-        }
-        if (~temp & NVIDIA_SMB_STS_DONE) {
-                msleep(10);
-                temp = inb_p(NVIDIA_SMB_STS);
-        }
-
-        if ((~temp & NVIDIA_SMB_STS_DONE) || (temp & NVIDIA_SMB_STS_STATUS)) {
-                dev_dbg(&adap->dev, "SMBus Timeout! (0x%02x)\n", temp);
+        if (nforce2_check_status(adap))
                 return -1;
-        }
 
         if (read_write == I2C_SMBUS_WRITE)
                 return 0;
@@ -202,7 +214,12 @@ static s32 nforce2_access(struct i2c_adapter * adap, u16 addr,
 
                 case I2C_SMBUS_BLOCK_DATA:
                         len = inb_p(NVIDIA_SMB_BCNT);
-                        len = min_t(u8, len, I2C_SMBUS_BLOCK_MAX);
+                        if ((len <= 0) || (len > I2C_SMBUS_BLOCK_MAX)) {
+                                dev_err(&adap->dev, "Transaction failed "
+                                        "(received block size: 0x%02x)\n",
+                                        len);
+                                return -1;
+                        }
                         for (i = 0; i < len; i++)
                                 data->block[i+1] = inb_p(NVIDIA_SMB_DATA + i);
                         data->block[0] = len;