2 files changed, 29 insertions, 20 deletions

diff --git a/drivers/i2c/busses/i2c-pnx.c b/drivers/i2c/busses/i2c-pnx.c
index b36e21f9fd5..8488bddfe46 100644
--- a/drivers/i2c/busses/i2c-pnx.c
+++ b/drivers/i2c/busses/i2c-pnx.c
@@ -291,31 +291,37 @@ static int i2c_pnx_master_rcv(struct i2c_pnx_algo_data *alg_data)
          * or we didn't 'ask' for it yet.
          */
         if (ioread32(I2C_REG_STS(alg_data)) & mstatus_rfe) {
-                dev_dbg(&alg_data->adapter.dev,
-                        "%s(): Write dummy data to fill Rx-fifo...\n",
-                        __func__);
+                /* 'Asking' is done asynchronously, e.g. dummy TX of several
+                 * bytes is done before the first actual RX arrives in FIFO.
+                 * Therefore, ordered bytes (via TX) are counted separately.
+                 */
+                if (alg_data->mif.order) {
+                        dev_dbg(&alg_data->adapter.dev,
+                                "%s(): Write dummy data to fill Rx-fifo...\n",
+                                __func__);
 
-                if (alg_data->mif.len == 1) {
-                        /* Last byte, do not acknowledge next rcv. */
-                        val |= stop_bit;
+                        if (alg_data->mif.order == 1) {
+                                /* Last byte, do not acknowledge next rcv. */
+                                val |= stop_bit;
+
+                                /*
+                                 * Enable interrupt RFDAIE (data in Rx fifo),
+                                 * and disable DRMIE (need data for Tx)
+                                 */
+                                ctl = ioread32(I2C_REG_CTL(alg_data));
+                                ctl |= mcntrl_rffie | mcntrl_daie;
+                                ctl &= ~mcntrl_drmie;
+                                iowrite32(ctl, I2C_REG_CTL(alg_data));
+                        }
 
                         /*
-                         * Enable interrupt RFDAIE (data in Rx fifo),
-                         * and disable DRMIE (need data for Tx)
+                         * Now we'll 'ask' for data:
+                         * For each byte we want to receive, we must
+                         * write a (dummy) byte to the Tx-FIFO.
                          */
-                        ctl = ioread32(I2C_REG_CTL(alg_data));
-                        ctl |= mcntrl_rffie | mcntrl_daie;
-                        ctl &= ~mcntrl_drmie;
-                        iowrite32(ctl, I2C_REG_CTL(alg_data));
+                        iowrite32(val, I2C_REG_TX(alg_data));
+                        alg_data->mif.order--;
                 }
-
-                /*
-                 * Now we'll 'ask' for data:
-                 * For each byte we want to receive, we must
-                 * write a (dummy) byte to the Tx-FIFO.
-                 */
-                iowrite32(val, I2C_REG_TX(alg_data));
-
                 return 0;
         }
 
@@ -515,6 +521,7 @@ i2c_pnx_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
 
                 alg_data->mif.buf = pmsg->buf;
                 alg_data->mif.len = pmsg->len;
+                alg_data->mif.order = pmsg->len;
                 alg_data->mif.mode = (pmsg->flags & I2C_M_RD) ?
                         I2C_SMBUS_READ : I2C_SMBUS_WRITE;
                 alg_data->mif.ret = 0;
@@ -567,6 +574,7 @@ i2c_pnx_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
         /* Cleanup to be sure... */
         alg_data->mif.buf = NULL;
         alg_data->mif.len = 0;
+        alg_data->mif.order = 0;
 
         dev_dbg(&alg_data->adapter.dev, "%s(): exiting, stat = %x\n",
                 __func__, ioread32(I2C_REG_STS(alg_data)));
diff --git a/include/linux/i2c-pnx.h b/include/linux/i2c-pnx.h
index 1bc74afe7a3..49ed17fdf05 100644
--- a/include/linux/i2c-pnx.h
+++ b/include/linux/i2c-pnx.h
@@ -22,6 +22,7 @@ struct i2c_pnx_mif {
         struct timer_list       timer;          /* Timeout */
         u8 *                    buf;            /* Data buffer */
         int                     len;            /* Length of data buffer */
+        int                     order;          /* RX Bytes to order via TX */
 };
 
 struct i2c_pnx_algo_data {