rt2x00: Cleanup indirect register access
authorIvo van Doorn <ivdoorn@gmail.com>
Mon, 10 Nov 2008 18:41:40 +0000 (19:41 +0100)
committerJohn W. Linville <linville@tuxdriver.com>
Tue, 25 Nov 2008 21:32:53 +0000 (16:32 -0500)
All code which accessed indirect registers was similar
in respect to the for-loop, the given timeout, etc.
Move it into a seperate function, which for PCI drivers
can be moved into rt2x00pci.

This allows us to cleanup the cleanup the code further
by removing the goto statementsand making the codepath
look a bit nicer.

Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
drivers/net/wireless/rt2x00/rt2400pci.c
drivers/net/wireless/rt2x00/rt2500pci.c
drivers/net/wireless/rt2x00/rt2500usb.c
drivers/net/wireless/rt2x00/rt2x00pci.c
drivers/net/wireless/rt2x00/rt2x00pci.h
drivers/net/wireless/rt2x00/rt61pci.c
drivers/net/wireless/rt2x00/rt73usb.c

index 78fca1bcc5447e13e08bf468189af3625ef068c0..6a977679124d549a954eb78a27b89b9cf7c304c3 100644 (file)
  * the access attempt is considered to have failed,
  * and we will print an error.
  */
-static u32 rt2400pci_bbp_check(struct rt2x00_dev *rt2x00dev)
-{
-       u32 reg;
-       unsigned int i;
-
-       for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-               rt2x00pci_register_read(rt2x00dev, BBPCSR, &reg);
-               if (!rt2x00_get_field32(reg, BBPCSR_BUSY))
-                       break;
-               udelay(REGISTER_BUSY_DELAY);
-       }
-
-       return reg;
-}
+#define WAIT_FOR_BBP(__dev, __reg) \
+       rt2x00pci_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
+#define WAIT_FOR_RF(__dev, __reg) \
+       rt2x00pci_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
 
 static void rt2400pci_bbp_write(struct rt2x00_dev *rt2x00dev,
                                const unsigned int word, const u8 value)
@@ -72,31 +62,20 @@ static void rt2400pci_bbp_write(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 new data into the register.
         */
-       reg = rt2400pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
-
-       /*
-        * Write the data into the BBP.
-        */
-       reg = 0;
-       rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
-       rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
-       rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
-       rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
-
-       rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
-
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
+               rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+               rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+               rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
+
+               rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+       }
 
-exit_fail:
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "BBPCSR register busy. Write failed.\n");
 }
 
 static void rt2400pci_bbp_read(struct rt2x00_dev *rt2x00dev,
@@ -107,74 +86,54 @@ static void rt2400pci_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 = rt2400pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+               rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+               rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
 
-       /*
-        * Write the request into the BBP.
-        */
-       reg = 0;
-       rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
-       rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
-       rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
+               rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
 
-       rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
-
-       /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt2400pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
+               WAIT_FOR_BBP(rt2x00dev, &reg);
+       }
 
        *value = rt2x00_get_field32(reg, BBPCSR_VALUE);
 
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
-
-exit_fail:
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
-       *value = 0xff;
 }
 
 static void rt2400pci_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, RFCSR, &reg);
-               if (!rt2x00_get_field32(reg, RFCSR_BUSY))
-                       goto rf_write;
-               udelay(REGISTER_BUSY_DELAY);
+       /*
+        * 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, RFCSR_VALUE, value);
+               rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
+               rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
+               rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
+
+               rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+               rt2x00_rf_write(rt2x00dev, word, value);
        }
 
-       mutex_unlock(&rt2x00dev->csr_mutex);
-       ERROR(rt2x00dev, "RFCSR register busy. Write failed.\n");
-       return;
-
-rf_write:
-       reg = 0;
-       rt2x00_set_field32(&reg, RFCSR_VALUE, value);
-       rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
-       rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
-       rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
-
-       rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
-       rt2x00_rf_write(rt2x00dev, word, value);
-
        mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
index 972b5a5c3864b3bfede5f6c586758044313e8a9b..d3bc218ec85cc2debcd5dfd52e8c00fcdf4124bf 100644 (file)
  * the access attempt is considered to have failed,
  * and we will print an error.
  */
