7 files changed, 452 insertions, 135 deletions

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index b8807fb12c92..3a003e6803a5 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -104,6 +104,7 @@
 #define B43_MMIO_MACFILTER_CONTROL      0x420
 #define B43_MMIO_MACFILTER_DATA         0x422
 #define B43_MMIO_RCMTA_COUNT            0x43C
+#define B43_MMIO_PSM_PHY_HDR            0x492
 #define B43_MMIO_RADIO_HWENABLED_LO     0x49A
 #define B43_MMIO_GPIO_CONTROL           0x49C
 #define B43_MMIO_GPIO_MASK              0x49E
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 9a374ef83a22..7965b70efbab 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -4349,11 +4349,10 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
         b43_set_phytxctl_defaults(dev);
 
         /* Minimum Contention Window */
-        if (phy->type == B43_PHYTYPE_B) {
+        if (phy->type == B43_PHYTYPE_B)
                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
-        } else {
+        else
                 b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
-        }
         /* Maximum Contention Window */
         b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
 
@@ -4572,6 +4571,23 @@ static void b43_op_sw_scan_complete_notifier(struct ieee80211_hw *hw)
         mutex_unlock(&wl->mutex);
 }
 
+static int b43_op_get_survey(struct ieee80211_hw *hw, int idx,
+                             struct survey_info *survey)
+{
+        struct b43_wl *wl = hw_to_b43_wl(hw);
+        struct b43_wldev *dev = wl->current_dev;
+        struct ieee80211_conf *conf = &hw->conf;
+
+        if (idx != 0)
+                return -ENOENT;
+
+        survey->channel = conf->channel;
+        survey->filled = SURVEY_INFO_NOISE_DBM;
+        survey->noise = dev->stats.link_noise;
+
+        return 0;
+}
+
 static const struct ieee80211_ops b43_hw_ops = {
         .tx                     = b43_op_tx,
         .conf_tx                = b43_op_conf_tx,
@@ -4591,6 +4607,7 @@ static const struct ieee80211_ops b43_hw_ops = {
         .sta_notify             = b43_op_sta_notify,
         .sw_scan_start          = b43_op_sw_scan_start_notifier,
         .sw_scan_complete       = b43_op_sw_scan_complete_notifier,
+        .get_survey             = b43_op_get_survey,
         .rfkill_poll            = b43_rfkill_poll,
 };
 
@@ -4906,8 +4923,7 @@ static int b43_wireless_init(struct ssb_device *dev)
 
         /* fill hw info */
         hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-                    IEEE80211_HW_SIGNAL_DBM |
-                    IEEE80211_HW_NOISE_DBM;
+                    IEEE80211_HW_SIGNAL_DBM;
 
         hw->wiphy->interface_modes =
                 BIT(NL80211_IFTYPE_AP) |
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 9c7cd282e46c..3d6b33775964 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -73,6 +73,22 @@ static void b43_nphy_rf_control_override(struct b43_wldev *dev, u16 field,
                                                 u16 value, u8 core, bool off);
 static void b43_nphy_rf_control_intc_override(struct b43_wldev *dev, u8 field,
                                                 u16 value, u8 core);
+static int nphy_channel_switch(struct b43_wldev *dev, unsigned int channel);
+
+static inline bool b43_empty_chanspec(struct b43_chanspec *chanspec)
+{
+        return !chanspec->channel && !chanspec->sideband &&
+                !chanspec->b_width && !chanspec->b_freq;
+}
+
+static inline bool b43_eq_chanspecs(struct b43_chanspec *chanspec1,
+                                        struct b43_chanspec *chanspec2)
+{
+        return (chanspec1->channel == chanspec2->channel &&
+                chanspec1->sideband == chanspec2->sideband &&
+                chanspec1->b_width == chanspec2->b_width &&
+                chanspec1->b_freq == chanspec2->b_freq);
+}
 
 void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
 {//TODO
@@ -89,34 +105,44 @@ static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
 }
 
 static void b43_chantab_radio_upload(struct b43_wldev *dev,
-                                     const struct b43_nphy_channeltab_entry *e)
-{
-        b43_radio_write16(dev, B2055_PLL_REF, e->radio_pll_ref);
-        b43_radio_write16(dev, B2055_RF_PLLMOD0, e->radio_rf_pllmod0);
-        b43_radio_write16(dev, B2055_RF_PLLMOD1, e->radio_rf_pllmod1);
-        b43_radio_write16(dev, B2055_VCO_CAPTAIL, e->radio_vco_captail);
-        b43_radio_write16(dev, B2055_VCO_CAL1, e->radio_vco_cal1);
-        b43_radio_write16(dev, B2055_VCO_CAL2, e->radio_vco_cal2);
-        b43_radio_write16(dev, B2055_PLL_LFC1, e->radio_pll_lfc1);
-        b43_radio_write16(dev, B2055_PLL_LFR1, e->radio_pll_lfr1);
-        b43_radio_write16(dev, B2055_PLL_LFC2, e->radio_pll_lfc2);
-        b43_radio_write16(dev, B2055_LGBUF_CENBUF, e->radio_lgbuf_cenbuf);
-        b43_radio_write16(dev, B2055_LGEN_TUNE1, e->radio_lgen_tune1);
-        b43_radio_write16(dev, B2055_LGEN_TUNE2, e->radio_lgen_tune2);
-        b43_radio_write16(dev, B2055_C1_LGBUF_ATUNE, e->radio_c1_lgbuf_atune);
-        b43_radio_write16(dev, B2055_C1_LGBUF_GTUNE, e->radio_c1_lgbuf_gtune);
-        b43_radio_write16(dev, B2055_C1_RX_RFR1, e->radio_c1_rx_rfr1);
-        b43_radio_write16(dev, B2055_C1_TX_PGAPADTN, e->radio_c1_tx_pgapadtn);
-        b43_radio_write16(dev, B2055_C1_TX_MXBGTRIM, e->radio_c1_tx_mxbgtrim);
-        b43_radio_write16(dev, B2055_C2_LGBUF_ATUNE, e->radio_c2_lgbuf_atune);
-        b43_radio_write16(dev, B2055_C2_LGBUF_GTUNE, e->radio_c2_lgbuf_gtune);
-        b43_radio_write16(dev, B2055_C2_RX_RFR1, e->radio_c2_rx_rfr1);
-        b43_radio_write16(dev, B2055_C2_TX_PGAPADTN, e->radio_c2_tx_pgapadtn);
-        b43_radio_write16(dev, B2055_C2_TX_MXBGTRIM, e->radio_c2_tx_mxbgtrim);
+                                const struct b43_nphy_channeltab_entry_rev2 *e)
+{
+        b43_radio_write(dev, B2055_PLL_REF, e->radio_pll_ref);
+        b43_radio_write(dev, B2055_RF_PLLMOD0, e->radio_rf_pllmod0);
+        b43_radio_write(dev, B2055_RF_PLLMOD1, e->radio_rf_pllmod1);
+        b43_radio_write(dev, B2055_VCO_CAPTAIL, e->radio_vco_captail);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+
+        b43_radio_write(dev, B2055_VCO_CAL1, e->radio_vco_cal1);
+        b43_radio_write(dev, B2055_VCO_CAL2, e->radio_vco_cal2);
+        b43_radio_write(dev, B2055_PLL_LFC1, e->radio_pll_lfc1);
+        b43_radio_write(dev, B2055_PLL_LFR1, e->radio_pll_lfr1);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+
+        b43_radio_write(dev, B2055_PLL_LFC2, e->radio_pll_lfc2);
+        b43_radio_write(dev, B2055_LGBUF_CENBUF, e->radio_lgbuf_cenbuf);
+        b43_radio_write(dev, B2055_LGEN_TUNE1, e->radio_lgen_tune1);
+        b43_radio_write(dev, B2055_LGEN_TUNE2, e->radio_lgen_tune2);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+
+        b43_radio_write(dev, B2055_C1_LGBUF_ATUNE, e->radio_c1_lgbuf_atune);
+        b43_radio_write(dev, B2055_C1_LGBUF_GTUNE, e->radio_c1_lgbuf_gtune);
+        b43_radio_write(dev, B2055_C1_RX_RFR1, e->radio_c1_rx_rfr1);
+        b43_radio_write(dev, B2055_C1_TX_PGAPADTN, e->radio_c1_tx_pgapadtn);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+
+        b43_radio_write(dev, B2055_C1_TX_MXBGTRIM, e->radio_c1_tx_mxbgtrim);
+        b43_radio_write(dev, B2055_C2_LGBUF_ATUNE, e->radio_c2_lgbuf_atune);
+        b43_radio_write(dev, B2055_C2_LGBUF_GTUNE, e->radio_c2_lgbuf_gtune);
+        b43_radio_write(dev, B2055_C2_RX_RFR1, e->radio_c2_rx_rfr1);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+
+        b43_radio_write(dev, B2055_C2_TX_PGAPADTN, e->radio_c2_tx_pgapadtn);
+        b43_radio_write(dev, B2055_C2_TX_MXBGTRIM, e->radio_c2_tx_mxbgtrim);
 }
 
 static void b43_chantab_phy_upload(struct b43_wldev *dev,
-                                   const struct b43_nphy_channeltab_entry *e)
+                                   const struct b43_phy_n_sfo_cfg *e)
 {
         b43_phy_write(dev, B43_NPHY_BW1A, e->phy_bw1a);
         b43_phy_write(dev, B43_NPHY_BW2, e->phy_bw2);
@@ -131,34 +157,20 @@ static void b43_nphy_tx_power_fix(struct b43_wldev *dev)
         //TODO
 }
 
