1 files changed, 13 insertions, 11 deletions

diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/b43/phy_ht.c
index c4dc8b020f60..7b445419036f 100644
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/b43/phy_ht.c
@@ -89,6 +89,14 @@ static void b43_radio_2059_channel_setup(struct b43_wldev *dev,
         udelay(300);
 }
 
+static void b43_radio_2059_init_pre(struct b43_wldev *dev)
+{
+        b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD, ~B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
+        b43_phy_set(dev, B43_PHY_HT_RF_CTL_CMD, B43_PHY_HT_RF_CTL_CMD_FORCE);
+        b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD, ~B43_PHY_HT_RF_CTL_CMD_FORCE);
+        b43_phy_set(dev, B43_PHY_HT_RF_CTL_CMD, B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
+}
+
 static void b43_radio_2059_init(struct b43_wldev *dev)
 {
         const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3 };
@@ -97,6 +105,9 @@ static void b43_radio_2059_init(struct b43_wldev *dev)
         };
         u16 i, j;
 
+        /* Prepare (reset?) radio */
+        b43_radio_2059_init_pre(dev);
+
         b43_radio_write(dev, R2059_ALL | 0x51, 0x0070);
         b43_radio_write(dev, R2059_ALL | 0x5a, 0x0003);
 
@@ -1002,19 +1013,10 @@ static void b43_phy_ht_op_software_rfkill(struct b43_wldev *dev,
         if (b43_read32(dev, B43_MMIO_MACCTL) & B43_MACCTL_ENABLED)
                 b43err(dev->wl, "MAC not suspended\n");
 
-        /* In the following PHY ops we copy wl's dummy behaviour.
-         * TODO: Find out if reads (currently hidden in masks/masksets) are
-         * needed and replace following ops with just writes or w&r.
-         * Note: B43_PHY_HT_RF_CTL1 register is tricky, wrong operation can
-         * cause delayed (!) machine lock up. */
         if (blocked) {
-                b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
+                b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD,
+                             ~B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
         } else {
-                b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
-                b43_phy_maskset(dev, B43_PHY_HT_RF_CTL1, 0, 0x1);
-                b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
-                b43_phy_maskset(dev, B43_PHY_HT_RF_CTL1, 0, 0x2);
-
                 if (dev->phy.radio_ver == 0x2059)
                         b43_radio_2059_init(dev);
                 else