1 files changed, 89 insertions, 0 deletions

diff --git a/drivers/net/qlge/qlge_mpi.c b/drivers/net/qlge/qlge_mpi.c
index c2e43073047e..8810f5be7862 100644
--- a/drivers/net/qlge/qlge_mpi.c
+++ b/drivers/net/qlge/qlge_mpi.c
@@ -768,6 +768,95 @@ static int ql_idc_wait(struct ql_adapter *qdev)
         return status;
 }
 
+int ql_mb_set_mgmnt_traffic_ctl(struct ql_adapter *qdev, u32 control)
+{
+        struct mbox_params mbc;
+        struct mbox_params *mbcp = &mbc;
+        int status;
+
+        memset(mbcp, 0, sizeof(struct mbox_params));
+
+        mbcp->in_count = 1;
+        mbcp->out_count = 2;
+
+        mbcp->mbox_in[0] = MB_CMD_SET_MGMNT_TFK_CTL;
+        mbcp->mbox_in[1] = control;
+
+        status = ql_mailbox_command(qdev, mbcp);
+        if (status)
+                return status;
+
+        if (mbcp->mbox_out[0] == MB_CMD_STS_GOOD)
+                return status;
+
+        if (mbcp->mbox_out[0] == MB_CMD_STS_INVLD_CMD) {
+                QPRINTK(qdev, DRV, ERR,
+                        "Command not supported by firmware.\n");
+                status = -EINVAL;
+        } else if (mbcp->mbox_out[0] == MB_CMD_STS_ERR) {
+                /* This indicates that the firmware is
+                 * already in the state we are trying to
+                 * change it to.
+                 */
+                QPRINTK(qdev, DRV, ERR,
+                        "Command parameters make no change.\n");
+        }
+        return status;
+}
+
+/* Returns a negative error code or the mailbox command status. */
+static int ql_mb_get_mgmnt_traffic_ctl(struct ql_adapter *qdev, u32 *control)
+{
+        struct mbox_params mbc;
+        struct mbox_params *mbcp = &mbc;
+        int status;
+
+        memset(mbcp, 0, sizeof(struct mbox_params));
+        *control = 0;
+
+        mbcp->in_count = 1;
+        mbcp->out_count = 1;
+
+        mbcp->mbox_in[0] = MB_CMD_GET_MGMNT_TFK_CTL;
+
+        status = ql_mailbox_command(qdev, mbcp);
+        if (status)
+                return status;
+
+        if (mbcp->mbox_out[0] == MB_CMD_STS_GOOD) {
+                *control = mbcp->mbox_in[1];
+                return status;
+        }
+
+        if (mbcp->mbox_out[0] == MB_CMD_STS_INVLD_CMD) {
+                QPRINTK(qdev, DRV, ERR,
+                        "Command not supported by firmware.\n");
+                status = -EINVAL;
+        } else if (mbcp->mbox_out[0] == MB_CMD_STS_ERR) {
+                QPRINTK(qdev, DRV, ERR,
+                        "Failed to get MPI traffic control.\n");
+                status = -EIO;
+        }
+        return status;
+}
+
+int ql_wait_fifo_empty(struct ql_adapter *qdev)
+{
+        int count = 5;
+        u32 mgmnt_fifo_empty;
+        u32 nic_fifo_empty;
+
+        do {
+                nic_fifo_empty = ql_read32(qdev, STS) & STS_NFE;
+                ql_mb_get_mgmnt_traffic_ctl(qdev, &mgmnt_fifo_empty);
+                mgmnt_fifo_empty &= MB_GET_MPI_TFK_FIFO_EMPTY;
+                if (nic_fifo_empty && mgmnt_fifo_empty)
+                        return 0;
+                msleep(100);
+        } while (count-- > 0);
+        return -ETIMEDOUT;
+}
+
 /* API called in work thread context to set new TX/RX
  * maximum frame size values to match MTU.
  */