-static u32 rt2500pci_bbp_check(struct rt2x00_dev *rt2x00dev)
-{
-       u32 reg;
-       unsigned int i;
-
-       for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-               rt2x00pci_register_read(rt2x00dev, BBPCSR, &reg);
-               if (!rt2x00_get_field32(reg, BBPCSR_BUSY))
-                       break;
-               udelay(REGISTER_BUSY_DELAY);
-       }
-
-       return reg;
-}
+#define WAIT_FOR_BBP(__dev, __reg) \
+       rt2x00pci_regbusy_read((__dev), BBPCSR, BBPCSR_BUSY, (__reg))
+#define WAIT_FOR_RF(__dev, __reg) \
+       rt2x00pci_regbusy_read((__dev), RFCSR, RFCSR_BUSY, (__reg))
 
 static void rt2500pci_bbp_write(struct rt2x00_dev *rt2x00dev,
                                const unsigned int word, const u8 value)
@@ -72,31 +62,20 @@ static void rt2500pci_bbp_write(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 new data into the register.
         */
-       reg = rt2500pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
-
-       /*
-        * Write the data into the BBP.
-        */
-       reg = 0;
-       rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
-       rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
-       rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
-       rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
-
-       rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
-
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
+               rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+               rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+               rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
+
+               rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+       }
 
-exit_fail:
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "BBPCSR register busy. Write failed.\n");
 }
 
 static void rt2500pci_bbp_read(struct rt2x00_dev *rt2x00dev,
@@ -107,74 +86,54 @@ static void rt2500pci_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 = rt2500pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+               rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+               rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
 
-       /*
-        * Write the request into the BBP.
-        */
-       reg = 0;
-       rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
-       rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
-       rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
+               rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
 
-       rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
-
-       /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt2500pci_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, BBPCSR_BUSY))
-               goto exit_fail;
+               WAIT_FOR_BBP(rt2x00dev, &reg);
+       }
 
        *value = rt2x00_get_field32(reg, BBPCSR_VALUE);
 
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
-
-exit_fail:
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
-       *value = 0xff;
 }
 
 static void rt2500pci_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, RFCSR, &reg);
