[PATCH] bcm43xx: sync interference mitigation code to the specs.
authorMichael Buesch <mbuesch@freenet.de>
Tue, 21 Mar 2006 17:16:28 +0000 (18:16 +0100)
committerJohn W. Linville <linville@tuxdriver.com>
Mon, 27 Mar 2006 16:19:43 +0000 (11:19 -0500)
This also includes a rewritten valuesave-stack.

Signed-off-by: Michael Buesch <mbuesch@freenet.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
drivers/net/wireless/bcm43xx/bcm43xx.h
drivers/net/wireless/bcm43xx/bcm43xx_radio.c

index 1fca1f9c48feffc0af98de5c667e23223e293ead..57fcaafcf7dd426432879642bb6fa0580a7e6355 100644 (file)
@@ -526,8 +526,16 @@ struct bcm43xx_radioinfo {
 
        /* Current Interference Mitigation mode */
        int interfmode;
-       /* Stack of saved values from the Interference Mitigation code */
-       u16 interfstack[20];
+       /* Stack of saved values from the Interference Mitigation code.
+        * Each value in the stack is layed out as follows:
+        * bit 0-11:  offset
+        * bit 12-15: register ID
+        * bit 16-32: value
+        * register ID is: 0x1 PHY, 0x2 Radio, 0x3 ILT
+        */
+#define BCM43xx_INTERFSTACK_SIZE       26
+       u32 interfstack[BCM43xx_INTERFSTACK_SIZE];
+
        /* Saved values from the NRSSI Slope calculation */
        s16 nrssi[2];
        s32 nrssislope;
index 4e8d8c936ab45d6b785ba8464c3534a422849707..63aae43ba838ab39b44b1d1cbf9641368131d53d 100644 (file)
@@ -879,24 +879,76 @@ void bcm43xx_calc_nrssi_threshold(struct bcm43xx_private *bcm)
        }
 }
 
-/* Helper macros to save on and restore values from the radio->interfstack */
-#ifdef stack_save
-# undef stack_save
-#endif
-#ifdef stack_restore
-# undef stack_restore
-#endif
-#define stack_save(value)  \
+/* Stack implementation to save/restore values from the
+ * interference mitigation code.
+ * It is save to restore values in random order.
+ */
+static void _stack_save(u32 *_stackptr, size_t *stackidx,
+                       u8 id, u16 offset, u16 value)
+{
+       u32 *stackptr = &(_stackptr[*stackidx]);
+
+       assert((offset & 0xF000) == 0x0000);
+       assert((id & 0xF0) == 0x00);
+       *stackptr = offset;
+       *stackptr |= ((u32)id) << 12;
+       *stackptr |= ((u32)value) << 16;
+       (*stackidx)++;
+       assert(*stackidx < BCM43xx_INTERFSTACK_SIZE);
+}
+
+static u16 _stack_restore(u32 *stackptr,
+                         u8 id, u16 offset)
+{
+       size_t i;
+
+       assert((offset & 0xF000) == 0x0000);
+       assert((id & 0xF0) == 0x00);
+       for (i = 0; i < BCM43xx_INTERFSTACK_SIZE; i++, stackptr++) {
+               if ((*stackptr & 0x00000FFF) != offset)
+                       continue;
+               if (((*stackptr & 0x0000F000) >> 12) != id)
+                       continue;
+               return ((*stackptr & 0xFFFF0000) >> 16);
+       }
+       assert(0);
+
+       return 0;
+}
+
+#define phy_stacksave(offset)                                  \
        do {                                                    \
-               assert(i < ARRAY_SIZE(radio->interfstack));     \
-               stack[i++] = (value);                           \
+               _stack_save(stack, &stackidx, 0x1, (offset),    \
+                           bcm43xx_phy_read(bcm, (offset)));   \
+       } while (0)
+#define phy_stackrestore(offset)                               \
+       do {                                                    \
+               bcm43xx_phy_write(bcm, (offset),                \
+                                 _stack_restore(stack, 0x1,    \
+                                                (offset)));    \
+       } while (0)
+#define radio_stacksave(offset)                                                \
+       do {                                                            \
+               _stack_save(stack, &stackidx, 0x2, (offset),            \
+                           bcm43xx_radio_read16(bcm, (offset)));       \
+       } while (0)
+#define radio_stackrestore(offset)                                     \
+       do {                                                            \
+               bcm43xx_radio_write16(bcm, (offset),                    \
+                                     _stack_restore(stack, 0x2,        \
+                                                    (offset)));        \
+       } while (0)
+#define ilt_stacksave(offset)                                  \
+       do {                                                    \
+               _stack_save(stack, &stackidx, 0x3, (offset),    \
+                           bcm43xx_ilt_read(bcm, (offset)));   \
+       } while (0)
+#define ilt_stackrestore(offset)                               \
+       do {                                                    \
+               bcm43xx_ilt_write(bcm, (offset),                \
+                                 _stack_restore(stack, 0x3,    \
+                                                (offset)));    \
        } while (0)