-/* Tune the hardware to a new channel. */
-static int nphy_channel_switch(struct b43_wldev *dev, unsigned int channel)
-{
-        const struct b43_nphy_channeltab_entry *tabent;
 
-        tabent = b43_nphy_get_chantabent(dev, channel);
-        if (!tabent)
-                return -ESRCH;
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/Radio/2055Setup */
+static void b43_radio_2055_setup(struct b43_wldev *dev,
+                                const struct b43_nphy_channeltab_entry_rev2 *e)
+{
+        B43_WARN_ON(dev->phy.rev >= 3);
 
-        //FIXME enable/disable band select upper20 in RXCTL
-        if (0 /*FIXME 5Ghz*/)
-                b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, 0x20);
-        else
-                b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, 0x50);
-        b43_chantab_radio_upload(dev, tabent);
+        b43_chantab_radio_upload(dev, e);
         udelay(50);
-        b43_radio_write16(dev, B2055_VCO_CAL10, 5);
-        b43_radio_write16(dev, B2055_VCO_CAL10, 45);
-        b43_radio_write16(dev, B2055_VCO_CAL10, 65);
+        b43_radio_write(dev, B2055_VCO_CAL10, 0x05);
+        b43_radio_write(dev, B2055_VCO_CAL10, 0x45);
+        b43_read32(dev, B43_MMIO_MACCTL); /* flush writes */
+        b43_radio_write(dev, B2055_VCO_CAL10, 0x65);
         udelay(300);