-               if (!rt2x00_get_field32(reg, RFCSR_BUSY))
-                       goto rf_write;
-               udelay(REGISTER_BUSY_DELAY);
+       /*
+        * 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, RFCSR_VALUE, value);
+               rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
+               rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
+               rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
+
+               rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+               rt2x00_rf_write(rt2x00dev, word, value);
        }
 
-       mutex_unlock(&rt2x00dev->csr_mutex);
-       ERROR(rt2x00dev, "RFCSR register busy. Write failed.\n");
-       return;
-
-rf_write:
-       reg = 0;
-       rt2x00_set_field32(&reg, RFCSR_VALUE, value);
-       rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
-       rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
-       rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
-
-       rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
-       rt2x00_rf_write(rt2x00dev, word, value);
-
        mutex_unlock(&rt2x00dev->csr_mutex);
 }
 
index e6bae4ae4c47adec752983c5f0d7287906a9041e..40eb64358821a54bd678cb96122c5ec59df2a5f4 100644 (file)
@@ -57,7 +57,7 @@ static inline void rt2500usb_register_read(struct rt2x00_dev *rt2x00dev,
        __le16 reg;
        rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
                                      USB_VENDOR_REQUEST_IN, offset,
-                                     &reg, sizeof(u16), REGISTER_TIMEOUT);
+                                     &reg, sizeof(reg), REGISTER_TIMEOUT);
        *value = le16_to_cpu(reg);
 }
 
@@ -68,7 +68,7 @@ static inline void rt2500usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
        __le16 reg;
        rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
                                       USB_VENDOR_REQUEST_IN, offset,
-                                      &reg, sizeof(u16), REGISTER_TIMEOUT);
+                                      &reg, sizeof(reg), REGISTER_TIMEOUT);
        *value = le16_to_cpu(reg);
 }
 
@@ -89,7 +89,7 @@ static inline void rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
        __le16 reg = cpu_to_le16(value);
        rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
                                      USB_VENDOR_REQUEST_OUT, offset,
-                                     &reg, sizeof(u16), REGISTER_TIMEOUT);
+                                     &reg, sizeof(reg), REGISTER_TIMEOUT);
 }
 
 static inline void rt2500usb_register_write_lock(struct rt2x00_dev *rt2x00dev,
@@ -99,7 +99,7 @@ static inline void rt2500usb_register_write_lock(struct rt2x00_dev *rt2x00dev,
        __le16 reg = cpu_to_le16(value);
        rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_WRITE,
                                       USB_VENDOR_REQUEST_OUT, offset,
-                                      &reg, sizeof(u16), REGISTER_TIMEOUT);
+                                      &reg, sizeof(reg), REGISTER_TIMEOUT);
 }
 
 static inline void rt2500usb_register_multiwrite(struct rt2x00_dev *rt2x00dev,
@@ -112,21 +112,32 @@ static inline void rt2500usb_register_multiwrite(struct rt2x00_dev *rt2x00dev,
                                      REGISTER_TIMEOUT16(length));
 }
 
-static u16 rt2500usb_bbp_check(struct rt2x00_dev *rt2x00dev)
+static int rt2500usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
+                                 const unsigned int offset,
+                                 struct rt2x00_field16 field,
+                                 u16 *reg)
 {
-       u16 reg;
        unsigned int i;
 
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-               rt2500usb_register_read_lock(rt2x00dev, PHY_CSR8, &reg);
-               if (!rt2x00_get_field16(reg, PHY_CSR8_BUSY))
-                       break;
+               rt2500usb_register_read_lock(rt2x00dev, offset, reg);
+               if (!rt2x00_get_field16(*reg, field))
+                       return 1;
                udelay(REGISTER_BUSY_DELAY);
        }
 
-       return reg;
+       ERROR(rt2x00dev, "Indirect register access failed: "
+             "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
+       *reg = ~0;
+
+       return 0;
 }
 
+#define WAIT_FOR_BBP(__dev, __reg) \
+       rt2500usb_regbusy_read((__dev), PHY_CSR8, PHY_CSR8_BUSY, (__reg))
+#define WAIT_FOR_RF(__dev, __reg) \
+       rt2500usb_regbusy_read((__dev), PHY_CSR10, PHY_CSR10_RF_BUSY, (__reg))
+
 static void rt2500usb_bbp_write(struct rt2x00_dev *rt2x00dev,
                                const unsigned int word, const u8 value)
 {
@@ -135,30 +146,19 @@ static void rt2500usb_bbp_write(struct rt2x00_dev *rt2x00dev,
        mutex_lock(&rt2x00dev->csr_mutex);
 
        /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt2500usb_bbp_check(rt2x00dev);
-       if (rt2x00_get_field16(reg, PHY_CSR8_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_field16(&reg, PHY_CSR7_DATA, value);
-       rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
-       rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 0);
-
-       rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
-
-       mutex_unlock(&rt2x00dev->csr_mutex);
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field16(&reg, PHY_CSR7_DATA, value);
+               rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
+               rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 0);
 
-       return;
+               rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
+       }
 
-exit_fail:
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "PHY_CSR8 register busy. Write failed.\n");
 }
 
 static void rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
@@ -169,77 +169,57 @@ static void rt2500usb_bbp_read(struct rt2x00_dev *rt2x00dev,
        mutex_lock(&rt2x00dev->csr_mutex);
 
        /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt2500usb_bbp_check(rt2x00dev);
-       if (rt2x00_get_field16(reg, PHY_CSR8_BUSY))
-               goto exit_fail;
-
-       /*
-        * Write the request into the BBP.
+        * 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 = 0;
-       rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
-       rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 1);
+       if (WAIT_FOR_BBP(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
+               rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 1);
 
-       rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
+               rt2500usb_register_write_lock(rt2x00dev, PHY_CSR7, reg);
 
-       /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt2500usb_bbp_check(rt2x00dev);
-       if (rt2x00_get_field16(reg, PHY_CSR8_BUSY))
-               goto exit_fail;
+               if (WAIT_FOR_BBP(rt2x00dev, &reg))
+                       rt2500usb_register_read_lock(rt2x00dev, PHY_CSR7, &reg);
+       }
 
-       rt2500usb_register_read_lock(rt2x00dev, PHY_CSR7, &reg);
        *value = rt2x00_get_field16(reg, PHY_CSR7_DATA);
 
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
-
-exit_fail:
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "PHY_CSR8 register busy. Read failed.\n");
-       *value = 0xff;
 }
 
 static void rt2500usb_rf_write(struct rt2x00_dev *rt2x00dev,
                               const unsigned int word, const u32 value)
 {
        u16 reg;
-       unsigned int i;
 
        if (!word)
                return;
 
        mutex_lock(&rt2x00dev->csr_mutex);
 
-       for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-               rt2500usb_register_read_lock(rt2x00dev, PHY_CSR10, &reg);
-               if (!rt2x00_get_field16(reg, PHY_CSR10_RF_BUSY))
-                       goto rf_write;
-               udelay(REGISTER_BUSY_DELAY);
-       }
-
-       mutex_unlock(&rt2x00dev->csr_mutex);
-       ERROR(rt2x00dev, "PHY_CSR10 register busy. Write failed.\n");
-       return;
-
-rf_write:
-       reg = 0;
-       rt2x00_set_field16(&reg, PHY_CSR9_RF_VALUE, value);
-       rt2500usb_register_write_lock(rt2x00dev, PHY_CSR9, reg);
+       /*
+        * 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_field16(&reg, PHY_CSR9_RF_VALUE, value);
+               rt2500usb_register_write_lock(rt2x00dev, PHY_CSR9, reg);
 
-       reg = 0;
-       rt2x00_set_field16(&reg, PHY_CSR10_RF_VALUE, value >> 16);
-       rt2x00_set_field16(&reg, PHY_CSR10_RF_NUMBER_OF_BITS, 20);
-       rt2x00_set_field16(&reg, PHY_CSR10_RF_IF_SELECT, 0);
-       rt2x00_set_field16(&reg, PHY_CSR10_RF_BUSY, 1);
+               reg = 0;
+               rt2x00_set_field16(&reg, PHY_CSR10_RF_VALUE, value >> 16);
+               rt2x00_set_field16(&reg, PHY_CSR10_RF_NUMBER_OF_BITS, 20);
+               rt2x00_set_field16(&reg, PHY_CSR10_RF_IF_SELECT, 0);
+               rt2x00_set_field16(&reg, PHY_CSR10_RF_BUSY, 1);
 
-       rt2500usb_register_write_lock(rt2x00dev, PHY_CSR10, reg);
-       rt2x00_rf_write(rt2x00dev, word, value);
+               rt2500usb_register_write_lock(rt2x00dev, PHY_CSR10, reg);
+               rt2x00_rf_write(rt2x00dev, word, value);
+       }
 
        mutex_unlock(&rt2x00dev->csr_mutex);
 }
index e33bd0f150c5ac3b712eaca98678a7555a8a7791..d52b22b82d1fb4662184b653efe50f99b43fec13 100644 (file)
 #include "rt2x00.h"
 #include "rt2x00pci.h"
 
+/*
+ * Register access.
+ */
+int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
+                          const unsigned int offset,
+                          const struct rt2x00_field32 field,
+                          u32 *reg)
+{
+       unsigned int i;
+
+       for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+               rt2x00pci_register_read(rt2x00dev, offset, reg);
+               if (!rt2x00_get_field32(*reg, field))
+                       return 1;
+               udelay(REGISTER_BUSY_DELAY);
+       }
+
+       ERROR(rt2x00dev, "Indirect register access failed: "
+             "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
+       *reg = ~0;
+
+       return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_regbusy_read);
+
 /*
  * TX data handlers.
  */