-
-#define stack_restore()  \
-       ({                                                      \
-               assert(i < ARRAY_SIZE(radio->interfstack));     \
-               stack[i++];                                     \
-       })
 
 static void
 bcm43xx_radio_interference_mitigation_enable(struct bcm43xx_private *bcm,
@@ -904,144 +956,231 @@ bcm43xx_radio_interference_mitigation_enable(struct bcm43xx_private *bcm,
 {
        struct bcm43xx_phyinfo *phy = bcm43xx_current_phy(bcm);
        struct bcm43xx_radioinfo *radio = bcm43xx_current_radio(bcm);
-       int i = 0;
-       u16 *stack = radio->interfstack;
        u16 tmp, flipped;
+       u32 tmp32;
+       size_t stackidx = 0;
+       u32 *stack = radio->interfstack;
 
        switch (mode) {
        case BCM43xx_RADIO_INTERFMODE_NONWLAN:
                if (phy->rev != 1) {
                        bcm43xx_phy_write(bcm, 0x042B,
-                                         bcm43xx_phy_read(bcm, 0x042B) & 0x0800);
+                                         bcm43xx_phy_read(bcm, 0x042B) | 0x0800);
                        bcm43xx_phy_write(bcm, BCM43xx_PHY_G_CRS,
                                          bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) & ~0x4000);
                        break;
                }
+               radio_stacksave(0x0078);
                tmp = (bcm43xx_radio_read16(bcm, 0x0078) & 0x001E);
                flipped = flip_4bit(tmp);
-               if ((flipped >> 1) >= 4)
-                       tmp = flipped - 3;
-               tmp = flip_4bit(tmp);
-               bcm43xx_radio_write16(bcm, 0x0078, tmp << 1);
+               if (flipped < 10 && flipped >= 8)
+                       flipped = 7;
+               else if (flipped >= 10)
+                       flipped -= 3;
+               flipped = flip_4bit(flipped);
+               flipped = (flipped << 1) | 0x0020;
+               bcm43xx_radio_write16(bcm, 0x0078, flipped);
 
                bcm43xx_calc_nrssi_threshold(bcm);
 
-               if (bcm->current_core->rev < 5) {
-                       stack_save(bcm43xx_phy_read(bcm, 0x0406));
-                       bcm43xx_phy_write(bcm, 0x0406, 0x7E28);
-               } else {
-                       stack_save(bcm43xx_phy_read(bcm, 0x04C0));
-                       stack_save(bcm43xx_phy_read(bcm, 0x04C1));
-                       bcm43xx_phy_write(bcm, 0x04C0, 0x3E04);
-                       bcm43xx_phy_write(bcm, 0x04C1, 0x0640);
-               }
+               phy_stacksave(0x0406);
+               bcm43xx_phy_write(bcm, 0x0406, 0x7E28);
 
                bcm43xx_phy_write(bcm, 0x042B,
                                  bcm43xx_phy_read(bcm, 0x042B) | 0x0800);
                bcm43xx_phy_write(bcm, BCM43xx_PHY_RADIO_BITFIELD,
                                  bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD) | 0x1000);
 
-               stack_save(bcm43xx_phy_read(bcm, 0x04A0));
+               phy_stacksave(0x04A0);
                bcm43xx_phy_write(bcm, 0x04A0,
                                  (bcm43xx_phy_read(bcm, 0x04A0) & 0xC0C0) | 0x0008);
-               stack_save(bcm43xx_phy_read(bcm, 0x04A1));
+               phy_stacksave(0x04A1);
                bcm43xx_phy_write(bcm, 0x04A1,
                                  (bcm43xx_phy_read(bcm, 0x04A1) & 0xC0C0) | 0x0605);
-               stack_save(bcm43xx_phy_read(bcm, 0x04A2));
+               phy_stacksave(0x04A2);
                bcm43xx_phy_write(bcm, 0x04A2,
                                  (bcm43xx_phy_read(bcm, 0x04A2) & 0xC0C0) | 0x0204);
-               stack_save(bcm43xx_phy_read(bcm, 0x04A8));
+               phy_stacksave(0x04A8);
                bcm43xx_phy_write(bcm, 0x04A8,
-                                 (bcm43xx_phy_read(bcm, 0x04A8) & 0xC0C0) | 0x0403);
-               stack_save(bcm43xx_phy_read(bcm, 0x04AB));
+                                 (bcm43xx_phy_read(bcm, 0x04A8) & 0xC0C0) | 0x0803);
+               phy_stacksave(0x04AB);
                bcm43xx_phy_write(bcm, 0x04AB,
-                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xC0C0) | 0x0504);
+                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xC0C0) | 0x0605);
 
