1 files changed, 59 insertions, 102 deletions

diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 89ac34fbadf2..d54443c25fe3 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -55,20 +55,13 @@ MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
  * the access attempt is considered to have failed,
  * and we will print an error.
  */
-static u32 rt61pci_bbp_check(struct rt2x00_dev *rt2x00dev)
-{
-        u32 reg;
-        unsigned int i;
-
-        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-                rt2x00pci_register_read(rt2x00dev, PHY_CSR3, &reg);
-                if (!rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-                        break;
-                udelay(REGISTER_BUSY_DELAY);
-        }
-
-        return reg;
-}
+#define WAIT_FOR_BBP(__dev, __reg) \
+        rt2x00pci_regbusy_read((__dev), PHY_CSR3, PHY_CSR3_BUSY, (__reg))
+#define WAIT_FOR_RF(__dev, __reg) \
+        rt2x00pci_regbusy_read((__dev), PHY_CSR4, PHY_CSR4_BUSY, (__reg))
+#define WAIT_FOR_MCU(__dev, __reg) \
+        rt2x00pci_regbusy_read((__dev), H2M_MAILBOX_CSR, \
+                               H2M_MAILBOX_CSR_OWNER, (__reg))
 
 static void rt61pci_bbp_write(struct rt2x00_dev *rt2x00dev,
                               const unsigned int word, const u8 value)
@@ -78,30 +71,20 @@ static void rt61pci_bbp_write(struct rt2x00_dev *rt2x00dev,
         mutex_lock(&rt2x00dev->csr_mutex);
 
         /*
-         * Wait until the BBP becomes ready.
-         */
-        reg = rt61pci_bbp_check(rt2x00dev);
-        if (rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-                goto exit_fail;
-
-        /*
-         * Write the data into the BBP.
+         * Wait until the BBP becomes available, afterwards we
+         * can safely write the new data into the register.
          */
-        reg = 0;
-        rt2x00_set_field32(&reg, PHY_CSR3_VALUE, value);
-        rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
-        rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
-        rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 0);
-
-        rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
-        mutex_unlock(&rt2x00dev->csr_mutex);
+        if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+                reg = 0;
+                rt2x00_set_field32(&reg, PHY_CSR3_VALUE, value);
+                rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+                rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+                rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 0);
 
-        return;
+                rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
+        }
 
-exit_fail:
         mutex_unlock(&rt2x00dev->csr_mutex);
-
-        ERROR(rt2x00dev, "PHY_CSR3 register busy. Write failed.\n");
 }
 
 static void rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
@@ -112,72 +95,53 @@ static void rt61pci_bbp_read(struct rt2x00_dev *rt2x00dev,
         mutex_lock(&rt2x00dev->csr_mutex);
 
         /*
-         * Wait until the BBP becomes ready.
+         * Wait until the BBP becomes available, afterwards we
+         * can safely write the read request into the register.
+         * After the data has been written, we wait until hardware
+         * returns the correct value, if at any time the register
+         * doesn't become available in time, reg will be 0xffffffff
+         * which means we return 0xff to the caller.
          */
-        reg = rt61pci_bbp_check(rt2x00dev);
-        if (rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-                goto exit_fail;
+        if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+                reg = 0;
+                rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+                rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+                rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 1);
 
-        /*
-         * Write the request into the BBP.
-         */
-        reg = 0;
-        rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
-        rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
-        rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 1);
+                rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
 
-        rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
-
-        /*
-         * Wait until the BBP becomes ready.
-         */
-        reg = rt61pci_bbp_check(rt2x00dev);
-        if (rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-                goto exit_fail;
+                WAIT_FOR_BBP(rt2x00dev, &reg);
+        }
 
         *value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
-        mutex_unlock(&rt2x00dev->csr_mutex);
-
-        return;
 
-exit_fail:
         mutex_unlock(&rt2x00dev->csr_mutex);
-
-        ERROR(rt2x00dev, "PHY_CSR3 register busy. Read failed.\n");
-        *value = 0xff;
 }
 
 static void rt61pci_rf_write(struct rt2x00_dev *rt2x00dev,
                              const unsigned int word, const u32 value)
 {
         u32 reg;
-        unsigned int i;
 
         if (!word)
                 return;
 
         mutex_lock(&rt2x00dev->csr_mutex);
 
-        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-                rt2x00pci_register_read(rt2x00dev, PHY_CSR4, &reg);
-                if (!rt2x00_get_field32(reg, PHY_CSR4_BUSY))
-                        goto rf_write;
-                udelay(REGISTER_BUSY_DELAY);
-        }
-
-        mutex_unlock(&rt2x00dev->csr_mutex);
-        ERROR(rt2x00dev, "PHY_CSR4 register busy. Write failed.\n");
-        return;
-
-rf_write:
-        reg = 0;
-        rt2x00_set_field32(&reg, PHY_CSR4_VALUE, value);
-        rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS, 21);
-        rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
-        rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
+        /*
+         * Wait until the RF becomes available, afterwards we
+         * can safely write the new data into the register.
+         */
+        if (WAIT_FOR_RF(rt2x00dev, &reg)) {
+                reg = 0;
+                rt2x00_set_field32(&reg, PHY_CSR4_VALUE, value);
+                rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS, 21);
+                rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
+                rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
 
-        rt2x00pci_register_write(rt2x00dev, PHY_CSR4, reg);
-        rt2x00_rf_write(rt2x00dev, word, value);
+                rt2x00pci_register_write(rt2x00dev, PHY_CSR4, reg);
+                rt2x00_rf_write(rt2x00dev, word, value);
+        }
 
         mutex_unlock(&rt2x00dev->csr_mutex);
 }
@@ -196,32 +160,25 @@ static void rt61pci_mcu_request(struct rt2x00_dev *rt2x00dev,
 
         mutex_lock(&rt2x00dev->csr_mutex);
 
-        rt2x00pci_register_read(rt2x00dev, H2M_MAILBOX_CSR, &reg);
-
-        if (rt2x00_get_field32(reg, H2M_MAILBOX_CSR_OWNER))
-                goto exit_fail;
-
-        rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_OWNER, 1);
-        rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_CMD_TOKEN, token);
-        rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG0, arg0);
-        rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG1, arg1);
-        rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
-
-        rt2x00pci_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
-        rt2x00_set_field32(&reg, HOST_CMD_CSR_HOST_COMMAND, command);
-        rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
-        rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, reg);
-
-        mutex_unlock(&rt2x00dev->csr_mutex);
+        /*
+         * Wait until the MCU becomes available, afterwards we
+         * can safely write the new data into the register.
+         */
+        if (WAIT_FOR_MCU(rt2x00dev, &reg)) {
+                rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_OWNER, 1);
+                rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_CMD_TOKEN, token);
+                rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG0, arg0);
+                rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG1, arg1);
+                rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
 
-        return;
+                rt2x00pci_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
+                rt2x00_set_field32(&reg, HOST_CMD_CSR_HOST_COMMAND, command);
+                rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
+                rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, reg);
+        }
 
-exit_fail:
         mutex_unlock(&rt2x00dev->csr_mutex);
 
-        ERROR(rt2x00dev,
-              "mcu request error. Request 0x%02x failed for token 0x%02x.\n",
-              command, token);
 }
 #endif /* CONFIG_RT2X00_LIB_LEDS */