index 96a2082a35322b00f1150e5d94066c8d21a54412..9c0a4d77bc1be1e16ab79940c3c2cb0aa65859c2 100644 (file)
@@ -76,6 +76,24 @@ rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
        memcpy_toio(rt2x00dev->csr.base + offset, value, length);
 }
 
+/**
+ * rt2x00pci_regbusy_read - Read from register with busy check
+ * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
+ * @offset: Register offset
+ * @field: Field to check if register is busy
+ * @reg: Pointer to where register contents should be stored
+ *
+ * This function will read the given register, and checks if the
+ * register is busy. If it is, it will sleep for a couple of
+ * microseconds before reading the register again. If the register
+ * is not read after a certain timeout, this function will return
+ * FALSE.
+ */
+int rt2x00pci_regbusy_read(struct rt2x00_dev *rt2x00dev,
+                          const unsigned int offset,
+                          const struct rt2x00_field32 field,
+                          u32 *reg);
+
 /**
  * rt2x00pci_write_tx_data - Initialize data for TX operation
  * @entry: The entry where the frame is located
index 89ac34fbadf2a4457765e67875da11f99abefe4f..d54443c25fe35f7f52473a17aa3c4ccbe9a766db 100644 (file)
@@ -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 */
 
index d1a63e0017daf35327900f959606dc2993ad710e..b4ca8488beffe6bc500ad1c7daa5946135cefbd2 100644 (file)
@@ -63,7 +63,7 @@ static inline void rt73usb_register_read(struct rt2x00_dev *rt2x00dev,
        __le32 reg;
        rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
                                      USB_VENDOR_REQUEST_IN, offset,
-                                     &reg, sizeof(u32), REGISTER_TIMEOUT);
+                                     &reg, sizeof(reg), REGISTER_TIMEOUT);
        *value = le32_to_cpu(reg);
 }
 