-               stack_save(bcm43xx_phy_read(bcm, 0x04A7));
+               phy_stacksave(0x04A7);
                bcm43xx_phy_write(bcm, 0x04A7, 0x0002);
-               stack_save(bcm43xx_phy_read(bcm, 0x04A3));
+               phy_stacksave(0x04A3);
                bcm43xx_phy_write(bcm, 0x04A3, 0x287A);
-               stack_save(bcm43xx_phy_read(bcm, 0x04A9));
+               phy_stacksave(0x04A9);
                bcm43xx_phy_write(bcm, 0x04A9, 0x2027);
-               stack_save(bcm43xx_phy_read(bcm, 0x0493));
+               phy_stacksave(0x0493);
                bcm43xx_phy_write(bcm, 0x0493, 0x32F5);
-               stack_save(bcm43xx_phy_read(bcm, 0x04AA));
+               phy_stacksave(0x04AA);
                bcm43xx_phy_write(bcm, 0x04AA, 0x2027);
-               stack_save(bcm43xx_phy_read(bcm, 0x04AC));
+               phy_stacksave(0x04AC);
                bcm43xx_phy_write(bcm, 0x04AC, 0x32F5);
                break;
        case BCM43xx_RADIO_INTERFMODE_MANUALWLAN:
-               if (bcm43xx_phy_read(bcm, 0x0033) == 0x0800)
+               if (bcm43xx_phy_read(bcm, 0x0033) & 0x0800)
                        break;
 
                radio->aci_enable = 1;
 
-               stack_save(bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD));
-               stack_save(bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS));
-               if (bcm->current_core->rev < 5) {
-                       stack_save(bcm43xx_phy_read(bcm, 0x0406));
+               phy_stacksave(BCM43xx_PHY_RADIO_BITFIELD);
+               phy_stacksave(BCM43xx_PHY_G_CRS);
+               if (phy->rev < 2) {
+                       phy_stacksave(0x0406);
                } else {
-                       stack_save(bcm43xx_phy_read(bcm, 0x04C0));
-                       stack_save(bcm43xx_phy_read(bcm, 0x04C1));
+                       phy_stacksave(0x04C0);
+                       phy_stacksave(0x04C1);
                }
-               stack_save(bcm43xx_phy_read(bcm, 0x0033));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A7));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A3));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A9));
-               stack_save(bcm43xx_phy_read(bcm, 0x04AA));
-               stack_save(bcm43xx_phy_read(bcm, 0x04AC));
-               stack_save(bcm43xx_phy_read(bcm, 0x0493));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A1));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A0));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A2));
-               stack_save(bcm43xx_phy_read(bcm, 0x048A));
-               stack_save(bcm43xx_phy_read(bcm, 0x04A8));
-               stack_save(bcm43xx_phy_read(bcm, 0x04AB));
+               phy_stacksave(0x0033);
+               phy_stacksave(0x04A7);
+               phy_stacksave(0x04A3);
+               phy_stacksave(0x04A9);
+               phy_stacksave(0x04AA);
+               phy_stacksave(0x04AC);
+               phy_stacksave(0x0493);
+               phy_stacksave(0x04A1);
+               phy_stacksave(0x04A0);
+               phy_stacksave(0x04A2);
+               phy_stacksave(0x048A);
+               phy_stacksave(0x04A8);
+               phy_stacksave(0x04AB);
+               if (phy->rev == 2) {
+                       phy_stacksave(0x04AD);
+                       phy_stacksave(0x04AE);
+               } else if (phy->rev >= 3) {
+                       phy_stacksave(0x04AD);
+                       phy_stacksave(0x0415);
+                       phy_stacksave(0x0416);
+                       phy_stacksave(0x0417);
+                       ilt_stacksave(0x1A00 + 0x2);
+                       ilt_stacksave(0x1A00 + 0x3);
+               }
+               phy_stacksave(0x042B);
+               phy_stacksave(0x048C);
 
                bcm43xx_phy_write(bcm, BCM43xx_PHY_RADIO_BITFIELD,
-                                 bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD) & 0xEFFF);
+                                 bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD)
+                                 & ~0x1000);
                bcm43xx_phy_write(bcm, BCM43xx_PHY_G_CRS,
-                                 (bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) & 0xEFFF) | 0x0002);
+                                 (bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS)
+                                  & 0xFFFC) | 0x0002);
 