-        if (0 /*FIXME 5Ghz*/)
-                b43_phy_set(dev, B43_NPHY_BANDCTL, B43_NPHY_BANDCTL_5GHZ);
-        else
-                b43_phy_mask(dev, B43_NPHY_BANDCTL, ~B43_NPHY_BANDCTL_5GHZ);
-        b43_chantab_phy_upload(dev, tabent);
-        b43_nphy_tx_power_fix(dev);
-
-        return 0;
 }
 
 static void b43_radio_init2055_pre(struct b43_wldev *dev)
@@ -174,52 +186,64 @@ static void b43_radio_init2055_pre(struct b43_wldev *dev)
 
 static void b43_radio_init2055_post(struct b43_wldev *dev)
 {
+        struct b43_phy_n *nphy = dev->phy.n;
         struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
         struct ssb_boardinfo *binfo = &(dev->dev->bus->boardinfo);
         int i;
         u16 val;
+        bool workaround = false;
+
+        if (sprom->revision < 4)
+                workaround = (binfo->vendor != PCI_VENDOR_ID_BROADCOM ||
+                                binfo->type != 0x46D ||
+                                binfo->rev < 0x41);
+        else
+                workaround = ((sprom->boardflags_hi & B43_BFH_NOPA) == 0);
 
         b43_radio_mask(dev, B2055_MASTER1, 0xFFF3);
-        msleep(1);
-        if ((sprom->revision != 4) ||
-           !(sprom->boardflags_hi & B43_BFH_RSSIINV)) {
-                if ((binfo->vendor != PCI_VENDOR_ID_BROADCOM) ||
-                    (binfo->type != 0x46D) ||
-                    (binfo->rev < 0x41)) {
-                        b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
-                        b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
-                        msleep(1);
-                }
+        if (workaround) {
+                b43_radio_mask(dev, B2055_C1_RX_BB_REG, 0x7F);
+                b43_radio_mask(dev, B2055_C2_RX_BB_REG, 0x7F);
         }
-        b43_radio_maskset(dev, B2055_RRCCAL_NOPTSEL, 0x3F, 0x2C);
-        msleep(1);
-        b43_radio_write16(dev, B2055_CAL_MISC, 0x3C);
-        msleep(1);
+        b43_radio_maskset(dev, B2055_RRCCAL_NOPTSEL, 0xFFC0, 0x2C);
+        b43_radio_write(dev, B2055_CAL_MISC, 0x3C);
         b43_radio_mask(dev, B2055_CAL_MISC, 0xFFBE);
-        msleep(1);
         b43_radio_set(dev, B2055_CAL_LPOCTL, 0x80);
-        msleep(1);
         b43_radio_set(dev, B2055_CAL_MISC, 0x1);
         msleep(1);
         b43_radio_set(dev, B2055_CAL_MISC, 0x40);
-        msleep(1);
-        for (i = 0; i < 100; i++) {
-                val = b43_radio_read16(dev, B2055_CAL_COUT2);
-                if (val & 0x80)
+        for (i = 0; i < 200; i++) {
+                val = b43_radio_read(dev, B2055_CAL_COUT2);
+                if (val & 0x80) {
+                        i = 0;
                         break;
+                }
                 udelay(10);
         }
-        msleep(1);
+        if (i)
+                b43err(dev->wl, "radio post init timeout\n");
         b43_radio_mask(dev, B2055_CAL_LPOCTL, 0xFF7F);
-        msleep(1);
         nphy_channel_switch(dev, dev->phy.channel);
-        b43_radio_write16(dev, B2055_C1_RX_BB_LPF, 0x9);
-        b43_radio_write16(dev, B2055_C2_RX_BB_LPF, 0x9);
-        b43_radio_write16(dev, B2055_C1_RX_BB_MIDACHP, 0x83);
-        b43_radio_write16(dev, B2055_C2_RX_BB_MIDACHP, 0x83);
+        b43_radio_write(dev, B2055_C1_RX_BB_LPF, 0x9);
+        b43_radio_write(dev, B2055_C2_RX_BB_LPF, 0x9);
+        b43_radio_write(dev, B2055_C1_RX_BB_MIDACHP, 0x83);
+        b43_radio_write(dev, B2055_C2_RX_BB_MIDACHP, 0x83);
+        b43_radio_maskset(dev, B2055_C1_LNA_GAINBST, 0xFFF8, 0x6);
+        b43_radio_maskset(dev, B2055_C2_LNA_GAINBST, 0xFFF8, 0x6);
+        if (!nphy->gain_boost) {
+                b43_radio_set(dev, B2055_C1_RX_RFSPC1, 0x2);
+                b43_radio_set(dev, B2055_C2_RX_RFSPC1, 0x2);
+        } else {
+                b43_radio_mask(dev, B2055_C1_RX_RFSPC1, 0xFFFD);
+                b43_radio_mask(dev, B2055_C2_RX_RFSPC1, 0xFFFD);
+        }
+        udelay(2);
 }
 
-/* Initialize a Broadcom 2055 N-radio */
+/*
+ * Initialize a Broadcom 2055 N-radio
+ * http://bcm-v4.sipsolutions.net/802.11/Radio/2055/Init
+ */
 static void b43_radio_init2055(struct b43_wldev *dev)
 {
         b43_radio_init2055_pre(dev);
@@ -230,16 +254,15 @@ static void b43_radio_init2055(struct b43_wldev *dev)
         b43_radio_init2055_post(dev);
 }
 
-void b43_nphy_radio_turn_on(struct b43_wldev *dev)
+/*
+ * Initialize a Broadcom 2056 N-radio
+ * http://bcm-v4.sipsolutions.net/802.11/Radio/2056/Init
+ */
+static void b43_radio_init2056(struct b43_wldev *dev)
 {
-        b43_radio_init2055(dev);
+        /* TODO */
 }
 
-void b43_nphy_radio_turn_off(struct b43_wldev *dev)
-{
-        b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
-                     ~B43_NPHY_RFCTL_CMD_EN);
-}
 
 /*
  * Upload the N-PHY tables.
@@ -647,6 +670,41 @@ static void b43_nphy_read_clip_detection(struct b43_wldev *dev, u16 *clip_st)
         clip_st[1] = b43_phy_read(dev, B43_NPHY_C2_CLIP1THRES);
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SuperSwitchInit */
+static void b43_nphy_superswitch_init(struct b43_wldev *dev, bool init)
+{
+        if (dev->phy.rev >= 3) {
+                if (!init)
+                        return;
+                if (0 /* FIXME */) {
+                        b43_ntab_write(dev, B43_NTAB16(9, 2), 0x211);
+                        b43_ntab_write(dev, B43_NTAB16(9, 3), 0x222);
+                        b43_ntab_write(dev, B43_NTAB16(9, 8), 0x144);
+                        b43_ntab_write(dev, B43_NTAB16(9, 12), 0x188);
+                }
+        } else {
+                b43_phy_write(dev, B43_NPHY_GPIO_LOOEN, 0);
+                b43_phy_write(dev, B43_NPHY_GPIO_HIOEN, 0);
+
+                ssb_chipco_gpio_control(&dev->dev->bus->chipco, 0xFC00,
+                                        0xFC00);
+                b43_write32(dev, B43_MMIO_MACCTL,
+                        b43_read32(dev, B43_MMIO_MACCTL) &
+                        ~B43_MACCTL_GPOUTSMSK);
+                b43_write16(dev, B43_MMIO_GPIO_MASK,
+                        b43_read16(dev, B43_MMIO_GPIO_MASK) | 0xFC00);
+                b43_write16(dev, B43_MMIO_GPIO_CONTROL,
+                        b43_read16(dev, B43_MMIO_GPIO_CONTROL) & ~0xFC00);
+
+                if (init) {
+                        b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO1, 0x2D8);
+                        b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP1, 0x301);
+                        b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_LO2, 0x2D8);
+                        b43_phy_write(dev, B43_NPHY_RFCTL_LUT_TRSW_UP2, 0x301);
+                }
+        }
+}
+
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/classifier */
 static u16 b43_nphy_classifier(struct b43_wldev *dev, u16 mask, u16 val)
 {
@@ -723,7 +781,7 @@ static void b43_nphy_spur_workaround(struct b43_wldev *dev)
 {
         struct b43_phy_n *nphy = dev->phy.n;
 
-        unsigned int channel;
+        u8 channel = nphy->radio_chanspec.channel;
         int tone[2] = { 57, 58 };
         u32 noise[2] = { 0x3FF, 0x3FF };
 
@@ -732,8 +790,6 @@ static void b43_nphy_spur_workaround(struct b43_wldev *dev)
         if (nphy->hang_avoid)
                 b43_nphy_stay_in_carrier_search(dev, 1);
 
-        /* FIXME: channel = radio_chanspec */
-
         if (nphy->gband_spurwar_en) {
                 /* TODO: N PHY Adjust Analog Pfbw (7) */
                 if (channel == 11 && dev->phy.is_40mhz)
@@ -779,6 +835,62 @@ static void b43_nphy_spur_workaround(struct b43_wldev *dev)
                 b43_nphy_stay_in_carrier_search(dev, 0);
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/AdjustLnaGainTbl */
+static void b43_nphy_adjust_lna_gain_table(struct b43_wldev *dev)
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        u8 i;
+        s16 tmp;
+        u16 data[4];
+        s16 gain[2];
+        u16 minmax[2];
+        u16 lna_gain[4] = { -2, 10, 19, 25 };
+
+        if (nphy->hang_avoid)
+                b43_nphy_stay_in_carrier_search(dev, 1);
+
+        if (nphy->gain_boost) {
+                if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
+                        gain[0] = 6;
+                        gain[1] = 6;
+                } else {
+                        tmp = 40370 - 315 * nphy->radio_chanspec.channel;
+                        gain[0] = ((tmp >> 13) + ((tmp >> 12) & 1));
+                        tmp = 23242 - 224 * nphy->radio_chanspec.channel;
+                        gain[1] = ((tmp >> 13) + ((tmp >> 12) & 1));
+                }
+        } else {
+                gain[0] = 0;
+                gain[1] = 0;
+        }
+
+        for (i = 0; i < 2; i++) {
+                if (nphy->elna_gain_config) {
+                        data[0] = 19 + gain[i];
+                        data[1] = 25 + gain[i];
+                        data[2] = 25 + gain[i];
+                        data[3] = 25 + gain[i];
+                } else {
+                        data[0] = lna_gain[0] + gain[i];
+                        data[1] = lna_gain[1] + gain[i];
+                        data[2] = lna_gain[2] + gain[i];
+                        data[3] = lna_gain[3] + gain[i];
+                }
+                b43_ntab_write_bulk(dev, B43_NTAB16(10, 8), 4, data);
+
+                minmax[i] = 23 + gain[i];
+        }
+
+        b43_phy_maskset(dev, B43_NPHY_C1_MINMAX_GAIN, ~B43_NPHY_C1_MINGAIN,
+                                minmax[0] << B43_NPHY_C1_MINGAIN_SHIFT);
+        b43_phy_maskset(dev, B43_NPHY_C2_MINMAX_GAIN, ~B43_NPHY_C2_MINGAIN,
+                                minmax[1] << B43_NPHY_C2_MINGAIN_SHIFT);
+
+        if (nphy->hang_avoid)
+                b43_nphy_stay_in_carrier_search(dev, 0);
+}
+
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/WorkaroundsGainCtrl */
 static void b43_nphy_gain_crtl_workarounds(struct b43_wldev *dev)
 {
@@ -863,7 +975,7 @@ static void b43_nphy_gain_crtl_workarounds(struct b43_wldev *dev)
                 b43_phy_write(dev, B43_NPHY_TABLE_DATALO,
                                         (code << 8 | 0x7C));
 
-                /* TODO: b43_nphy_adjust_lna_gain_table(dev); */
+                b43_nphy_adjust_lna_gain_table(dev);
 
                 if (nphy->elna_gain_config) {
                         b43_phy_write(dev, B43_NPHY_TABLE_ADDR, 0x0808);
@@ -1970,12 +2082,12 @@ static void b43_nphy_restore_rssi_cal(struct b43_wldev *dev)
         u16 *rssical_phy_regs = NULL;
 
         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
-                if (!nphy->rssical_chanspec_2G)
+                if (b43_empty_chanspec(&nphy->rssical_chanspec_2G))
                         return;
                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_2G;
                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_2G;
         } else {
-                if (!nphy->rssical_chanspec_5G)
+                if (b43_empty_chanspec(&nphy->rssical_chanspec_5G))
                         return;
                 rssical_radio_regs = nphy->rssical_cache.rssical_radio_regs_5G;
                 rssical_phy_regs = nphy->rssical_cache.rssical_phy_regs_5G;
@@ -2395,7 +2507,7 @@ static void b43_nphy_save_cal(struct b43_wldev *dev)
 
         struct b43_phy_n_iq_comp *rxcal_coeffs = NULL;
         u16 *txcal_radio_regs = NULL;
-        u8 *iqcal_chanspec;
+        struct b43_chanspec *iqcal_chanspec;
         u16 *table = NULL;
 
         if (nphy->hang_avoid)
@@ -2451,12 +2563,12 @@ static void b43_nphy_restore_cal(struct b43_wldev *dev)
         struct b43_phy_n_iq_comp *rxcal_coeffs = NULL;
 
         if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
-                if (nphy->iqcal_chanspec_2G == 0)
+                if (b43_empty_chanspec(&nphy->iqcal_chanspec_2G))
                         return;
                 table = nphy->cal_cache.txcal_coeffs_2G;
                 loft = &nphy->cal_cache.txcal_coeffs_2G[5];
         } else {
-                if (nphy->iqcal_chanspec_5G == 0)
+                if (b43_empty_chanspec(&nphy->iqcal_chanspec_5G))
                         return;
                 table = nphy->cal_cache.txcal_coeffs_5G;
                 loft = &nphy->cal_cache.txcal_coeffs_5G[5];
@@ -2689,7 +2801,7 @@ static int b43_nphy_cal_tx_iq_lo(struct b43_wldev *dev,
                         }
                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 88), 4,
                                                 buffer);
-                        b43_ntab_write_bulk(dev, B43_NTAB16(15, 101), 2,
+                        b43_ntab_read_bulk(dev, B43_NTAB16(15, 101), 2,
                                                 buffer);
                         b43_ntab_write_bulk(dev, B43_NTAB16(15, 85), 2,
                                                 buffer);
@@ -2701,8 +2813,7 @@ static int b43_nphy_cal_tx_iq_lo(struct b43_wldev *dev,
                         b43_ntab_read_bulk(dev, B43_NTAB16(15, 96), length,
                                                 nphy->txiqlocal_bestc);
                         nphy->txiqlocal_coeffsvalid = true;
-                        /* TODO: Set nphy->txiqlocal_chanspec to
-                                the current channel */
+                        nphy->txiqlocal_chanspec = nphy->radio_chanspec;
                 } else {
                         length = 11;
                         if (dev->phy.rev < 3)
@@ -2737,7 +2848,8 @@ static void b43_nphy_reapply_tx_cal_coeffs(struct b43_wldev *dev)
         u16 buffer[7];
         bool equal = true;
 
-        if (!nphy->txiqlocal_coeffsvalid || 1 /* FIXME */)
+        if (!nphy->txiqlocal_coeffsvalid ||
+            b43_eq_chanspecs(&nphy->txiqlocal_chanspec, &nphy->radio_chanspec))
                 return;
 
         b43_ntab_read_bulk(dev, B43_NTAB16(15, 80), 7, buffer);
@@ -3092,9 +3204,11 @@ int b43_phy_initn(struct b43_wldev *dev)
         do_rssi_cal = false;
         if (phy->rev >= 3) {
                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
-                        do_rssi_cal = (nphy->rssical_chanspec_2G == 0);
+                        do_rssi_cal =
+                                b43_empty_chanspec(&nphy->rssical_chanspec_2G);
                 else
-                        do_rssi_cal = (nphy->rssical_chanspec_5G == 0);
+                        do_rssi_cal =
+                                b43_empty_chanspec(&nphy->rssical_chanspec_5G);
 
                 if (do_rssi_cal)
                         b43_nphy_rssi_cal(dev);
@@ -3106,9 +3220,9 @@ int b43_phy_initn(struct b43_wldev *dev)
 
         if (!((nphy->measure_hold & 0x6) != 0)) {
                 if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
-                        do_cal = (nphy->iqcal_chanspec_2G == 0);
+                        do_cal = b43_empty_chanspec(&nphy->iqcal_chanspec_2G);
                 else
-                        do_cal = (nphy->iqcal_chanspec_5G == 0);
+                        do_cal = b43_empty_chanspec(&nphy->iqcal_chanspec_5G);
 
                 if (nphy->mute)
                         do_cal = false;
@@ -3117,7 +3231,7 @@ int b43_phy_initn(struct b43_wldev *dev)
                         target = b43_nphy_get_tx_gains(dev);
 
                         if (nphy->antsel_type == 2)
-                                ;/*TODO NPHY Superswitch Init with argument 1*/
+                                b43_nphy_superswitch_init(dev, true);
                         if (nphy->perical != 2) {
                                 b43_nphy_rssi_cal(dev);
                                 if (phy->rev >= 3) {
@@ -3155,6 +3269,133 @@ int b43_phy_initn(struct b43_wldev *dev)
         return 0;
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/ChanspecSetup */
+static void b43_nphy_chanspec_setup(struct b43_wldev *dev,
+                                const struct b43_phy_n_sfo_cfg *e,
+                                struct b43_chanspec chanspec)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        u16 tmp;
+        u32 tmp32;
+
+        tmp = b43_phy_read(dev, B43_NPHY_BANDCTL) & B43_NPHY_BANDCTL_5GHZ;
+        if (chanspec.b_freq == 1 && tmp == 0) {
+                tmp32 = b43_read32(dev, B43_MMIO_PSM_PHY_HDR);
+                b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32 | 4);
+                b43_phy_set(dev, B43_PHY_B_BBCFG, 0xC000);
+                b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32);
+                b43_phy_set(dev, B43_NPHY_BANDCTL, B43_NPHY_BANDCTL_5GHZ);
+        } else if (chanspec.b_freq == 1) {
+                b43_phy_mask(dev, B43_NPHY_BANDCTL, ~B43_NPHY_BANDCTL_5GHZ);
+                tmp32 = b43_read32(dev, B43_MMIO_PSM_PHY_HDR);
+                b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32 | 4);
+                b43_phy_mask(dev, B43_PHY_B_BBCFG, (u16)~0xC000);
+                b43_write32(dev, B43_MMIO_PSM_PHY_HDR, tmp32);
+        }
+
+        b43_chantab_phy_upload(dev, e);
+
+        tmp = chanspec.channel;
+        if (chanspec.b_freq == 1)
+                tmp |= 0x0100;
+        if (chanspec.b_width == 3)
+                tmp |= 0x0200;
+        b43_shm_write16(dev, B43_SHM_SHARED, 0xA0, tmp);
+
+        if (nphy->radio_chanspec.channel == 14) {
+                b43_nphy_classifier(dev, 2, 0);
+                b43_phy_set(dev, B43_PHY_B_TEST, 0x0800);
+        } else {
+                b43_nphy_classifier(dev, 2, 2);
+                if (chanspec.b_freq == 2)
+                        b43_phy_mask(dev, B43_PHY_B_TEST, ~0x840);
+        }
+
+        if (nphy->txpwrctrl)
+                b43_nphy_tx_power_fix(dev);
+
+        if (dev->phy.rev < 3)
+                b43_nphy_adjust_lna_gain_table(dev);
+
+        b43_nphy_tx_lp_fbw(dev);
+
+        if (dev->phy.rev >= 3 && 0) {
+                /* TODO */
+        }
+
+        b43_phy_write(dev, B43_NPHY_NDATAT_DUP40, 0x3830);
+
+        if (phy->rev >= 3)
+                b43_nphy_spur_workaround(dev);
+}
+
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/SetChanspec */
+static int b43_nphy_set_chanspec(struct b43_wldev *dev,
+                                        struct b43_chanspec chanspec)
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        const struct b43_nphy_channeltab_entry_rev2 *tabent_r2;
+        const struct b43_nphy_channeltab_entry_rev3 *tabent_r3;
+
+        u8 tmp;
+        u8 channel = chanspec.channel;
+
+        if (dev->phy.rev >= 3) {
+                /* TODO */
+                tabent_r3 = NULL;
+                if (!tabent_r3)
+                        return -ESRCH;
+        } else {
+                tabent_r2 = b43_nphy_get_chantabent_rev2(dev, channel);
+                if (!tabent_r2)
+                        return -ESRCH;
+        }
+
+        nphy->radio_chanspec = chanspec;
+
+        if (chanspec.b_width != nphy->b_width)
+                ; /* TODO: BMAC BW Set (chanspec.b_width) */
+
+        /* TODO: use defines */
+        if (chanspec.b_width == 3) {
+                if (chanspec.sideband == 2)
+                        b43_phy_set(dev, B43_NPHY_RXCTL,
+                                        B43_NPHY_RXCTL_BSELU20);
+                else
+                        b43_phy_mask(dev, B43_NPHY_RXCTL,
+                                        ~B43_NPHY_RXCTL_BSELU20);
+        }
+
+        if (dev->phy.rev >= 3) {
+                tmp = (chanspec.b_freq == 1) ? 4 : 0;
+                b43_radio_maskset(dev, 0x08, 0xFFFB, tmp);
+                /* TODO: PHY Radio2056 Setup (dev, tabent_r3); */
+                b43_nphy_chanspec_setup(dev, &(tabent_r3->phy_regs), chanspec);
+        } else {
+                tmp = (chanspec.b_freq == 1) ? 0x0020 : 0x0050;
+                b43_radio_maskset(dev, B2055_MASTER1, 0xFF8F, tmp);
+                b43_radio_2055_setup(dev, tabent_r2);
+                b43_nphy_chanspec_setup(dev, &(tabent_r2->phy_regs), chanspec);
+        }
+
+        return 0;
+}
+
+/* Tune the hardware to a new channel */
+static int nphy_channel_switch(struct b43_wldev *dev, unsigned int channel)
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        struct b43_chanspec chanspec;
+        chanspec = nphy->radio_chanspec;
+        chanspec.channel = channel;
+
+        return b43_nphy_set_chanspec(dev, chanspec);
+}
+
 static int b43_nphy_op_allocate(struct b43_wldev *dev)
 {
         struct b43_phy_n *nphy;
@@ -3243,9 +3484,43 @@ static void b43_nphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
         b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/Radio/Switch%20Radio */
 static void b43_nphy_op_software_rfkill(struct b43_wldev *dev,
                                         bool blocked)
-{//TODO
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        if (b43_read32(dev, B43_MMIO_MACCTL) & B43_MACCTL_ENABLED)
+                b43err(dev->wl, "MAC not suspended\n");
+
+        if (blocked) {
+                b43_phy_mask(dev, B43_NPHY_RFCTL_CMD,
+                                ~B43_NPHY_RFCTL_CMD_CHIP0PU);
+                if (dev->phy.rev >= 3) {
+                        b43_radio_mask(dev, 0x09, ~0x2);
+
+                        b43_radio_write(dev, 0x204D, 0);
+                        b43_radio_write(dev, 0x2053, 0);
+                        b43_radio_write(dev, 0x2058, 0);
+                        b43_radio_write(dev, 0x205E, 0);
+                        b43_radio_mask(dev, 0x2062, ~0xF0);
+                        b43_radio_write(dev, 0x2064, 0);
+
+                        b43_radio_write(dev, 0x304D, 0);
+                        b43_radio_write(dev, 0x3053, 0);
+                        b43_radio_write(dev, 0x3058, 0);
+                        b43_radio_write(dev, 0x305E, 0);
+                        b43_radio_mask(dev, 0x3062, ~0xF0);
+                        b43_radio_write(dev, 0x3064, 0);
+                }
+        } else {
+                if (dev->phy.rev >= 3) {
+                        b43_radio_init2056(dev);
+                        b43_nphy_set_chanspec(dev, nphy->radio_chanspec);
+                } else {
+                        b43_radio_init2055(dev);
+                }
+        }
 }
 
 static void b43_nphy_op_switch_analog(struct b43_wldev *dev, bool on)
diff --git a/drivers/net/wireless/b43/phy_n.h b/drivers/net/wireless/b43/phy_n.h
index 403aad3f894f..8b6d570dd0aa 100644
--- a/drivers/net/wireless/b43/phy_n.h
+++ b/drivers/net/wireless/b43/phy_n.h
@@ -711,6 +711,8 @@
 #define B43_NPHY_PAPD_EN1                       B43_PHY_N(0x29B) /* PAPD Enable1 TBD */
 #define B43_NPHY_EPS_TABLE_ADJ1                 B43_PHY_N(0x29C) /* EPS Table Adj1 TBD */
 
+#define B43_PHY_B_BBCFG                         B43_PHY_N_BMODE(0x001) /* BB config */
+#define B43_PHY_B_TEST                          B43_PHY_N_BMODE(0x00A)
 
 
 /* Broadcom 2055 radio registers */
@@ -924,6 +926,13 @@
 
 struct b43_wldev;
 
+struct b43_chanspec {
+        u8 channel;
+        u8 sideband;
+        u8 b_width;
+        u8 b_freq;
+};
+
 struct b43_phy_n_iq_comp {
         s16 a0;
         s16 b0;
@@ -975,7 +984,8 @@ struct b43_phy_n {
         u16 papd_epsilon_offset[2];
         s32 preamble_override;
         u32 bb_mult_save;
-        u16 radio_chanspec;
+        u8 b_width;
+        struct b43_chanspec radio_chanspec;
 
         bool gain_boost;
         bool elna_gain_config;
@@ -991,6 +1001,7 @@ struct b43_phy_n {
         u16 txiqlocal_bestc[11];
         bool txiqlocal_coeffsvalid;
         struct b43_phy_n_txpwrindex txpwrindex[2];
+        struct b43_chanspec txiqlocal_chanspec;
 
         u8 txrx_chain;
         u16 tx_rx_cal_phy_saveregs[11];
@@ -1006,12 +1017,12 @@ struct b43_phy_n {
         bool gband_spurwar_en;
 
         bool ipa2g_on;
-        u8 iqcal_chanspec_2G;
-        u8 rssical_chanspec_2G;
+        struct b43_chanspec iqcal_chanspec_2G;
+        struct b43_chanspec rssical_chanspec_2G;
 
         bool ipa5g_on;
-        u8 iqcal_chanspec_5G;
-        u8 rssical_chanspec_5G;
+        struct b43_chanspec iqcal_chanspec_5G;
+        struct b43_chanspec rssical_chanspec_5G;
 
         struct b43_phy_n_rssical_cache rssical_cache;
         struct b43_phy_n_cal_cache cal_cache;
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/b43/tables_nphy.c
index a00d509150f7..d96e870ab8fe 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/b43/tables_nphy.c
@@ -318,14 +318,14 @@ void b2055_upload_inittab(struct b43_wldev *dev,
         .radio_c2_tx_mxbgtrim   = r21
 
 #define PHYREGS(r0, r1, r2, r3, r4, r5) \
-        .phy_bw1a       = r0,           \
-        .phy_bw2        = r1,           \
-        .phy_bw3        = r2,           \
-        .phy_bw4        = r3,           \
-        .phy_bw5        = r4,           \
-        .phy_bw6        = r5
-
-static const struct b43_nphy_channeltab_entry b43_nphy_channeltab[] = {
+        .phy_regs.phy_bw1a      = r0,   \
+        .phy_regs.phy_bw2       = r1,   \
+        .phy_regs.phy_bw3       = r2,   \
+        .phy_regs.phy_bw4       = r3,   \
+        .phy_regs.phy_bw5       = r4,   \
+        .phy_regs.phy_bw6       = r5
+
+static const struct b43_nphy_channeltab_entry_rev2 b43_nphy_channeltab[] = {
   {     .channel                = 184,
         .freq                   = 4920, /* MHz */
         .unk2                   = 3280,
@@ -1320,10 +1320,10 @@ static const struct b43_nphy_channeltab_entry b43_nphy_channeltab[] = {
   },
 };
 
-const struct b43_nphy_channeltab_entry *
-b43_nphy_get_chantabent(struct b43_wldev *dev, u8 channel)
+const struct b43_nphy_channeltab_entry_rev2 *
+b43_nphy_get_chantabent_rev2(struct b43_wldev *dev, u8 channel)
 {
-        const struct b43_nphy_channeltab_entry *e;
+        const struct b43_nphy_channeltab_entry_rev2 *e;
         unsigned int i;
 
         for (i = 0; i < ARRAY_SIZE(b43_nphy_channeltab); i++) {
diff --git a/drivers/net/wireless/b43/tables_nphy.h b/drivers/net/wireless/b43/tables_nphy.h
index 9c1c6ecd3672..8fc1da9f8fe5 100644
--- a/drivers/net/wireless/b43/tables_nphy.h
+++ b/drivers/net/wireless/b43/tables_nphy.h
@@ -4,9 +4,22 @@
 #include <linux/types.h>
 
 
-struct b43_nphy_channeltab_entry {
+struct b43_phy_n_sfo_cfg {
+        u16 phy_bw1a;
+        u16 phy_bw2;
+        u16 phy_bw3;
+        u16 phy_bw4;
+        u16 phy_bw5;
+        u16 phy_bw6;
+};
+
+struct b43_nphy_channeltab_entry_rev2 {
         /* The channel number */
         u8 channel;
+        /* The channel frequency in MHz */
+        u16 freq;
+        /* An unknown value */
+        u16 unk2;
         /* Radio register values on channelswitch */
         u8 radio_pll_ref;
         u8 radio_rf_pllmod0;
@@ -31,16 +44,18 @@ struct b43_nphy_channeltab_entry {
         u8 radio_c2_tx_pgapadtn;
         u8 radio_c2_tx_mxbgtrim;
         /* PHY register values on channelswitch */
-        u16 phy_bw1a;
-        u16 phy_bw2;
-        u16 phy_bw3;
-        u16 phy_bw4;
-        u16 phy_bw5;
-        u16 phy_bw6;
+        struct b43_phy_n_sfo_cfg phy_regs;
+};
+
+struct b43_nphy_channeltab_entry_rev3 {
+        /* The channel number */
+        u8 channel;
         /* The channel frequency in MHz */
         u16 freq;
-        /* An unknown value */
-        u16 unk2;
+        /* Radio register values on channelswitch */
+        /* TODO */
+        /* PHY register values on channelswitch */
+        struct b43_phy_n_sfo_cfg phy_regs;
 };
 
 
@@ -77,8 +92,8 @@ void b2055_upload_inittab(struct b43_wldev *dev,
 
 /* Get the NPHY Channel Switch Table entry for a channel number.
  * Returns NULL on failure to find an entry. */
-const struct b43_nphy_channeltab_entry *
-b43_nphy_get_chantabent(struct b43_wldev *dev, u8 channel);
+const struct b43_nphy_channeltab_entry_rev2 *
+b43_nphy_get_chantabent_rev2(struct b43_wldev *dev, u8 channel);
 
 
 /* The N-PHY tables. */
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index eda06529ef5f..e6b0528f3b52 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -610,7 +610,6 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
         }
 
         /* Link quality statistics */
-        status.noise = dev->stats.link_noise;
         if ((chanstat & B43_RX_CHAN_PHYTYPE) == B43_PHYTYPE_N) {
 //              s8 rssi = max(rxhdr->power0, rxhdr->power1);
                 //TODO: Find out what the rssi value is (dBm or percentage?)