@@ -73,7 +73,7 @@ static inline void rt73usb_register_read_lock(struct rt2x00_dev *rt2x00dev,
        __le32 reg;
        rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_READ,
                                       USB_VENDOR_REQUEST_IN, offset,
-                                      &reg, sizeof(u32), REGISTER_TIMEOUT);
+                                      &reg, sizeof(reg), REGISTER_TIMEOUT);
        *value = le32_to_cpu(reg);
 }
 
@@ -93,7 +93,7 @@ static inline void rt73usb_register_write(struct rt2x00_dev *rt2x00dev,
        __le32 reg = cpu_to_le32(value);
        rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
                                      USB_VENDOR_REQUEST_OUT, offset,
-                                     &reg, sizeof(u32), REGISTER_TIMEOUT);
+                                     &reg, sizeof(reg), REGISTER_TIMEOUT);
 }
 
 static inline void rt73usb_register_write_lock(struct rt2x00_dev *rt2x00dev,
@@ -102,7 +102,7 @@ static inline void rt73usb_register_write_lock(struct rt2x00_dev *rt2x00dev,
        __le32 reg = cpu_to_le32(value);
        rt2x00usb_vendor_req_buff_lock(rt2x00dev, USB_MULTI_WRITE,
                                       USB_VENDOR_REQUEST_OUT, offset,
-                                     &reg, sizeof(u32), REGISTER_TIMEOUT);
+                                      &reg, sizeof(reg), REGISTER_TIMEOUT);
 }
 
 static inline void rt73usb_register_multiwrite(struct rt2x00_dev *rt2x00dev,
@@ -115,21 +115,32 @@ static inline void rt73usb_register_multiwrite(struct rt2x00_dev *rt2x00dev,
                                      REGISTER_TIMEOUT32(length));
 }
 