-               bcm43xx_phy_write(bcm, 0x04A7, 0x0800);
-               bcm43xx_phy_write(bcm, 0x04A3, 0x287A);
-               bcm43xx_phy_write(bcm, 0x04A9, 0x2027);
-               bcm43xx_phy_write(bcm, 0x0493, 0x32F5);
-               bcm43xx_phy_write(bcm, 0x04AA, 0x2027);
-               bcm43xx_phy_write(bcm, 0x04AC, 0x32F5);
+               bcm43xx_phy_write(bcm, 0x0033, 0x0800);
+               bcm43xx_phy_write(bcm, 0x04A3, 0x2027);
+               bcm43xx_phy_write(bcm, 0x04A9, 0x1CA8);
+               bcm43xx_phy_write(bcm, 0x0493, 0x287A);
+               bcm43xx_phy_write(bcm, 0x04AA, 0x1CA8);
+               bcm43xx_phy_write(bcm, 0x04AC, 0x287A);
 
                bcm43xx_phy_write(bcm, 0x04A0,
-                                 (bcm43xx_phy_read(bcm, 0x04A0) & 0xFFC0) | 0x001A);
-               if (bcm->current_core->rev < 5) {
-                       bcm43xx_phy_write(bcm, 0x0406, 0x280D);
-               } else {
-                       bcm43xx_phy_write(bcm, 0x04C0, 0x0640);
+                                 (bcm43xx_phy_read(bcm, 0x04A0)
+                                  & 0xFFC0) | 0x001A);
+               bcm43xx_phy_write(bcm, 0x04A7, 0x000D);
+
+               if (phy->rev < 2) {
+                       bcm43xx_phy_write(bcm, 0x0406, 0xFF0D);
+               } else if (phy->rev == 2) {
+                       bcm43xx_phy_write(bcm, 0x04C0, 0xFFFF);
                        bcm43xx_phy_write(bcm, 0x04C1, 0x00A9);
+               } else {
+                       bcm43xx_phy_write(bcm, 0x04C0, 0x00C1);
+                       bcm43xx_phy_write(bcm, 0x04C1, 0x0059);
                }
 
                bcm43xx_phy_write(bcm, 0x04A1,
-                                 (bcm43xx_phy_read(bcm, 0x04A1) & 0xC0FF) | 0x1800);
+                                 (bcm43xx_phy_read(bcm, 0x04A1)
+                                  & 0xC0FF) | 0x1800);
                bcm43xx_phy_write(bcm, 0x04A1,
-                                 (bcm43xx_phy_read(bcm, 0x04A1) & 0xFFC0) | 0x0016);
-               bcm43xx_phy_write(bcm, 0x04A2,
-                                 (bcm43xx_phy_read(bcm, 0x04A2) & 0xF0FF) | 0x0900);
-               bcm43xx_phy_write(bcm, 0x04A0,
-                                 (bcm43xx_phy_read(bcm, 0x04A0) & 0xF0FF) | 0x0700);
-               bcm43xx_phy_write(bcm, 0x04A2,
-                                 (bcm43xx_phy_read(bcm, 0x04A2) & 0xFFF0) | 0x000D);
+                                 (bcm43xx_phy_read(bcm, 0x04A1)
+                                  & 0xFFC0) | 0x0015);
                bcm43xx_phy_write(bcm, 0x04A8,
-                                 (bcm43xx_phy_read(bcm, 0x04A8) & 0xCFFF) | 0x1000);
+                                 (bcm43xx_phy_read(bcm, 0x04A8)
+                                  & 0xCFFF) | 0x1000);
                bcm43xx_phy_write(bcm, 0x04A8,
-                                 (bcm43xx_phy_read(bcm, 0x04A8) & 0xF0FF) | 0x0A00);
+                                 (bcm43xx_phy_read(bcm, 0x04A8)
+                                  & 0xF0FF) | 0x0A00);
                bcm43xx_phy_write(bcm, 0x04AB,
-                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xCFFF) | 0x1000);
+                                 (bcm43xx_phy_read(bcm, 0x04AB)
+                                  & 0xCFFF) | 0x1000);
                bcm43xx_phy_write(bcm, 0x04AB,
-                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xF0FF) | 0x0800);
+                                 (bcm43xx_phy_read(bcm, 0x04AB)
+                                  & 0xF0FF) | 0x0800);
                bcm43xx_phy_write(bcm, 0x04AB,
-                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xFFCF) | 0x0010);
+                                 (bcm43xx_phy_read(bcm, 0x04AB)
+                                  & 0xFFCF) | 0x0010);
                bcm43xx_phy_write(bcm, 0x04AB,
-                                 (bcm43xx_phy_read(bcm, 0x04AB) & 0xFFF0) | 0x0006);
+                                 (bcm43xx_phy_read(bcm, 0x04AB)
+                                  & 0xFFF0) | 0x0005);
+               bcm43xx_phy_write(bcm, 0x04A8,
+                                 (bcm43xx_phy_read(bcm, 0x04A8)
+                                  & 0xFFCF) | 0x0010);
+               bcm43xx_phy_write(bcm, 0x04A8,
+                                 (bcm43xx_phy_read(bcm, 0x04A8)
+                                  & 0xFFF0) | 0x0006);
+               bcm43xx_phy_write(bcm, 0x04A2,
+                                 (bcm43xx_phy_read(bcm, 0x04A2)
+                                  & 0xF0FF) | 0x0800);
+               bcm43xx_phy_write(bcm, 0x04A0,
+                                 (bcm43xx_phy_read(bcm, 0x04A0)
+                                  & 0xF0FF) | 0x0500);
+               bcm43xx_phy_write(bcm, 0x04A2,
+                                 (bcm43xx_phy_read(bcm, 0x04A2)
+                                  & 0xFFF0) | 0x000B);
 
