1 files changed, 31 insertions, 17 deletions

diff --git a/drivers/i2c/algos/i2c-algo-pcf.c b/drivers/i2c/algos/i2c-algo-pcf.c
index 8907b0191677..1e328d19cd6d 100644
--- a/drivers/i2c/algos/i2c-algo-pcf.c
+++ b/drivers/i2c/algos/i2c-algo-pcf.c
@@ -78,6 +78,36 @@ static void i2c_stop(struct i2c_algo_pcf_data *adap)
         set_pcf(adap, 1, I2C_PCF_STOP);
 }
 
+static void handle_lab(struct i2c_algo_pcf_data *adap, const int *status)
+{
+        DEB2(printk(KERN_INFO
+                "i2c-algo-pcf.o: lost arbitration (CSR 0x%02x)\n",
+                 *status));
+
+        /* Cleanup from LAB -- reset and enable ESO.
+         * This resets the PCF8584; since we've lost the bus, no
+         * further attempts should be made by callers to clean up
+         * (no i2c_stop() etc.)
+         */
+        set_pcf(adap, 1, I2C_PCF_PIN);
+        set_pcf(adap, 1, I2C_PCF_ESO);
+
+        /* We pause for a time period sufficient for any running
+         * I2C transaction to complete -- the arbitration logic won't
+         * work properly until the next START is seen.
+         * It is assumed the bus driver or client has set a proper value.
+         *
+         * REVISIT: should probably use msleep instead of mdelay if we
+         * know we can sleep.
+         */
+        if (adap->lab_mdelay)
+                mdelay(adap->lab_mdelay);
+
+        DEB2(printk(KERN_INFO
+                "i2c-algo-pcf.o: reset LAB condition (CSR 0x%02x)\n",
+                get_pcf(adap, 1)));
+}
+
 static int wait_for_bb(struct i2c_algo_pcf_data *adap) {
 
         int timeout = DEF_TIMEOUT;
@@ -109,23 +139,7 @@ static int wait_for_pin(struct i2c_algo_pcf_data *adap, int *status) {
                 *status = get_pcf(adap, 1);
         }
         if (*status & I2C_PCF_LAB) {
-                DEB2(printk(KERN_INFO 
-                        "i2c-algo-pcf.o: lost arbitration (CSR 0x%02x)\n",
-                         *status));
-                /* Cleanup from LAB-- reset and enable ESO.
-                 * This resets the PCF8584; since we've lost the bus, no
-                 * further attempts should be made by callers to clean up 
-                 * (no i2c_stop() etc.)
-                 */
-                set_pcf(adap, 1, I2C_PCF_PIN);
-                set_pcf(adap, 1, I2C_PCF_ESO);
-                /* TODO: we should pause for a time period sufficient for any
-                 * running I2C transaction to complete-- the arbitration
-                 * logic won't work properly until the next START is seen.
-                 */
-                DEB2(printk(KERN_INFO 
-                        "i2c-algo-pcf.o: reset LAB condition (CSR 0x%02x)\n", 
-                        get_pcf(adap,1)));
+                handle_lab(adap, status);
                 return(-EINTR);
         }
 #endif