-static u32 rt73usb_bbp_check(struct rt2x00_dev *rt2x00dev)
+static int rt73usb_regbusy_read(struct rt2x00_dev *rt2x00dev,
+                               const unsigned int offset,
+                               struct rt2x00_field32 field,
+                               u32 *reg)
 {
-       u32 reg;
        unsigned int i;
 
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
-               rt73usb_register_read_lock(rt2x00dev, PHY_CSR3, &reg);
-               if (!rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-                       break;
+               rt73usb_register_read_lock(rt2x00dev, offset, reg);
+               if (!rt2x00_get_field32(*reg, field))
+                       return 1;
                udelay(REGISTER_BUSY_DELAY);
        }
 
-       return reg;
+       ERROR(rt2x00dev, "Indirect register access failed: "
+             "offset=0x%.08x, value=0x%.08x\n", offset, *reg);
+       *reg = ~0;
+
+       return 0;
 }
 
+#define WAIT_FOR_BBP(__dev, __reg) \
+       rt73usb_regbusy_read((__dev), PHY_CSR3, PHY_CSR3_BUSY, (__reg))
+#define WAIT_FOR_RF(__dev, __reg) \
+       rt73usb_regbusy_read((__dev), PHY_CSR4, PHY_CSR4_BUSY, (__reg))
+
 static void rt73usb_bbp_write(struct rt2x00_dev *rt2x00dev,
                              const unsigned int word, const u8 value)
 {
@@ -138,30 +149,20 @@ static void rt73usb_bbp_write(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 new data into the register.
         */
-       reg = rt73usb_bbp_check(rt2x00dev);
-       if (rt2x00_get_field32(reg, PHY_CSR3_BUSY))
-               goto exit_fail;
-
-       /*
-        * Write the data into the BBP.
-        */
-       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);
-
-       rt73usb_register_write_lock(rt2x00dev, PHY_CSR3, reg);
-       mutex_unlock(&rt2x00dev->csr_mutex);
-
-       return;
+       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);
+
+               rt73usb_register_write_lock(rt2x00dev, PHY_CSR3, reg);
+       }
 
-exit_fail:
        mutex_unlock(&rt2x00dev->csr_mutex);
-
-       ERROR(rt2x00dev, "PHY_CSR3 register busy. Write failed.\n");
 }
 
 static void rt73usb_bbp_read(struct rt2x00_dev *rt2x00dev,
@@ -172,79 +173,59 @@ static void rt73usb_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 = rt73usb_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);
+               rt73usb_register_write_lock(rt2x00dev, PHY_CSR3, reg);
 
-       rt73usb_register_write_lock(rt2x00dev, PHY_CSR3, reg);
-
-       /*
-        * Wait until the BBP becomes ready.
-        */
-       reg = rt73usb_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 rt73usb_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++) {
-               rt73usb_register_read_lock(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);
-
        /*
-        * RF5225 and RF2527 contain 21 bits per RF register value,
-        * all others contain 20 bits.
+        * Wait until the RF becomes available, afterwards we
+        * can safely write the new data into the register.
         */
-       rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS,
-                          20 + (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
-                                rt2x00_rf(&rt2x00dev->chip, RF2527)));
-       rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
-       rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
-
-       rt73usb_register_write_lock(rt2x00dev, PHY_CSR4, reg);
-       rt2x00_rf_write(rt2x00dev, word, value);
+       if (WAIT_FOR_RF(rt2x00dev, &reg)) {
+               reg = 0;
+               rt2x00_set_field32(&reg, PHY_CSR4_VALUE, value);
+               /*
+                * RF5225 and RF2527 contain 21 bits per RF register value,
+                * all others contain 20 bits.
+                */
+               rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS,
+                                  20 + (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+                                        rt2x00_rf(&rt2x00dev->chip, RF2527)));
+               rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
+               rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
+
+               rt73usb_register_write_lock(rt2x00dev, PHY_CSR4, reg);
+               rt2x00_rf_write(rt2x00dev, word, value);
+       }
 
        mutex_unlock(&rt2x00dev->csr_mutex);
 }