+               if (phy->rev >= 3) {
+                       bcm43xx_phy_write(bcm, 0x048A,
+                                         bcm43xx_phy_read(bcm, 0x048A)
+                                         & ~0x8000);
+                       bcm43xx_phy_write(bcm, 0x0415,
+                                         (bcm43xx_phy_read(bcm, 0x0415)
+                                          & 0x8000) | 0x36D8);
+                       bcm43xx_phy_write(bcm, 0x0416,
+                                         (bcm43xx_phy_read(bcm, 0x0416)
+                                          & 0x8000) | 0x36D8);
+                       bcm43xx_phy_write(bcm, 0x0417,
+                                         (bcm43xx_phy_read(bcm, 0x0417)
+                                          & 0xFE00) | 0x016D);
+               } else {
+                       bcm43xx_phy_write(bcm, 0x048A,
+                                         bcm43xx_phy_read(bcm, 0x048A)
+                                         | 0x1000);
+                       bcm43xx_phy_write(bcm, 0x048A,
+                                         (bcm43xx_phy_read(bcm, 0x048A)
+                                          & 0x9FFF) | 0x2000);
+                       tmp32 = bcm43xx_shm_read32(bcm, BCM43xx_SHM_SHARED,
+                                                  BCM43xx_UCODEFLAGS_OFFSET);
+                       if (!(tmp32 & 0x800)) {
+                               tmp32 |= 0x800;
+                               bcm43xx_shm_write32(bcm, BCM43xx_SHM_SHARED,
+                                                   BCM43xx_UCODEFLAGS_OFFSET,
+                                                   tmp32);
+                       }
+               }
+               if (phy->rev >= 2) {
+                       bcm43xx_phy_write(bcm, 0x042B,
+                                         bcm43xx_phy_read(bcm, 0x042B)
+                                         | 0x0800);
+               }
+               bcm43xx_phy_write(bcm, 0x048C,
+                                 (bcm43xx_phy_read(bcm, 0x048C)
+                                  & 0xF0FF) | 0x0200);
+               if (phy->rev == 2) {
+                       bcm43xx_phy_write(bcm, 0x04AE,
+                                         (bcm43xx_phy_read(bcm, 0x04AE)
+                                          & 0xFF00) | 0x007F);
+                       bcm43xx_phy_write(bcm, 0x04AD,
+                                         (bcm43xx_phy_read(bcm, 0x04AD)
+                                          & 0x00FF) | 0x1300);
+               } else if (phy->rev >= 6) {
+                       bcm43xx_ilt_write(bcm, 0x1A00 + 0x3, 0x007F);
+                       bcm43xx_ilt_write(bcm, 0x1A00 + 0x2, 0x007F);
+                       bcm43xx_phy_write(bcm, 0x04AD,
+                                         bcm43xx_phy_read(bcm, 0x04AD)
+                                         & 0x00FF);
+               }
                bcm43xx_calc_nrssi_slope(bcm);
                break;
        default:
