1 files changed, 14 insertions, 19 deletions

diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index b8c5187b9ee0..d52d84937ad3 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -97,7 +97,6 @@ enum {
 enum {
         MV64XXX_I2C_ACTION_INVALID,
         MV64XXX_I2C_ACTION_CONTINUE,
-        MV64XXX_I2C_ACTION_OFFLOAD_SEND_START,
         MV64XXX_I2C_ACTION_SEND_START,
         MV64XXX_I2C_ACTION_SEND_RESTART,
         MV64XXX_I2C_ACTION_OFFLOAD_RESTART,
@@ -204,6 +203,9 @@ static int mv64xxx_i2c_offload_msg(struct mv64xxx_i2c_data *drv_data)
         unsigned long ctrl_reg;
         struct i2c_msg *msg = drv_data->msgs;
 
+        if (!drv_data->offload_enabled)
+                return -EOPNOTSUPP;
+
         drv_data->msg = msg;
         drv_data->byte_posn = 0;
         drv_data->bytes_left = msg->len;
@@ -433,8 +435,7 @@ mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
 
                 drv_data->msgs++;
                 drv_data->num_msgs--;
-                if (!(drv_data->offload_enabled &&
-                                mv64xxx_i2c_offload_msg(drv_data))) {
+                if (mv64xxx_i2c_offload_msg(drv_data) < 0) {
                         drv_data->cntl_bits |= MV64XXX_I2C_REG_CONTROL_START;
                         writel(drv_data->cntl_bits,
                         drv_data->reg_base + drv_data->reg_offsets.control);
@@ -458,15 +459,14 @@ mv64xxx_i2c_do_action(struct mv64xxx_i2c_data *drv_data)
                         drv_data->reg_base + drv_data->reg_offsets.control);
                 break;
 
-        case MV64XXX_I2C_ACTION_OFFLOAD_SEND_START:
-                if (!mv64xxx_i2c_offload_msg(drv_data))
-                        break;
-                else
-                        drv_data->action = MV64XXX_I2C_ACTION_SEND_START;
-                /* FALLTHRU */
         case MV64XXX_I2C_ACTION_SEND_START:
-                writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_START,
-                        drv_data->reg_base + drv_data->reg_offsets.control);
+                /* Can we offload this msg ? */
+                if (mv64xxx_i2c_offload_msg(drv_data) < 0) {
+                        /* No, switch to standard path */
+                        mv64xxx_i2c_prepare_for_io(drv_data, drv_data->msgs);
+                        writel(drv_data->cntl_bits | MV64XXX_I2C_REG_CONTROL_START,
+                                drv_data->reg_base + drv_data->reg_offsets.control);
+                }
                 break;
 
         case MV64XXX_I2C_ACTION_SEND_ADDR_1:
@@ -625,15 +625,10 @@ mv64xxx_i2c_execute_msg(struct mv64xxx_i2c_data *drv_data, struct i2c_msg *msg,
         unsigned long   flags;
 
         spin_lock_irqsave(&drv_data->lock, flags);
-        if (drv_data->offload_enabled) {
-                drv_data->action = MV64XXX_I2C_ACTION_OFFLOAD_SEND_START;
-                drv_data->state = MV64XXX_I2C_STATE_WAITING_FOR_START_COND;
-        } else {
-                mv64xxx_i2c_prepare_for_io(drv_data, msg);
 
-                drv_data->action = MV64XXX_I2C_ACTION_SEND_START;
-                drv_data->state = MV64XXX_I2C_STATE_WAITING_FOR_START_COND;
-        }
+        drv_data->action = MV64XXX_I2C_ACTION_SEND_START;
+        drv_data->state = MV64XXX_I2C_STATE_WAITING_FOR_START_COND;
+
         drv_data->send_stop = is_last;
         drv_data->block = 1;
         mv64xxx_i2c_do_action(drv_data);