@@ -1055,9 +1194,8 @@ bcm43xx_radio_interference_mitigation_disable(struct bcm43xx_private *bcm,
 {
        struct bcm43xx_phyinfo *phy = bcm43xx_current_phy(bcm);
        struct bcm43xx_radioinfo *radio = bcm43xx_current_radio(bcm);
-       int i = 0;
-       u16 *stack = radio->interfstack;
-       u16 tmp, flipped;
+       u32 tmp32;
+       u32 *stack = radio->interfstack;
 
        switch (mode) {
        case BCM43xx_RADIO_INTERFMODE_NONWLAN:
@@ -1065,71 +1203,80 @@ bcm43xx_radio_interference_mitigation_disable(struct bcm43xx_private *bcm,
                        bcm43xx_phy_write(bcm, 0x042B,
                                          bcm43xx_phy_read(bcm, 0x042B) & ~0x0800);
                        bcm43xx_phy_write(bcm, BCM43xx_PHY_G_CRS,
-                                         bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) & 0x4000);
+                                         bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) | 0x4000);
                        break;
                }
-               tmp = (bcm43xx_radio_read16(bcm, 0x0078) & 0x001E);
-               flipped = flip_4bit(tmp);
-               if ((flipped >> 1) >= 0x000C)
-                       tmp = flipped + 3;
-               tmp = flip_4bit(tmp);
-               bcm43xx_radio_write16(bcm, 0x0078, tmp << 1);
-
+               phy_stackrestore(0x0078);
                bcm43xx_calc_nrssi_threshold(bcm);
-
-               if (bcm->current_core->rev < 5) {
-                       bcm43xx_phy_write(bcm, 0x0406, stack_restore());
-               } else {
-                       bcm43xx_phy_write(bcm, 0x04C0, stack_restore());
-                       bcm43xx_phy_write(bcm, 0x04C1, stack_restore());
-               }
+               phy_stackrestore(0x0406);
                bcm43xx_phy_write(bcm, 0x042B,
                                  bcm43xx_phy_read(bcm, 0x042B) & ~0x0800);
-
-               if (!bcm->bad_frames_preempt)
+               if (!bcm->bad_frames_preempt) {
                        bcm43xx_phy_write(bcm, BCM43xx_PHY_RADIO_BITFIELD,
-                                         bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD) & ~(1 << 11));
+                                         bcm43xx_phy_read(bcm, BCM43xx_PHY_RADIO_BITFIELD)
+                                         & ~(1 << 11));
+               }
                bcm43xx_phy_write(bcm, BCM43xx_PHY_G_CRS,
-                                 bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) & 0x4000);
-               bcm43xx_phy_write(bcm, 0x04A0, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A1, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A2, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A8, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AB, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A7, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A3, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A9, stack_restore());
-               bcm43xx_phy_write(bcm, 0x0493, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AA, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AC, stack_restore());
+                                 bcm43xx_phy_read(bcm, BCM43xx_PHY_G_CRS) | 0x4000);
+               phy_stackrestore(0x04A0);
+               phy_stackrestore(0x04A1);
+               phy_stackrestore(0x04A2);
+               phy_stackrestore(0x04A8);
+               phy_stackrestore(0x04AB);
+               phy_stackrestore(0x04A7);
+               phy_stackrestore(0x04A3);
+               phy_stackrestore(0x04A9);
+               phy_stackrestore(0x0493);
+               phy_stackrestore(0x04AA);
+               phy_stackrestore(0x04AC);
                break;
        case BCM43xx_RADIO_INTERFMODE_MANUALWLAN:
-               if (bcm43xx_phy_read(bcm, 0x0033) != 0x0800)
+               if (!(bcm43xx_phy_read(bcm, 0x0033) & 0x0800))
                        break;
 
                radio->aci_enable = 0;
 
-               bcm43xx_phy_write(bcm, BCM43xx_PHY_RADIO_BITFIELD, stack_restore());
-               bcm43xx_phy_write(bcm, BCM43xx_PHY_G_CRS, stack_restore());
-               if (bcm->current_core->rev < 5) {
-                       bcm43xx_phy_write(bcm, 0x0406, stack_restore());
-               } else {
-                       bcm43xx_phy_write(bcm, 0x04C0, stack_restore());
-                       bcm43xx_phy_write(bcm, 0x04C1, stack_restore());
+               phy_stackrestore(BCM43xx_PHY_RADIO_BITFIELD);
+               phy_stackrestore(BCM43xx_PHY_G_CRS);
+               phy_stackrestore(0x0033);
+               phy_stackrestore(0x04A3);
+               phy_stackrestore(0x04A9);
+               phy_stackrestore(0x0493);
+               phy_stackrestore(0x04AA);
+               phy_stackrestore(0x04AC);
+               phy_stackrestore(0x04A0);
+               phy_stackrestore(0x04A7);
+               if (phy->rev >= 2) {
+                       phy_stackrestore(0x04C0);
+                       phy_stackrestore(0x04C1);
+               } else
+                       phy_stackrestore(0x0406);
+               phy_stackrestore(0x04A1);
+               phy_stackrestore(0x04AB);
+               phy_stackrestore(0x04A8);
+               if (phy->rev == 2) {
+                       phy_stackrestore(0x04AD);
+                       phy_stackrestore(0x04AE);
+               } else if (phy->rev >= 3) {
+                       phy_stackrestore(0x04AD);
+                       phy_stackrestore(0x0415);
+                       phy_stackrestore(0x0416);
+                       phy_stackrestore(0x0417);
+                       ilt_stackrestore(0x1A00 + 0x2);
+                       ilt_stackrestore(0x1A00 + 0x3);
+               }
+               phy_stackrestore(0x04A2);
+               phy_stackrestore(0x04A8);
+               phy_stackrestore(0x042B);
+               phy_stackrestore(0x048C);
+               tmp32 = bcm43xx_shm_read32(bcm, BCM43xx_SHM_SHARED,
+                                          BCM43xx_UCODEFLAGS_OFFSET);
+               if (tmp32 & 0x800) {
+                       tmp32 &= ~0x800;
+                       bcm43xx_shm_write32(bcm, BCM43xx_SHM_SHARED,
+                                           BCM43xx_UCODEFLAGS_OFFSET,
+                                           tmp32);
                }
-               bcm43xx_phy_write(bcm, 0x0033, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A7, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A3, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A9, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AA, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AC, stack_restore());
-               bcm43xx_phy_write(bcm, 0x0493, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A1, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A0, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A2, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04A8, stack_restore());
-               bcm43xx_phy_write(bcm, 0x04AB, stack_restore());
-
                bcm43xx_calc_nrssi_slope(bcm);
                break;
        default:
@@ -1137,8 +1284,12 @@ bcm43xx_radio_interference_mitigation_disable(struct bcm43xx_private *bcm,
        }
 }
 
-#undef stack_save
-#undef stack_restore
+#undef phy_stacksave
+#undef phy_stackrestore
+#undef radio_stacksave
+#undef radio_stackrestore
+#undef ilt_stacksave
+#undef ilt_stackrestore
 
 int bcm43xx_radio_set_interference_mitigation(struct bcm43xx_private *bcm,
                                              int mode)