Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/linville/wireless-next-2.6

72 files changed, 7384 insertions, 6535 deletions

diff --git a/drivers/net/wireless/ath9k/ath9k.h b/drivers/net/wireless/ath9k/ath9k.h
index d1b0fbae5a32..28b8d84f49b4 100644
--- a/drivers/net/wireless/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath9k/ath9k.h
@@ -144,6 +144,7 @@ struct ath_desc {
 #define ATH9K_TXDESC_EXT_AND_CTL        0x0080
 #define ATH9K_TXDESC_VMF                0x0100
 #define ATH9K_TXDESC_FRAG_IS_ON         0x0200
+#define ATH9K_TXDESC_CAB                0x0400
 
 #define ATH9K_RXDESC_INTREQ             0x0020
 
@@ -564,8 +565,6 @@ enum ath9k_cipher {
 #define CTL_5GHT40              8
 
 #define AR_EEPROM_MAC(i)        (0x1d+(i))
-#define EEP_SCALE       100
-#define EEP_DELTA       10
 
 #define AR_EEPROM_RFSILENT_GPIO_SEL     0x001c
 #define AR_EEPROM_RFSILENT_GPIO_SEL_S   2
@@ -606,9 +605,6 @@ struct ath9k_country_entry {
 #define REG_CLR_BIT(_a, _r, _f) \
         REG_WRITE(_a, _r, REG_READ(_a, _r) & ~_f)
 
-#define ATH9K_COMP_BUF_MAX_SIZE   9216
-#define ATH9K_COMP_BUF_ALIGN_SIZE 512
-
 #define ATH9K_TXQ_USE_LOCKOUT_BKOFF_DIS   0x00000001
 
 #define INIT_AIFS       2
@@ -632,12 +628,6 @@ struct ath9k_country_entry {
                                  (IEEE80211_WEP_IVLEN +         \
                                   IEEE80211_WEP_KIDLEN +        \
                                   IEEE80211_WEP_CRCLEN))
-#define IEEE80211_MAX_LEN       (2300 + FCS_LEN +               \
-                                 (IEEE80211_WEP_IVLEN +         \
-                                  IEEE80211_WEP_KIDLEN +        \
-                                  IEEE80211_WEP_CRCLEN))
-
-#define MAX_REG_ADD_COUNT   129
 #define MAX_RATE_POWER 63
 
 enum ath9k_power_mode {
@@ -707,13 +697,6 @@ enum phytype {
 };
 #define PHY_CCK PHY_DS
 
-enum start_adhoc_option {
-        START_ADHOC_NO_11A,
-        START_ADHOC_PER_11D,
-        START_ADHOC_IN_11A,
-        START_ADHOC_IN_11B,
-};
-
 enum ath9k_tp_scale {
         ATH9K_TP_SCALE_MAX = 0,
         ATH9K_TP_SCALE_50,
@@ -769,14 +752,11 @@ struct ath9k_node_stats {
 
 #define ATH9K_RSSI_EP_MULTIPLIER  (1<<7)
 
-enum ath9k_gpio_output_mux_type {
-        ATH9K_GPIO_OUTPUT_MUX_AS_OUTPUT,
-        ATH9K_GPIO_OUTPUT_MUX_AS_PCIE_ATTENTION_LED,
-        ATH9K_GPIO_OUTPUT_MUX_AS_PCIE_POWER_LED,
-        ATH9K_GPIO_OUTPUT_MUX_AS_MAC_NETWORK_LED,
-        ATH9K_GPIO_OUTPUT_MUX_AS_MAC_POWER_LED,
-        ATH9K_GPIO_OUTPUT_MUX_NUM_ENTRIES
-};
+#define AR_GPIO_OUTPUT_MUX_AS_OUTPUT             0
+#define AR_GPIO_OUTPUT_MUX_AS_PCIE_ATTENTION_LED 1
+#define AR_GPIO_OUTPUT_MUX_AS_PCIE_POWER_LED     2
+#define AR_GPIO_OUTPUT_MUX_AS_MAC_NETWORK_LED    5
+#define AR_GPIO_OUTPUT_MUX_AS_MAC_POWER_LED      6
 
 enum {
         ATH9K_RESET_POWER_ON,
@@ -790,19 +770,20 @@ struct ath_hal {
         u32 ah_magic;
         u16 ah_devid;
         u16 ah_subvendorid;
-        struct ath_softc *ah_sc;
-        void __iomem *ah_sh;
-        u16 ah_countryCode;
         u32 ah_macVersion;
         u16 ah_macRev;
         u16 ah_phyRev;
         u16 ah_analog5GhzRev;
         u16 ah_analog2GhzRev;
-        u8 ah_decompMask[ATH9K_DECOMP_MASK_SIZE];
-        u32 ah_flags;
+
+        void __iomem *ah_sh;
+        struct ath_softc *ah_sc;
         enum ath9k_opmode ah_opmode;
         struct ath9k_ops_config ah_config;
         struct ath9k_hw_capabilities ah_caps;
+
+        u16 ah_countryCode;
+        u32 ah_flags;
         int16_t ah_powerLimit;
         u16 ah_maxPowerLevel;
         u32 ah_tpScale;
@@ -812,15 +793,16 @@ struct ath_hal {
         u16 ah_currentRD5G;
         u16 ah_currentRD2G;
         char ah_iso[4];
-        enum start_adhoc_option ah_adHocMode;
-        bool ah_commonMode;
+
         struct ath9k_channel ah_channels[150];
-        u32 ah_nchan;
         struct ath9k_channel *ah_curchan;
+        u32 ah_nchan;
+
         u16 ah_rfsilent;
         bool ah_rfkillEnabled;
         bool ah_isPciExpress;
         u16 ah_txTrigLevel;
+
 #ifndef ATH_NF_PER_CHAN
         struct ath9k_nfcal_hist nfCalHist[NUM_NF_READINGS];
 #endif
@@ -853,7 +835,7 @@ bool ath9k_regd_init_channels(struct ath_hal *ah,
 u32 ath9k_hw_mhz2ieee(struct ath_hal *ah, u32 freq, u32 flags);
 enum ath9k_int ath9k_hw_set_interrupts(struct ath_hal *ah,
                                      enum ath9k_int ints);
-bool ath9k_hw_reset(struct ath_hal *ah, enum ath9k_opmode opmode,
+bool ath9k_hw_reset(struct ath_hal *ah,
                     struct ath9k_channel *chan,
                     enum ath9k_ht_macmode macmode,
                     u8 txchainmask, u8 rxchainmask,
@@ -1018,4 +1000,7 @@ void ath9k_hw_get_channel_centers(struct ath_hal *ah,
 bool ath9k_get_channel_edges(struct ath_hal *ah,
                              u16 flags, u16 *low,
                              u16 *high);
+void ath9k_hw_cfg_output(struct ath_hal *ah, u32 gpio,
+                        u32 ah_signal_type);
+void ath9k_hw_set_gpio(struct ath_hal *ah, u32 gpio, u32 value);
 #endif
diff --git a/drivers/net/wireless/ath9k/beacon.c b/drivers/net/wireless/ath9k/beacon.c
index caf569401a34..c43fd5861163 100644
--- a/drivers/net/wireless/ath9k/beacon.c
+++ b/drivers/net/wireless/ath9k/beacon.c
@@ -33,7 +33,7 @@ static int ath_beaconq_config(struct ath_softc *sc)
         struct ath9k_tx_queue_info qi;
 
         ath9k_hw_get_txq_props(ah, sc->sc_bhalq, &qi);
-        if (sc->sc_opmode == ATH9K_M_HOSTAP) {
+        if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP) {
                 /* Always burst out beacon and CAB traffic. */
                 qi.tqi_aifs = 1;
                 qi.tqi_cwmin = 0;
@@ -85,7 +85,7 @@ static void ath_beacon_setup(struct ath_softc *sc,
 
         flags = ATH9K_TXDESC_NOACK;
 
-        if (sc->sc_opmode == ATH9K_M_IBSS &&
+        if (sc->sc_ah->ah_opmode == ATH9K_M_IBSS &&
             (ah->ah_caps.hw_caps & ATH9K_HW_CAP_VEOL)) {
                 ds->ds_link = bf->bf_daddr; /* self-linked */
                 flags |= ATH9K_TXDESC_VEOL;
@@ -111,24 +111,24 @@ static void ath_beacon_setup(struct ath_softc *sc,
         rix = 0;
         rt = sc->sc_currates;
         rate = rt->info[rix].rateCode;
-        if (sc->sc_flags & ATH_PREAMBLE_SHORT)
+        if (sc->sc_flags & SC_OP_PREAMBLE_SHORT)
                 rate |= rt->info[rix].shortPreamble;
 
-        ath9k_hw_set11n_txdesc(ah, ds
-                              , skb->len + FCS_LEN /* frame length */
-                              , ATH9K_PKT_TYPE_BEACON /* Atheros packet type */
-                              , avp->av_btxctl.txpower /* txpower XXX */
-                              , ATH9K_TXKEYIX_INVALID /* no encryption */
-                              , ATH9K_KEY_TYPE_CLEAR /* no encryption */
-                              , flags /* no ack, veol for beacons */
+        ath9k_hw_set11n_txdesc(ah, ds,
+                               skb->len + FCS_LEN, /* frame length */
+                               ATH9K_PKT_TYPE_BEACON, /* Atheros packet type */
+                               avp->av_btxctl.txpower, /* txpower XXX */
+                               ATH9K_TXKEYIX_INVALID, /* no encryption */
+                               ATH9K_KEY_TYPE_CLEAR, /* no encryption */
+                               flags /* no ack, veol for beacons */
                 );
 
         /* NB: beacon's BufLen must be a multiple of 4 bytes */
-        ath9k_hw_filltxdesc(ah, ds
-                           , roundup(skb->len, 4) /* buffer length */
-                           , true /* first segment */
-                           , true /* last segment */
-                           , ds /* first descriptor */
+        ath9k_hw_filltxdesc(ah, ds,
+                            roundup(skb->len, 4), /* buffer length */
+                            true, /* first segment */
+                            true, /* last segment */
+                            ds /* first descriptor */
                 );
 
         memzero(series, sizeof(struct ath9k_11n_rate_series) * 4);
@@ -140,55 +140,6 @@ static void ath_beacon_setup(struct ath_softc *sc,
                 ctsrate, ctsduration, series, 4, 0);
 }
 
-/* Move everything from the vap's mcast queue to the hardware cab queue.
- * Caller must hold mcasq lock and cabq lock
- * XXX MORE_DATA bit?
- */
-static void empty_mcastq_into_cabq(struct ath_hal *ah,
-        struct ath_txq *mcastq, struct ath_txq *cabq)
-{
-        struct ath_buf *bfmcast;
-
-        BUG_ON(list_empty(&mcastq->axq_q));
-
-        bfmcast = list_first_entry(&mcastq->axq_q, struct ath_buf, list);
-
-        /* link the descriptors */
-        if (!cabq->axq_link)
-                ath9k_hw_puttxbuf(ah, cabq->axq_qnum, bfmcast->bf_daddr);
-        else
-                *cabq->axq_link = bfmcast->bf_daddr;
-
-        /* append the private vap mcast list to  the cabq */
-
-        cabq->axq_depth += mcastq->axq_depth;
-        cabq->axq_totalqueued += mcastq->axq_totalqueued;
-        cabq->axq_linkbuf = mcastq->axq_linkbuf;
-        cabq->axq_link = mcastq->axq_link;
-        list_splice_tail_init(&mcastq->axq_q, &cabq->axq_q);
-        mcastq->axq_depth = 0;
-        mcastq->axq_totalqueued = 0;
-        mcastq->axq_linkbuf = NULL;
-        mcastq->axq_link = NULL;
-}
-
-/* This is only run at DTIM. We move everything from the vap's mcast queue
- * to the hardware cab queue. Caller must hold the mcastq lock. */
-static void trigger_mcastq(struct ath_hal *ah,
-        struct ath_txq *mcastq, struct ath_txq *cabq)
-{
-        spin_lock_bh(&cabq->axq_lock);
-
-        if (!list_empty(&mcastq->axq_q))
-                empty_mcastq_into_cabq(ah, mcastq, cabq);
-
-        /* cabq is gated by beacon so it is safe to start here */
-        if (!list_empty(&cabq->axq_q))
-                ath9k_hw_txstart(ah, cabq->axq_qnum);
-
-        spin_unlock_bh(&cabq->axq_lock);
-}
-
 /*
  *  Generate beacon frame and queue cab data for a vap.
  *
@@ -199,19 +150,14 @@ static void trigger_mcastq(struct ath_hal *ah,
 */
 static struct ath_buf *ath_beacon_generate(struct ath_softc *sc, int if_id)
 {
-        struct ath_hal *ah = sc->sc_ah;
         struct ath_buf *bf;
         struct ath_vap *avp;
         struct sk_buff *skb;
         int cabq_depth;
-        int mcastq_depth;
-        int is_beacon_dtim = 0;
-        unsigned int curlen;
         struct ath_txq *cabq;
-        struct ath_txq *mcastq;
+        struct ieee80211_tx_info *info;
         avp = sc->sc_vaps[if_id];
 
-        mcastq = &avp->av_mcastq;
         cabq = sc->sc_cabq;
 
         ASSERT(avp);
@@ -223,32 +169,33 @@ static struct ath_buf *ath_beacon_generate(struct ath_softc *sc, int if_id)
         }
         bf = avp->av_bcbuf;
         skb = (struct sk_buff *) bf->bf_mpdu;
+        if (skb) {
+                pci_unmap_single(sc->pdev, bf->bf_dmacontext,
+                                 skb_end_pointer(skb) - skb->head,
+                                 PCI_DMA_TODEVICE);
+        }
 
-        /*
-         * Update dynamic beacon contents.  If this returns
-         * non-zero then we need to remap the memory because
-         * the beacon frame changed size (probably because
-         * of the TIM bitmap).
-         */
-        curlen = skb->len;
-
-        /* XXX: spin_lock_bh should not be used here, but sparse bitches
-         * otherwise. We should fix sparse :) */
-        spin_lock_bh(&mcastq->axq_lock);
-        mcastq_depth = avp->av_mcastq.axq_depth;
-
-        if (ath_update_beacon(sc, if_id, &avp->av_boff, skb, mcastq_depth) ==
-            1) {
-                ath_skb_unmap_single(sc, skb, PCI_DMA_TODEVICE,
-                                     get_dma_mem_context(bf, bf_dmacontext));
-                bf->bf_buf_addr = ath_skb_map_single(sc, skb, PCI_DMA_TODEVICE,
-                        get_dma_mem_context(bf, bf_dmacontext));
-        } else {
-                pci_dma_sync_single_for_cpu(sc->pdev,
-                                            bf->bf_buf_addr,
-                                            skb_tailroom(skb),
-                                            PCI_DMA_TODEVICE);
+        skb = ieee80211_beacon_get(sc->hw, avp->av_if_data);
+        bf->bf_mpdu = skb;
+        if (skb == NULL)
+                return NULL;
+        info = IEEE80211_SKB_CB(skb);
+        if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+                /*
+                 * TODO: make sure the seq# gets assigned properly (vs. other
+                 * TX frames)
+                 */
+                struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+                sc->seq_no += 0x10;
+                hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
+                hdr->seq_ctrl |= cpu_to_le16(sc->seq_no);
         }
+        bf->bf_buf_addr = bf->bf_dmacontext =
+                pci_map_single(sc->pdev, skb->data,
+                               skb_end_pointer(skb) - skb->head,
+                               PCI_DMA_TODEVICE);
+
+        skb = ieee80211_get_buffered_bc(sc->hw, avp->av_if_data);
 
         /*
          * if the CABQ traffic from previous DTIM is pending and the current
@@ -262,9 +209,7 @@ static struct ath_buf *ath_beacon_generate(struct ath_softc *sc, int if_id)
         cabq_depth = cabq->axq_depth;
         spin_unlock_bh(&cabq->axq_lock);
 
-        is_beacon_dtim = avp->av_boff.bo_tim[4] & 1;
-
-        if (mcastq_depth && is_beacon_dtim && cabq_depth) {
+        if (skb && cabq_depth) {
                 /*
                  * Unlock the cabq lock as ath_tx_draintxq acquires
                  * the lock again which is a common function and that
@@ -284,10 +229,11 @@ static struct ath_buf *ath_beacon_generate(struct ath_softc *sc, int if_id)
          * Enable the CAB queue before the beacon queue to
          * insure cab frames are triggered by this beacon.
          */
-        if (is_beacon_dtim)
-                trigger_mcastq(ah, mcastq, cabq);
+        while (skb) {
+                ath_tx_cabq(sc, skb);
+                skb = ieee80211_get_buffered_bc(sc->hw, avp->av_if_data);
+        }
 
-        spin_unlock_bh(&mcastq->axq_lock);
         return bf;
 }
 
@@ -375,7 +321,7 @@ int ath_beacon_alloc(struct ath_softc *sc, int if_id)
                                 struct ath_buf, list);
                 list_del(&avp->av_bcbuf->list);
 
-                if (sc->sc_opmode == ATH9K_M_HOSTAP ||
+                if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP ||
                     !(sc->sc_ah->ah_caps.hw_caps & ATH9K_HW_CAP_VEOL)) {
                         int slot;
                         /*
@@ -408,8 +354,9 @@ int ath_beacon_alloc(struct ath_softc *sc, int if_id)
         bf = avp->av_bcbuf;
         if (bf->bf_mpdu != NULL) {
                 skb = (struct sk_buff *)bf->bf_mpdu;
-                ath_skb_unmap_single(sc, skb, PCI_DMA_TODEVICE,
-                                     get_dma_mem_context(bf, bf_dmacontext));
+                pci_unmap_single(sc->pdev, bf->bf_dmacontext,
+                                 skb_end_pointer(skb) - skb->head,
+                                 PCI_DMA_TODEVICE);
                 dev_kfree_skb_any(skb);
                 bf->bf_mpdu = NULL;
         }
@@ -418,7 +365,7 @@ int ath_beacon_alloc(struct ath_softc *sc, int if_id)
          * NB: the beacon data buffer must be 32-bit aligned;
          * we assume the wbuf routines will return us something
          * with this alignment (perhaps should assert).
-         * FIXME: Fill avp->av_boff.bo_tim,avp->av_btxctl.txpower and
+         * FIXME: Fill avp->av_btxctl.txpower and
          * avp->av_btxctl.shortPreamble
          */
         skb = ieee80211_beacon_get(sc->hw, avp->av_if_data);
@@ -439,9 +386,8 @@ int ath_beacon_alloc(struct ath_softc *sc, int if_id)
                 __le64 val;
                 int intval;
 
-                /* FIXME: Use default value for now: Sujith */
-
-                intval = ATH_DEFAULT_BINTVAL;
+                intval = sc->hw->conf.beacon_int ?
+                        sc->hw->conf.beacon_int : ATH_DEFAULT_BINTVAL;
 
                 /*
                  * The beacon interval is in TU's; the TSF in usecs.
@@ -466,8 +412,10 @@ int ath_beacon_alloc(struct ath_softc *sc, int if_id)
                 memcpy(&wh[1], &val, sizeof(val));
         }
 
-        bf->bf_buf_addr = ath_skb_map_single(sc, skb, PCI_DMA_TODEVICE,
-                get_dma_mem_context(bf, bf_dmacontext));
+        bf->bf_buf_addr = bf->bf_dmacontext =
+                pci_map_single(sc->pdev, skb->data,
+                               skb_end_pointer(skb) - skb->head,
+                               PCI_DMA_TODEVICE);
         bf->bf_mpdu = skb;
 
         return 0;
@@ -493,8 +441,9 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vap *avp)
                 bf = avp->av_bcbuf;
                 if (bf->bf_mpdu != NULL) {
                         struct sk_buff *skb = (struct sk_buff *)bf->bf_mpdu;
-                        ath_skb_unmap_single(sc, skb, PCI_DMA_TODEVICE,
-                                get_dma_mem_context(bf, bf_dmacontext));
+                        pci_unmap_single(sc->pdev, bf->bf_dmacontext,
+                                         skb_end_pointer(skb) - skb->head,
+                                         PCI_DMA_TODEVICE);
                         dev_kfree_skb_any(skb);
                         bf->bf_mpdu = NULL;
                 }
@@ -505,30 +454,6 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vap *avp)
 }
 
 /*
- *  Reclaim beacon resources and return buffer to the pool.
- *
- *  This function will free any wbuf frames that are still attached to the
- *  beacon buffers in the ATH object.  Note that this does not de-allocate
- *  any wbuf objects that are in the transmit queue and have not yet returned
- *  to the ATH object.
-*/
-
-void ath_beacon_free(struct ath_softc *sc)
-{
-        struct ath_buf *bf;
-
-        list_for_each_entry(bf, &sc->sc_bbuf, list) {
-                if (bf->bf_mpdu != NULL) {
-                        struct sk_buff *skb = (struct sk_buff *) bf->bf_mpdu;
-                        ath_skb_unmap_single(sc, skb, PCI_DMA_TODEVICE,
-                                get_dma_mem_context(bf, bf_dmacontext));
-                        dev_kfree_skb_any(skb);
-                        bf->bf_mpdu = NULL;
-                }
-        }
-}
-
-/*
  * Tasklet for Sending Beacons
  *
  * Transmit one or more beacon frames at SWBA.  Dynamic updates to the frame
@@ -540,9 +465,6 @@ void ath_beacon_free(struct ath_softc *sc)
 
 void ath9k_beacon_tasklet(unsigned long data)
 {
-#define TSF_TO_TU(_h,_l)                                        \
-        ((((u32)(_h)) << 22) | (((u32)(_l)) >> 10))
-
         struct ath_softc *sc = (struct ath_softc *)data;
         struct ath_hal *ah = sc->sc_ah;
         struct ath_buf *bf = NULL;
@@ -555,7 +477,7 @@ void ath9k_beacon_tasklet(unsigned long data)
         u32 tsftu;
         u16 intval;
 
-        if (sc->sc_noreset) {
+        if (sc->sc_flags & SC_OP_NO_RESET) {
                 show_cycles = ath9k_hw_GetMibCycleCountsPct(ah,
                                                             &rx_clear,
                                                             &rx_frame,
@@ -577,7 +499,7 @@ void ath9k_beacon_tasklet(unsigned long data)
                  *      (in that layer).
                  */
                 if (sc->sc_bmisscount < BSTUCK_THRESH) {
-                        if (sc->sc_noreset) {
+                        if (sc->sc_flags & SC_OP_NO_RESET) {
                                 DPRINTF(sc, ATH_DBG_BEACON,
                                         "%s: missed %u consecutive beacons\n",
                                         __func__, sc->sc_bmisscount);
@@ -605,7 +527,7 @@ void ath9k_beacon_tasklet(unsigned long data)
                                         __func__, sc->sc_bmisscount);
                         }
                 } else if (sc->sc_bmisscount >= BSTUCK_THRESH) {
-                        if (sc->sc_noreset) {
+                        if (sc->sc_flags & SC_OP_NO_RESET) {
                                 if (sc->sc_bmisscount == BSTUCK_THRESH) {
                                         DPRINTF(sc,
                                                 ATH_DBG_BEACON,
@@ -624,7 +546,7 @@ void ath9k_beacon_tasklet(unsigned long data)
                 return;
         }
         if (sc->sc_bmisscount != 0) {
-                if (sc->sc_noreset) {
+                if (sc->sc_flags & SC_OP_NO_RESET) {
                         DPRINTF(sc,
                                 ATH_DBG_BEACON,
                                 "%s: resume beacon xmit after %u misses\n",
@@ -643,8 +565,8 @@ void ath9k_beacon_tasklet(unsigned long data)
          * on the tsf to safeguard against missing an swba.
          */
 
-        /* FIXME: Use default value for now - Sujith */
-        intval = ATH_DEFAULT_BINTVAL;
+        intval = sc->hw->conf.beacon_int ?
+                sc->hw->conf.beacon_int : ATH_DEFAULT_BINTVAL;
 
         tsf = ath9k_hw_gettsf64(ah);
         tsftu = TSF_TO_TU(tsf>>32, tsf);
@@ -704,7 +626,6 @@ void ath9k_beacon_tasklet(unsigned long data)
 
                 sc->ast_be_xmit += bc;     /* XXX per-vap? */
         }
-#undef TSF_TO_TU
 }
 
 /*
@@ -719,7 +640,7 @@ void ath_bstuck_process(struct ath_softc *sc)
         DPRINTF(sc, ATH_DBG_BEACON,
                 "%s: stuck beacon; resetting (bmiss count %u)\n",
                 __func__, sc->sc_bmisscount);
-        ath_internal_reset(sc);
+        ath_reset(sc, false);
 }
 
 /*
@@ -740,8 +661,6 @@ void ath_bstuck_process(struct ath_softc *sc)
 
 void ath_beacon_config(struct ath_softc *sc, int if_id)
 {
-#define TSF_TO_TU(_h,_l)                                        \
-        ((((u32)(_h)) << 22) | (((u32)(_l)) >> 10))
         struct ath_hal *ah = sc->sc_ah;
         u32 nexttbtt, intval;
         struct ath_beacon_config conf;
@@ -750,7 +669,7 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
         if (if_id != ATH_IF_ID_ANY)
                 av_opmode = sc->sc_vaps[if_id]->av_opmode;
         else
-                av_opmode = sc->sc_opmode;
+                av_opmode = sc->sc_ah->ah_opmode;
 
         memzero(&conf, sizeof(struct ath_beacon_config));
 
@@ -760,7 +679,8 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
          * Protocol stack doesn't support dynamic beacon configuration,
          * use default configurations.
          */
-        conf.beacon_interval = ATH_DEFAULT_BINTVAL;
+        conf.beacon_interval = sc->hw->conf.beacon_int ?
+                sc->hw->conf.beacon_int : ATH_DEFAULT_BINTVAL;
         conf.listen_interval = 1;
         conf.dtim_period = conf.beacon_interval;
         conf.dtim_count = 1;
@@ -770,7 +690,7 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
         nexttbtt = TSF_TO_TU(get_unaligned_le32(conf.u.last_tstamp + 4),
                              get_unaligned_le32(conf.u.last_tstamp));
         /* XXX conditionalize multi-bss support? */
-        if (sc->sc_opmode == ATH9K_M_HOSTAP) {
+        if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP) {
                 /*
                  * For multi-bss ap support beacons are either staggered
                  * evenly over N slots or burst together.  For the former
@@ -791,7 +711,7 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
         DPRINTF(sc, ATH_DBG_BEACON, "%s: nexttbtt %u intval %u (%u)\n",
                 __func__, nexttbtt, intval, conf.beacon_interval);
         /* Check for ATH9K_M_HOSTAP and sc_nostabeacons for WDS client */
-        if (sc->sc_opmode == ATH9K_M_STA) {
+        if (sc->sc_ah->ah_opmode == ATH9K_M_STA) {
                 struct ath9k_beacon_state bs;
                 u64 tsf;
                 u32 tsftu;
@@ -886,19 +806,19 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
                         "cfp:period %u "
                         "maxdur %u "
                         "next %u "
-                        "timoffset %u\n"
-                        , __func__
-                        , (unsigned long long)tsf, tsftu
-                        , bs.bs_intval
-                        , bs.bs_nexttbtt
-                        , bs.bs_dtimperiod
-                        , bs.bs_nextdtim
-                        , bs.bs_bmissthreshold
-                        , bs.bs_sleepduration
-                        , bs.bs_cfpperiod
-                        , bs.bs_cfpmaxduration
-                        , bs.bs_cfpnext
-                        , bs.bs_timoffset
+                        "timoffset %u\n",
+                        __func__,
+                        (unsigned long long)tsf, tsftu,
+                        bs.bs_intval,
+                        bs.bs_nexttbtt,
+                        bs.bs_dtimperiod,
+                        bs.bs_nextdtim,
+                        bs.bs_bmissthreshold,
+                        bs.bs_sleepduration,
+                        bs.bs_cfpperiod,
+                        bs.bs_cfpmaxduration,
+                        bs.bs_cfpnext,
+                        bs.bs_timoffset
                         );
 
                 ath9k_hw_set_interrupts(ah, 0);
@@ -911,7 +831,7 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
                 ath9k_hw_set_interrupts(ah, 0);
                 if (nexttbtt == intval)
                         intval |= ATH9K_BEACON_RESET_TSF;
-                if (sc->sc_opmode == ATH9K_M_IBSS) {
+                if (sc->sc_ah->ah_opmode == ATH9K_M_IBSS) {
                         /*
                          * Pull nexttbtt forward to reflect the current
                          * TSF .
@@ -943,7 +863,7 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
                         if (!(ah->ah_caps.hw_caps & ATH9K_HW_CAP_VEOL))
                                 sc->sc_imask |= ATH9K_INT_SWBA;
                         ath_beaconq_config(sc);
-                } else if (sc->sc_opmode == ATH9K_M_HOSTAP) {
+                } else if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP) {
                         /*
                          * In AP mode we enable the beacon timers and
                          * SWBA interrupts to prepare beacon frames.
@@ -959,11 +879,10 @@ void ath_beacon_config(struct ath_softc *sc, int if_id)
                  * When using a self-linked beacon descriptor in
                  * ibss mode load it once here.
                  */
-                if (sc->sc_opmode == ATH9K_M_IBSS &&
+                if (sc->sc_ah->ah_opmode == ATH9K_M_IBSS &&
                     (ah->ah_caps.hw_caps & ATH9K_HW_CAP_VEOL))
                         ath_beacon_start_adhoc(sc, 0);
         }
-#undef TSF_TO_TU
 }
 
 /* Function to collect beacon rssi data and resync beacon if necessary */
@@ -975,5 +894,5 @@ void ath_beacon_sync(struct ath_softc *sc, int if_id)
          * beacon frame we just received.
          */
         ath_beacon_config(sc, if_id);
-        sc->sc_beacons = 1;
+        sc->sc_flags |= SC_OP_BEACONS;
 }
diff --git a/drivers/net/wireless/ath9k/core.c b/drivers/net/wireless/ath9k/core.c
index f6c45288d0e7..c262ef279ff3 100644
--- a/drivers/net/wireless/ath9k/core.c
+++ b/drivers/net/wireless/ath9k/core.c
@@ -21,9 +21,6 @@
 
 static int ath_outdoor;         /* enable outdoor use */
 
-static const u8 ath_bcast_mac[ETH_ALEN] =
-    { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
 static u32 ath_chainmask_sel_up_rssi_thres =
         ATH_CHAINMASK_SEL_UP_RSSI_THRES;
 static u32 ath_chainmask_sel_down_rssi_thres =
@@ -54,10 +51,8 @@ static void bus_read_cachesize(struct ath_softc *sc, int *csz)
  *  Set current operating mode
  *
  *  This function initializes and fills the rate table in the ATH object based
- *  on the operating mode.  The blink rates are also set up here, although
- *  they have been superceeded by the ath_led module.
+ *  on the operating mode.
 */
-
 static void ath_setcurmode(struct ath_softc *sc, enum wireless_mode mode)
 {
         const struct ath9k_rate_table *rt;
@@ -235,7 +230,7 @@ static int ath_setup_channels(struct ath_softc *sc)
  *  Determine mode from channel flags
  *
  *  This routine will provide the enumerated WIRELESSS_MODE value based
- *  on the settings of the channel flags.  If ho valid set of flags
+ *  on the settings of the channel flags.  If no valid set of flags
  *  exist, the lowest mode (11b) is selected.
 */
 
@@ -260,7 +255,8 @@ static enum wireless_mode ath_chan2mode(struct ath9k_channel *chan)
         else if (chan->chanmode == CHANNEL_G_HT40MINUS)
                 return ATH9K_MODE_11NG_HT40MINUS;
 
-        /* NB: should not get here */
+        WARN_ON(1); /* should not get here */
+
         return ATH9K_MODE_11B;
 }
 
@@ -275,14 +271,12 @@ static int ath_stop(struct ath_softc *sc)
 {
         struct ath_hal *ah = sc->sc_ah;
 
-        DPRINTF(sc, ATH_DBG_CONFIG, "%s: invalid %u\n",
-                __func__, sc->sc_invalid);
+        DPRINTF(sc, ATH_DBG_CONFIG, "%s: invalid %ld\n",
+                __func__, sc->sc_flags & SC_OP_INVALID);
 
         /*
          * Shutdown the hardware and driver:
          *    stop output from above
-         *    reset 802.11 state machine
-         *      (sends station deassoc/deauth frames)
          *    turn off timers
          *    disable interrupts
          *    clear transmit machinery
@@ -294,10 +288,10 @@ static int ath_stop(struct ath_softc *sc)
          * hardware is gone (invalid).
          */
 
-        if (!sc->sc_invalid)
+        if (!(sc->sc_flags & SC_OP_INVALID))
                 ath9k_hw_set_interrupts(ah, 0);
         ath_draintxq(sc, false);
-        if (!sc->sc_invalid) {
+        if (!(sc->sc_flags & SC_OP_INVALID)) {
                 ath_stoprecv(sc);
                 ath9k_hw_phy_disable(ah);
         } else
@@ -307,56 +301,6 @@ static int ath_stop(struct ath_softc *sc)
 }
 
 /*
- *  Start Scan
- *
- *  This function is called when starting a channel scan.  It will perform
- *  power save wakeup processing, set the filter for the scan, and get the
- *  chip ready to send broadcast packets out during the scan.
-*/
-
-void ath_scan_start(struct ath_softc *sc)
-{
-        struct ath_hal *ah = sc->sc_ah;
-        u32 rfilt;
-        u32 now = (u32) jiffies_to_msecs(get_timestamp());
-
-        sc->sc_scanning = 1;
-        rfilt = ath_calcrxfilter(sc);
-        ath9k_hw_setrxfilter(ah, rfilt);
-        ath9k_hw_write_associd(ah, ath_bcast_mac, 0);
-
-        /* Restore previous power management state. */
-
-        DPRINTF(sc, ATH_DBG_CONFIG, "%d.%03d | %s: RX filter 0x%x aid 0\n",
-                now / 1000, now % 1000, __func__, rfilt);
-}
-
-/*
- *  Scan End
- *
- *  This routine is called by the upper layer when the scan is completed.  This
- *  will set the filters back to normal operating mode, set the BSSID to the
- *  correct value, and restore the power save state.
-*/
-
-void ath_scan_end(struct ath_softc *sc)
-{
-        struct ath_hal *ah = sc->sc_ah;
-        u32 rfilt;
-        u32 now = (u32) jiffies_to_msecs(get_timestamp());
-
-        sc->sc_scanning = 0;
-        /* Request for a full reset due to rx packet filter changes */
-        sc->sc_full_reset = 1;
-        rfilt = ath_calcrxfilter(sc);
-        ath9k_hw_setrxfilter(ah, rfilt);
-        ath9k_hw_write_associd(ah, sc->sc_curbssid, sc->sc_curaid);
-
-        DPRINTF(sc, ATH_DBG_CONFIG, "%d.%03d | %s: RX filter 0x%x aid 0x%x\n",
-                now / 1000, now % 1000, __func__, rfilt, sc->sc_curaid);
-}
-
-/*
  * Set the current channel
  *
  * Set/change channels.  If the channel is really being changed, it's done
@@ -367,25 +311,23 @@ int ath_set_channel(struct ath_softc *sc, struct ath9k_channel *hchan)
 {
         struct ath_hal *ah = sc->sc_ah;
         bool fastcc = true, stopped;
-        enum ath9k_ht_macmode ht_macmode;
 
-        if (sc->sc_invalid)     /* if the device is invalid or removed */
+        if (sc->sc_flags & SC_OP_INVALID) /* the device is invalid or removed */
                 return -EIO;
 
         DPRINTF(sc, ATH_DBG_CONFIG,
                 "%s: %u (%u MHz) -> %u (%u MHz), cflags:%x\n",
                 __func__,
-                ath9k_hw_mhz2ieee(ah, sc->sc_curchan.channel,
-                                  sc->sc_curchan.channelFlags),
-                sc->sc_curchan.channel,
+                ath9k_hw_mhz2ieee(ah, sc->sc_ah->ah_curchan->channel,
+                                  sc->sc_ah->ah_curchan->channelFlags),
+                sc->sc_ah->ah_curchan->channel,
                 ath9k_hw_mhz2ieee(ah, hchan->channel, hchan->channelFlags),
                 hchan->channel, hchan->channelFlags);
 
-        ht_macmode = ath_cwm_macmode(sc);
-
-        if (hchan->channel != sc->sc_curchan.channel ||
-            hchan->channelFlags != sc->sc_curchan.channelFlags ||
-            sc->sc_update_chainmask || sc->sc_full_reset) {
+        if (hchan->channel != sc->sc_ah->ah_curchan->channel ||
+            hchan->channelFlags != sc->sc_ah->ah_curchan->channelFlags ||
+            (sc->sc_flags & SC_OP_CHAINMASK_UPDATE) ||
+            (sc->sc_flags & SC_OP_FULL_RESET)) {
                 int status;
                 /*
                  * This is only performed if the channel settings have
@@ -404,15 +346,16 @@ int ath_set_channel(struct ath_softc *sc, struct ath9k_channel *hchan)
                  * to flush data frames already in queue because of
                  * changing channel. */
 
-                if (!stopped || sc->sc_full_reset)
+                if (!stopped || (sc->sc_flags & SC_OP_FULL_RESET))
                         fastcc = false;
 
                 spin_lock_bh(&sc->sc_resetlock);
-                if (!ath9k_hw_reset(ah, sc->sc_opmode, hchan,
-                                        ht_macmode, sc->sc_tx_chainmask,
-                                        sc->sc_rx_chainmask,
-                                        sc->sc_ht_extprotspacing,
-                                        fastcc, &status)) {
+                if (!ath9k_hw_reset(ah, hchan,
+                                    sc->sc_ht_info.tx_chan_width,
+                                    sc->sc_tx_chainmask,
+                                    sc->sc_rx_chainmask,
+                                    sc->sc_ht_extprotspacing,
+                                    fastcc, &status)) {
                         DPRINTF(sc, ATH_DBG_FATAL,
                                 "%s: unable to reset channel %u (%uMhz) "
                                 "flags 0x%x hal status %u\n", __func__,
@@ -424,9 +367,8 @@ int ath_set_channel(struct ath_softc *sc, struct ath9k_channel *hchan)
                 }
                 spin_unlock_bh(&sc->sc_resetlock);
 
-                sc->sc_curchan = *hchan;
-                sc->sc_update_chainmask = 0;
-                sc->sc_full_reset = 0;
+                sc->sc_flags &= ~SC_OP_CHAINMASK_UPDATE;
+                sc->sc_flags &= ~SC_OP_FULL_RESET;
 
                 /* Re-enable rx framework */
                 if (ath_startrecv(sc) != 0) {
@@ -537,7 +479,7 @@ int ath_chainmask_sel_logic(struct ath_softc *sc, struct ath_node *an)
 
 void ath_update_chainmask(struct ath_softc *sc, int is_ht)
 {
-        sc->sc_update_chainmask = 1;
+        sc->sc_flags |= SC_OP_CHAINMASK_UPDATE;
         if (is_ht) {
                 sc->sc_tx_chainmask = sc->sc_ah->ah_caps.tx_chainmask;
                 sc->sc_rx_chainmask = sc->sc_ah->ah_caps.rx_chainmask;
@@ -554,62 +496,6 @@ void ath_update_chainmask(struct ath_softc *sc, int is_ht)
 /* VAP management */
 /******************/
 
-/*
- *  VAP in Listen mode
- *
- *  This routine brings the VAP out of the down state into a "listen" state
- *  where it waits for association requests.  This is used in AP and AdHoc
- *  modes.
-*/
-
-int ath_vap_listen(struct ath_softc *sc, int if_id)
-{
-        struct ath_hal *ah = sc->sc_ah;
-        struct ath_vap *avp;
-        u32 rfilt = 0;
-        DECLARE_MAC_BUF(mac);
-
-        avp = sc->sc_vaps[if_id];
-        if (avp == NULL) {
-                DPRINTF(sc, ATH_DBG_FATAL, "%s: invalid interface id %u\n",
-                        __func__, if_id);
-                return -EINVAL;
-        }
-
-#ifdef CONFIG_SLOW_ANT_DIV
-        ath_slow_ant_div_stop(&sc->sc_antdiv);
-#endif
-
-        /* update ratectrl about the new state */
-        ath_rate_newstate(sc, avp);
-
-        rfilt = ath_calcrxfilter(sc);
-        ath9k_hw_setrxfilter(ah, rfilt);
-
-        if (sc->sc_opmode == ATH9K_M_STA || sc->sc_opmode == ATH9K_M_IBSS) {
-                memcpy(sc->sc_curbssid, ath_bcast_mac, ETH_ALEN);
-                ath9k_hw_write_associd(ah, sc->sc_curbssid, sc->sc_curaid);
-        } else
-                sc->sc_curaid = 0;
-
-        DPRINTF(sc, ATH_DBG_CONFIG,
-                "%s: RX filter 0x%x bssid %s aid 0x%x\n",
-                __func__, rfilt, print_mac(mac,
-                        sc->sc_curbssid), sc->sc_curaid);
-
-        /*
-         * XXXX
-         * Disable BMISS interrupt when we're not associated
-         */
-        ath9k_hw_set_interrupts(ah,
-                sc->sc_imask & ~(ATH9K_INT_SWBA | ATH9K_INT_BMISS));
-        sc->sc_imask &= ~(ATH9K_INT_SWBA | ATH9K_INT_BMISS);
-        /* need to reconfigure the beacons when it moves to RUN */
-        sc->sc_beacons = 0;
-
-        return 0;
-}
-
 int ath_vap_attach(struct ath_softc *sc,
                    int if_id,
                    struct ieee80211_vif *if_data,
@@ -647,16 +533,13 @@ int ath_vap_attach(struct ath_softc *sc,
         /* Set the VAP opmode */
         avp->av_opmode = opmode;
         avp->av_bslot = -1;
-        INIT_LIST_HEAD(&avp->av_mcastq.axq_q);
-        INIT_LIST_HEAD(&avp->av_mcastq.axq_acq);
-        spin_lock_init(&avp->av_mcastq.axq_lock);
 
         ath9k_hw_set_tsfadjust(sc->sc_ah, 1);
 
         sc->sc_vaps[if_id] = avp;
         sc->sc_nvaps++;
         /* Set the device opmode */
-        sc->sc_opmode = opmode;
+        sc->sc_ah->ah_opmode = opmode;
 
         /* default VAP configuration */
         avp->av_config.av_fixed_rateset = IEEE80211_FIXED_RATE_NONE;
@@ -689,9 +572,6 @@ int ath_vap_detach(struct ath_softc *sc, int if_id)
         ath_stoprecv(sc);       /* stop recv side */
         ath_flushrecv(sc);      /* flush recv queue */
 
-        /* Reclaim any pending mcast bufs on the vap. */
-        ath_tx_draintxq(sc, &avp->av_mcastq, false);
-
         kfree(avp);
         sc->sc_vaps[if_id] = NULL;
         sc->sc_nvaps--;
@@ -728,9 +608,9 @@ int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan)
         struct ath_hal *ah = sc->sc_ah;
         int status;
         int error = 0;
-        enum ath9k_ht_macmode ht_macmode = ath_cwm_macmode(sc);
 
-        DPRINTF(sc, ATH_DBG_CONFIG, "%s: mode %d\n", __func__, sc->sc_opmode);
+        DPRINTF(sc, ATH_DBG_CONFIG, "%s: mode %d\n",
+                __func__, sc->sc_ah->ah_opmode);
 
         /*
          * Stop anything previously setup.  This is safe
@@ -752,16 +632,16 @@ int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan)
          * be followed by initialization of the appropriate bits
          * and then setup of the interrupt mask.
          */
-        sc->sc_curchan = *initial_chan;
 
         spin_lock_bh(&sc->sc_resetlock);
-        if (!ath9k_hw_reset(ah, sc->sc_opmode, &sc->sc_curchan, ht_macmode,
-                           sc->sc_tx_chainmask, sc->sc_rx_chainmask,
-                           sc->sc_ht_extprotspacing, false, &status)) {
+        if (!ath9k_hw_reset(ah, initial_chan,
+                            sc->sc_ht_info.tx_chan_width,
+                            sc->sc_tx_chainmask, sc->sc_rx_chainmask,
+                            sc->sc_ht_extprotspacing, false, &status)) {
                 DPRINTF(sc, ATH_DBG_FATAL,
                         "%s: unable to reset hardware; hal status %u "
                         "(freq %u flags 0x%x)\n", __func__, status,
-                        sc->sc_curchan.channel, sc->sc_curchan.channelFlags);
+                        initial_chan->channel, initial_chan->channelFlags);
                 error = -EIO;
                 spin_unlock_bh(&sc->sc_resetlock);
                 goto done;
@@ -802,7 +682,8 @@ int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan)
          * Note we only do this (at the moment) for station mode.
          */
         if (ath9k_hw_phycounters(ah) &&
-            ((sc->sc_opmode == ATH9K_M_STA) || (sc->sc_opmode == ATH9K_M_IBSS)))
+            ((sc->sc_ah->ah_opmode == ATH9K_M_STA) ||
+             (sc->sc_ah->ah_opmode == ATH9K_M_IBSS)))
                 sc->sc_imask |= ATH9K_INT_MIB;
         /*
          * Some hardware processes the TIM IE and fires an
@@ -811,7 +692,7 @@ int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan)
          * enable the TIM interrupt when operating as station.
          */
         if ((ah->ah_caps.hw_caps & ATH9K_HW_CAP_ENHANCEDPM) &&
-            (sc->sc_opmode == ATH9K_M_STA) &&
+            (sc->sc_ah->ah_opmode == ATH9K_M_STA) &&
             !sc->sc_config.swBeaconProcess)
                 sc->sc_imask |= ATH9K_INT_TIM;
         /*
@@ -823,34 +704,34 @@ int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan)
 
         /* XXX: we must make sure h/w is ready and clear invalid flag
          * before turning on interrupt. */
-        sc->sc_invalid = 0;
+        sc->sc_flags &= ~SC_OP_INVALID;
 done:
         return error;
 }
 
-/*
- * Reset the hardware w/o losing operational state.  This is
- * basically a more efficient way of doing ath_stop, ath_init,
- * followed by state transitions to the current 802.11
- * operational state.  Used to recover from errors rx overrun
- * and to reset the hardware when rf gain settings must be reset.
- */
-
-static int ath_reset_start(struct ath_softc *sc, u32 flag)
+int ath_reset(struct ath_softc *sc, bool retry_tx)
 {
         struct ath_hal *ah = sc->sc_ah;
+        int status;
+        int error = 0;
 
         ath9k_hw_set_interrupts(ah, 0); /* disable interrupts */
-        ath_draintxq(sc, flag & RESET_RETRY_TXQ);       /* stop xmit side */
-        ath_stoprecv(sc);       /* stop recv side */
-        ath_flushrecv(sc);      /* flush recv queue */
+        ath_draintxq(sc, retry_tx);     /* stop xmit */
+        ath_stoprecv(sc);               /* stop recv */
+        ath_flushrecv(sc);              /* flush recv queue */
 
-        return 0;
-}
-
-static int ath_reset_end(struct ath_softc *sc, u32 flag)
-{
-        struct ath_hal *ah = sc->sc_ah;
+        /* Reset chip */
+        spin_lock_bh(&sc->sc_resetlock);
+        if (!ath9k_hw_reset(ah, sc->sc_ah->ah_curchan,
+                            sc->sc_ht_info.tx_chan_width,
+                            sc->sc_tx_chainmask, sc->sc_rx_chainmask,
+                            sc->sc_ht_extprotspacing, false, &status)) {
+                DPRINTF(sc, ATH_DBG_FATAL,
+                        "%s: unable to reset hardware; hal status %u\n",
+                        __func__, status);
+                error = -EIO;
+        }
+        spin_unlock_bh(&sc->sc_resetlock);
 
         if (ath_startrecv(sc) != 0)     /* restart recv */
                 DPRINTF(sc, ATH_DBG_FATAL,
@@ -861,16 +742,17 @@ static int ath_reset_end(struct ath_softc *sc, u32 flag)
          * that changes the channel so update any state that
          * might change as a result.
          */
-        ath_setcurmode(sc, ath_chan2mode(&sc->sc_curchan));
+        ath_setcurmode(sc, ath_chan2mode(sc->sc_ah->ah_curchan));
 
-        ath_update_txpow(sc);   /* update tx power state */
+        ath_update_txpow(sc);
 
-        if (sc->sc_beacons)
+        if (sc->sc_flags & SC_OP_BEACONS)
                 ath_beacon_config(sc, ATH_IF_ID_ANY);   /* restart beacons */
+
         ath9k_hw_set_interrupts(ah, sc->sc_imask);
 
         /* Restart the txq */
-        if (flag & RESET_RETRY_TXQ) {
+        if (retry_tx) {
                 int i;
                 for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
                         if (ATH_TXQ_SETUP(sc, i)) {
@@ -880,28 +762,6 @@ static int ath_reset_end(struct ath_softc *sc, u32 flag)
                         }
                 }
         }
-        return 0;
-}
-
-int ath_reset(struct ath_softc *sc)
-{
-        struct ath_hal *ah = sc->sc_ah;
-        int status;
-        int error = 0;
-        enum ath9k_ht_macmode ht_macmode = ath_cwm_macmode(sc);
-
-        /* NB: indicate channel change so we do a full reset */
-        spin_lock_bh(&sc->sc_resetlock);
-        if (!ath9k_hw_reset(ah, sc->sc_opmode, &sc->sc_curchan,
-                           ht_macmode,
-                           sc->sc_tx_chainmask, sc->sc_rx_chainmask,
-                           sc->sc_ht_extprotspacing, false, &status)) {
-                DPRINTF(sc, ATH_DBG_FATAL,
-                        "%s: unable to reset hardware; hal status %u\n",
-                        __func__, status);
-                error = -EIO;
-        }
-        spin_unlock_bh(&sc->sc_resetlock);
 
         return error;
 }
@@ -911,7 +771,7 @@ int ath_suspend(struct ath_softc *sc)
         struct ath_hal *ah = sc->sc_ah;
 
         /* No I/O if device has been surprise removed */
-        if (sc->sc_invalid)
+        if (sc->sc_flags & SC_OP_INVALID)
                 return -EIO;
 
         /* Shut off the interrupt before setting sc->sc_invalid to '1' */
@@ -919,7 +779,7 @@ int ath_suspend(struct ath_softc *sc)
 
         /* XXX: we must make sure h/w will not generate any interrupt
          * before setting the invalid flag. */
-        sc->sc_invalid = 1;
+        sc->sc_flags |= SC_OP_INVALID;
 
         /* disable HAL and put h/w to sleep */
         ath9k_hw_disable(sc->sc_ah);
@@ -940,7 +800,7 @@ irqreturn_t ath_isr(int irq, void *dev)
         bool sched = false;
 
         do {
-                if (sc->sc_invalid) {
+                if (sc->sc_flags & SC_OP_INVALID) {
                         /*
                          * The hardware is not ready/present, don't
                          * touch anything. Note this can happen early
@@ -1050,7 +910,7 @@ static void ath9k_tasklet(unsigned long data)
 
         if (status & ATH9K_INT_FATAL) {
                 /* need a chip reset */
-                ath_internal_reset(sc);
+                ath_reset(sc, false);
                 return;
         } else {
 
@@ -1093,10 +953,9 @@ int ath_init(u16 devid, struct ath_softc *sc)
         int status;
         int error = 0, i;
         int csz = 0;
-        u32 rd;
 
         /* XXX: hardware will not be ready until ath_open() being called */
-        sc->sc_invalid = 1;
+        sc->sc_flags |= SC_OP_INVALID;
 
         sc->sc_debug = DBG_DEFAULT;
         DPRINTF(sc, ATH_DBG_CONFIG, "%s: devid 0x%x\n", __func__, devid);
@@ -1126,9 +985,6 @@ int ath_init(u16 devid, struct ath_softc *sc)
         }
         sc->sc_ah = ah;
 
-        /* Get the chipset-specific aggr limit. */
-        sc->sc_rtsaggrlimit = ah->ah_caps.rts_aggr_limit;
-
         /* Get the hardware key cache size. */
         sc->sc_keymax = ah->ah_caps.keycache_size;
         if (sc->sc_keymax > ATH_KEYMAX) {
@@ -1162,14 +1018,12 @@ int ath_init(u16 devid, struct ath_softc *sc)
          * is resposible for filtering this list based on settings
          * like the phy mode.
          */
-        rd = ah->ah_currentRD;
-
         error = ath_setup_channels(sc);
         if (error)
                 goto bad;
 
         /* default to STA mode */
-        sc->sc_opmode = ATH9K_M_MONITOR;
+        sc->sc_ah->ah_opmode = ATH9K_M_MONITOR;
 
         /* Setup rate tables */
 
@@ -1240,7 +1094,7 @@ int ath_init(u16 devid, struct ath_softc *sc)
 
         sc->sc_rc = ath_rate_attach(ah);
         if (sc->sc_rc == NULL) {
-                error = EIO;
+                error = -EIO;
                 goto bad2;
         }
 
@@ -1280,20 +1134,13 @@ int ath_init(u16 devid, struct ath_softc *sc)
 
         /* 11n Capabilities */
         if (ah->ah_caps.hw_caps & ATH9K_HW_CAP_HT) {
-                sc->sc_txaggr = 1;
-                sc->sc_rxaggr = 1;
+                sc->sc_flags |= SC_OP_TXAGGR;
+                sc->sc_flags |= SC_OP_RXAGGR;
         }
 
         sc->sc_tx_chainmask = ah->ah_caps.tx_chainmask;
         sc->sc_rx_chainmask = ah->ah_caps.rx_chainmask;
 
-        /* Configuration for rx chain detection */
-        sc->sc_rxchaindetect_ref = 0;
-        sc->sc_rxchaindetect_thresh5GHz = 35;
-        sc->sc_rxchaindetect_thresh2GHz = 35;
-        sc->sc_rxchaindetect_delta5GHz = 30;
-        sc->sc_rxchaindetect_delta2GHz = 30;
-
         ath9k_hw_setcapability(ah, ATH9K_CAP_DIVERSITY, 1, true, NULL);
         sc->sc_defant = ath9k_hw_getdefantenna(ah);
 
@@ -1337,7 +1184,7 @@ void ath_deinit(struct ath_softc *sc)
         DPRINTF(sc, ATH_DBG_CONFIG, "%s\n", __func__);
 
         ath_stop(sc);
-        if (!sc->sc_invalid)
+        if (!(sc->sc_flags & SC_OP_INVALID))
                 ath9k_hw_setpower(sc->sc_ah, ATH9K_PM_AWAKE);
         ath_rate_detach(sc->sc_rc);
         /* cleanup tx queues */
@@ -1464,9 +1311,9 @@ void ath_newassoc(struct ath_softc *sc,
         /* if station reassociates, tear down the aggregation state. */
         if (!isnew) {
                 for (tidno = 0; tidno < WME_NUM_TID; tidno++) {
-                        if (sc->sc_txaggr)
+                        if (sc->sc_flags & SC_OP_TXAGGR)
                                 ath_tx_aggr_teardown(sc, an, tidno);
-                        if (sc->sc_rxaggr)
+                        if (sc->sc_flags & SC_OP_RXAGGR)
                                 ath_rx_aggr_teardown(sc, an, tidno);
                 }
         }
@@ -1815,13 +1662,6 @@ void ath_descdma_cleanup(struct ath_softc *sc,
 /* Utilities */
 /*************/
 
-void ath_internal_reset(struct ath_softc *sc)
-{
-        ath_reset_start(sc, 0);
-        ath_reset(sc);
-        ath_reset_end(sc, 0);
-}
-
 int ath_get_hal_qnum(u16 queue, struct ath_softc *sc)
 {
         int qnum;
diff --git a/drivers/net/wireless/ath9k/core.h b/drivers/net/wireless/ath9k/core.h
index 673b3d81133a..1faa1effa02c 100644
--- a/drivers/net/wireless/ath9k/core.h
+++ b/drivers/net/wireless/ath9k/core.h
@@ -39,6 +39,7 @@
 #include <linux/scatterlist.h>
 #include <asm/page.h>
 #include <net/mac80211.h>
+#include <linux/leds.h>
 
 #include "ath9k.h"
 #include "rc.h"
@@ -79,12 +80,12 @@ struct ath_node;
                 }                               \
         } while (0)
 
+#define TSF_TO_TU(_h,_l) \
+        ((((u32)(_h)) << 22) | (((u32)(_l)) >> 10))
+
 /* XXX: remove */
 #define memzero(_buf, _len) memset(_buf, 0, _len)
 
-#define get_dma_mem_context(var, field) (&((var)->field))
-#define copy_dma_mem_context(dst, src)  (*dst = *src)
-
 #define ATH9K_BH_STATUS_INTACT          0
 #define ATH9K_BH_STATUS_CHANGE          1
 
@@ -95,6 +96,8 @@ static inline unsigned long get_timestamp(void)
         return ((jiffies / HZ) * 1000) + (jiffies % HZ) * (1000 / HZ);
 }
 
+static const u8 ath_bcast_mac[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+
 /*************/
 /* Debugging */
 /*************/
@@ -175,11 +178,6 @@ void ath_update_chainmask(struct ath_softc *sc, int is_ht);
 /* Descriptor Management */
 /*************************/
 
-/* Number of descriptors per buffer. The only case where we see skbuff
-chains is due to FF aggregation in the driver. */
-#define ATH_TXDESC          1
-/* if there's more fragment for this MSDU */
-#define ATH_BF_MORE_MPDU    1
 #define ATH_TXBUF_RESET(_bf) do {                               \
                 (_bf)->bf_status = 0;                           \
                 (_bf)->bf_lastbf = NULL;                        \
@@ -189,28 +187,29 @@ chains is due to FF aggregation in the driver. */
                             sizeof(struct ath_buf_state));      \
         } while (0)
 
+enum buffer_type {
+        BUF_DATA                = BIT(0),
+        BUF_AGGR                = BIT(1),
+        BUF_AMPDU               = BIT(2),
+        BUF_HT                  = BIT(3),
+        BUF_RETRY               = BIT(4),
+        BUF_XRETRY              = BIT(5),
+        BUF_SHORT_PREAMBLE      = BIT(6),
+        BUF_BAR                 = BIT(7),
+        BUF_PSPOLL              = BIT(8),
+        BUF_AGGR_BURST          = BIT(9),
+        BUF_CALC_AIRTIME        = BIT(10),
+};
+
 struct ath_buf_state {
-        int bfs_nframes;        /* # frames in aggregate */
-        u16 bfs_al;             /* length of aggregate */
-        u16 bfs_frmlen;         /* length of frame */
-        int bfs_seqno;          /* sequence number */
-        int bfs_tidno;          /* tid of this frame */
-        int bfs_retries;        /* current retries */
+        int bfs_nframes;                        /* # frames in aggregate */
+        u16 bfs_al;                             /* length of aggregate */
+        u16 bfs_frmlen;                         /* length of frame */
+        int bfs_seqno;                          /* sequence number */
+        int bfs_tidno;                          /* tid of this frame */
+        int bfs_retries;                        /* current retries */
         struct ath_rc_series bfs_rcs[4];        /* rate series */
-        u8 bfs_isdata:1;        /* is a data frame/aggregate */
-        u8 bfs_isaggr:1;        /* is an aggregate */
-        u8 bfs_isampdu:1;       /* is an a-mpdu, aggregate or not */
-        u8 bfs_ht:1;            /* is an HT frame */
-        u8 bfs_isretried:1;     /* is retried */
-        u8 bfs_isxretried:1;    /* is excessive retried */
-        u8 bfs_shpreamble:1;    /* is short preamble */
-        u8 bfs_isbar:1;         /* is a BAR */
-        u8 bfs_ispspoll:1;      /* is a PS-Poll */
-        u8 bfs_aggrburst:1;     /* is a aggr burst */
-        u8 bfs_calcairtime:1;   /* requests airtime be calculated
-                                when set for tx frame */
-        int bfs_rifsburst_elem; /* RIFS burst/bar */
-        int bfs_nrifsubframes;  /* # of elements in burst */
+        u32 bf_type;                            /* BUF_* (enum buffer_type) */
         /* key type use to encrypt this frame */
         enum ath9k_key_type bfs_keytype;
 };
@@ -222,26 +221,22 @@ struct ath_buf_state {
 #define bf_seqno                bf_state.bfs_seqno
 #define bf_tidno                bf_state.bfs_tidno
 #define bf_rcs                  bf_state.bfs_rcs
-#define bf_isdata               bf_state.bfs_isdata
-#define bf_isaggr               bf_state.bfs_isaggr
-#define bf_isampdu              bf_state.bfs_isampdu
-#define bf_ht                   bf_state.bfs_ht
-#define bf_isretried            bf_state.bfs_isretried
-#define bf_isxretried           bf_state.bfs_isxretried
-#define bf_shpreamble           bf_state.bfs_shpreamble
-#define bf_rifsburst_elem       bf_state.bfs_rifsburst_elem
-#define bf_nrifsubframes        bf_state.bfs_nrifsubframes
 #define bf_keytype              bf_state.bfs_keytype
-#define bf_isbar                bf_state.bfs_isbar
-#define bf_ispspoll             bf_state.bfs_ispspoll
-#define bf_aggrburst            bf_state.bfs_aggrburst
-#define bf_calcairtime          bf_state.bfs_calcairtime
+#define bf_isdata(bf)           (bf->bf_state.bf_type & BUF_DATA)
+#define bf_isaggr(bf)           (bf->bf_state.bf_type & BUF_AGGR)
+#define bf_isampdu(bf)          (bf->bf_state.bf_type & BUF_AMPDU)
+#define bf_isht(bf)             (bf->bf_state.bf_type & BUF_HT)
+#define bf_isretried(bf)        (bf->bf_state.bf_type & BUF_RETRY)
+#define bf_isxretried(bf)       (bf->bf_state.bf_type & BUF_XRETRY)
+#define bf_isshpreamble(bf)     (bf->bf_state.bf_type & BUF_SHORT_PREAMBLE)
+#define bf_isbar(bf)            (bf->bf_state.bf_type & BUF_BAR)
+#define bf_ispspoll(bf)         (bf->bf_state.bf_type & BUF_PSPOLL)
+#define bf_isaggrburst(bf)      (bf->bf_state.bf_type & BUF_AGGR_BURST)
 
 /*
  * Abstraction of a contiguous buffer to transmit/receive.  There is only
  * a single hw descriptor encapsulated here.
  */
-
 struct ath_buf {
         struct list_head list;
         struct list_head *last;
@@ -391,10 +386,10 @@ int ath_rx_input(struct ath_softc *sc,
                  struct sk_buff *skb,
                  struct ath_recv_status *rx_status,
                  enum ATH_RX_TYPE *status);
-int ath__rx_indicate(struct ath_softc *sc,
-                    struct sk_buff *skb,
-                    struct ath_recv_status *status,
-                    u16 keyix);
+int _ath_rx_indicate(struct ath_softc *sc,
+                     struct sk_buff *skb,
+                     struct ath_recv_status *status,
+                     u16 keyix);
 int ath_rx_subframe(struct ath_node *an, struct sk_buff *skb,
                     struct ath_recv_status *status);
 
@@ -402,8 +397,7 @@ int ath_rx_subframe(struct ath_node *an, struct sk_buff *skb,
 /* TX */
 /******/
 
-#define ATH_FRAG_PER_MSDU       1
-#define ATH_TXBUF               (512/ATH_FRAG_PER_MSDU)
+#define ATH_TXBUF               512
 /* max number of transmit attempts (tries) */
 #define ATH_TXMAXTRY            13
 /* max number of 11n transmit attempts (tries) */
@@ -522,7 +516,6 @@ struct ath_tx_control {
         u32 keyix;
         int min_rate;
         int mcast_rate;
-        u16 nextfraglen;
         struct ath_softc *dev;
         dma_addr_t dmacontext;
 };
@@ -557,10 +550,10 @@ void ath_tx_cleanupq(struct ath_softc *sc, struct ath_txq *txq);
 int ath_tx_setup(struct ath_softc *sc, int haltype);
 void ath_draintxq(struct ath_softc *sc, bool retry_tx);
 void ath_tx_draintxq(struct ath_softc *sc,
-        struct ath_txq *txq, bool retry_tx);
+                     struct ath_txq *txq, bool retry_tx);
 void ath_tx_node_init(struct ath_softc *sc, struct ath_node *an);
 void ath_tx_node_cleanup(struct ath_softc *sc,
-        struct ath_node *an, bool bh_flag);
+                         struct ath_node *an, bool bh_flag);
 void ath_tx_node_free(struct ath_softc *sc, struct ath_node *an);
 void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq);
 int ath_tx_init(struct ath_softc *sc, int nbufs);
@@ -575,6 +568,7 @@ u32 ath_txq_aggr_depth(struct ath_softc *sc, int qnum);
 void ath_notify_txq_status(struct ath_softc *sc, u16 queue_depth);
 void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
                      struct ath_xmit_status *tx_status, struct ath_node *an);
+void ath_tx_cabq(struct ath_softc *sc, struct sk_buff *skb);
 
 /**********************/
 /* Node / Aggregation */
@@ -585,7 +579,6 @@ void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
 /* indicates the node is 80211 power save */
 #define ATH_NODE_PWRSAVE        0x2
 
-#define ADDBA_TIMEOUT              200 /* 200 milliseconds */
 #define ADDBA_EXCHANGE_ATTEMPTS    10
 #define ATH_AGGR_DELIM_SZ          4   /* delimiter size   */
 #define ATH_AGGR_MINPLEN           256 /* in bytes, minimum packet length */
@@ -705,9 +698,6 @@ struct ath_node *ath_node_find(struct ath_softc *sc, u8 *addr);
 #define ATH_BCBUF                       4   /* number of beacon buffers */
 #define ATH_DEFAULT_BINTVAL             100 /* default beacon interval in TU */
 #define ATH_DEFAULT_BMISS_LIMIT         10
-#define ATH_BEACON_AIFS_DEFAULT         0  /* Default aifs for ap beacon q */
-#define ATH_BEACON_CWMIN_DEFAULT        0  /* Default cwmin for ap beacon q */
-#define ATH_BEACON_CWMAX_DEFAULT        0  /* Default cwmax for ap beacon q */
 #define IEEE80211_MS_TO_TU(x)           (((x) * 1000) / 1024)
 
 /* beacon configuration */
@@ -724,30 +714,16 @@ struct ath_beacon_config {
         } u; /* last received beacon/probe response timestamp of this BSS. */
 };
 
-/* offsets in a beacon frame for
- * quick acess of beacon content by low-level driver */
-struct ath_beacon_offset {
-        u8 *bo_tim;     /* start of atim/dtim */
-};
-
 void ath9k_beacon_tasklet(unsigned long data);
 void ath_beacon_config(struct ath_softc *sc, int if_id);
 int ath_beaconq_setup(struct ath_hal *ah);
 int ath_beacon_alloc(struct ath_softc *sc, int if_id);
 void ath_bstuck_process(struct ath_softc *sc);
-void ath_beacon_tasklet(struct ath_softc *sc, int *needmark);
-void ath_beacon_free(struct ath_softc *sc);
 void ath_beacon_return(struct ath_softc *sc, struct ath_vap *avp);
 void ath_beacon_sync(struct ath_softc *sc, int if_id);
-void ath_update_beacon_info(struct ath_softc *sc, int avgbrssi);
 void ath_get_beaconconfig(struct ath_softc *sc,
                           int if_id,
                           struct ath_beacon_config *conf);
-int ath_update_beacon(struct ath_softc *sc,
-                      int if_id,
-                      struct ath_beacon_offset *bo,
-                      struct sk_buff *skb,
-                      int mcast);
 /********/
 /* VAPs */
 /********/
@@ -774,10 +750,8 @@ struct ath_vap {
         struct ieee80211_vif *av_if_data;
         enum ath9k_opmode av_opmode;    /* VAP operational mode */
         struct ath_buf *av_bcbuf;       /* beacon buffer */
-        struct ath_beacon_offset av_boff; /* dynamic update state */
         struct ath_tx_control av_btxctl;  /* txctl information for beacon */
         int av_bslot;                   /* beacon slot index */
-        struct ath_txq av_mcastq;       /* multicast transmit queue */
         struct ath_vap_config av_config;/* vap configuration parameters*/
         struct ath_rate_node *rc_node;
 };
@@ -788,8 +762,7 @@ int ath_vap_attach(struct ath_softc *sc,
                    enum ath9k_opmode opmode);
 int ath_vap_detach(struct ath_softc *sc, int if_id);
 int ath_vap_config(struct ath_softc *sc,
-        int if_id, struct ath_vap_config *if_config);
-int ath_vap_listen(struct ath_softc *sc, int if_id);
+                   int if_id, struct ath_vap_config *if_config);
 
 /*********************/
 /* Antenna diversity */
@@ -830,6 +803,27 @@ void ath_slow_ant_div(struct ath_antdiv *antdiv,
 void ath_setdefantenna(void *sc, u32 antenna);
 
 /********************/
+/*   LED Control    */
+/********************/
+
+#define ATH_LED_PIN     1
+
+enum ath_led_type {
+        ATH_LED_RADIO,
+        ATH_LED_ASSOC,
+        ATH_LED_TX,
+        ATH_LED_RX
+};
+
+struct ath_led {
+        struct ath_softc *sc;
+        struct led_classdev led_cdev;
+        enum ath_led_type led_type;
+        char name[32];
+        bool registered;
+};
+
+/********************/
 /* Main driver core */
 /********************/
 
@@ -841,11 +835,7 @@ void ath_setdefantenna(void *sc, u32 antenna);
 #define ATH_DEFAULT_NOISE_FLOOR -95
 #define ATH_REGCLASSIDS_MAX     10
 #define ATH_CABQ_READY_TIME     80  /* % of beacon interval */
-#define ATH_PREAMBLE_SHORT      (1<<0)
-#define ATH_PROTECT_ENABLE      (1<<1)
 #define ATH_MAX_SW_RETRIES      10
-/* Num farmes difference in tx to flip default recv */
-#define ATH_ANTENNA_DIFF        2
 #define ATH_CHAN_MAX            255
 #define IEEE80211_WEP_NKID      4       /* number of key ids */
 #define IEEE80211_RATE_VAL      0x7f
@@ -859,9 +849,7 @@ void ath_setdefantenna(void *sc, u32 antenna);
  */
 #define ATH_KEYMAX              128        /* max key cache size we handle */
 
-#define RESET_RETRY_TXQ         0x00000001
 #define ATH_IF_ID_ANY           0xff
-
 #define ATH_TXPOWER_MAX         100     /* .5 dBm units */
 
 #define RSSI_LPF_THRESHOLD         -20
@@ -907,60 +895,61 @@ struct ath_ht_info {
         u8 ext_chan_offset;
 };
 
+#define SC_OP_INVALID           BIT(0)
+#define SC_OP_BEACONS           BIT(1)
+#define SC_OP_RXAGGR            BIT(2)
+#define SC_OP_TXAGGR            BIT(3)
+#define SC_OP_CHAINMASK_UPDATE  BIT(4)
+#define SC_OP_FULL_RESET        BIT(5)
+#define SC_OP_NO_RESET          BIT(6)
+#define SC_OP_PREAMBLE_SHORT    BIT(7)
+#define SC_OP_PROTECT_ENABLE    BIT(8)
+#define SC_OP_RXFLUSH           BIT(9)
+#define SC_OP_LED_ASSOCIATED    BIT(10)
+
 struct ath_softc {
         struct ieee80211_hw *hw;
         struct pci_dev *pdev;
-        void __iomem *mem;
         struct tasklet_struct intr_tq;
         struct tasklet_struct bcon_tasklet;
-        struct ath_config sc_config;    /* load-time parameters */
-        int sc_debug;
+        struct ath_config sc_config;
         struct ath_hal *sc_ah;
-        struct ath_rate_softc *sc_rc;   /* tx rate control support */
+        struct ath_rate_softc *sc_rc;
+        void __iomem *mem;
+
+        u8 sc_curbssid[ETH_ALEN];
+        u8 sc_myaddr[ETH_ALEN];
+        u8 sc_bssidmask[ETH_ALEN];
+
+        int sc_debug;
         u32 sc_intrstatus;
-        enum ath9k_opmode sc_opmode;    /* current operating mode */
-
-        u8 sc_invalid;                  /* being detached */
-        u8 sc_beacons;                  /* beacons running */
-        u8 sc_scanning;                 /* scanning active */
-        u8 sc_txaggr;                   /* enable 11n tx aggregation */
-        u8 sc_rxaggr;                   /* enable 11n rx aggregation */
-        u8 sc_update_chainmask;         /* change chain mask */
-        u8 sc_full_reset;               /* force full reset */
-        enum wireless_mode sc_curmode;  /* current phy mode */
+        u32 sc_flags; /* SC_OP_* */
+        unsigned int rx_filter;
         u16 sc_curtxpow;
         u16 sc_curaid;
-        u8 sc_curbssid[ETH_ALEN];
-        u8 sc_myaddr[ETH_ALEN];
+        u16 sc_cachelsz;
+        int sc_slotupdate;              /* slot to next advance fsm */
+        int sc_slottime;
+        int sc_bslot[ATH_BCBUF];
+        u8 sc_tx_chainmask;
+        u8 sc_rx_chainmask;
+        enum ath9k_int sc_imask;
+        enum wireless_mode sc_curmode;  /* current phy mode */
         enum PROT_MODE sc_protmode;
-        u8 sc_mcastantenna;
-        u8 sc_txantenna;                /* data tx antenna (fixed or auto) */
+
         u8 sc_nbcnvaps;                 /* # of vaps sending beacons */
         u16 sc_nvaps;                   /* # of active virtual ap's */
         struct ath_vap *sc_vaps[ATH_BCBUF];
-        enum ath9k_int sc_imask;
-        u8 sc_bssidmask[ETH_ALEN];
+
+        u8 sc_mcastantenna;
         u8 sc_defant;                   /* current default antenna */
         u8 sc_rxotherant;               /* rx's on non-default antenna */
-        u16 sc_cachelsz;
-        int sc_slotupdate;              /* slot to next advance fsm */
-        int sc_slottime;
-        u8 sc_noreset;
-        int sc_bslot[ATH_BCBUF];
+
         struct ath9k_node_stats sc_halstats; /* station-mode rssi stats */
         struct list_head node_list;
         struct ath_ht_info sc_ht_info;
-        int16_t sc_noise_floor;         /* signal noise floor in dBm */
         enum ath9k_ht_extprotspacing sc_ht_extprotspacing;
-        u8 sc_tx_chainmask;
-        u8 sc_rx_chainmask;
-        u8 sc_rxchaindetect_ref;
-        u8 sc_rxchaindetect_thresh5GHz;
-        u8 sc_rxchaindetect_thresh2GHz;
-        u8 sc_rxchaindetect_delta5GHz;
-        u8 sc_rxchaindetect_delta2GHz;
-        u32 sc_rtsaggrlimit;            /* Chipset specific aggr limit */
-        u32 sc_flags;
+
 #ifdef CONFIG_SLOW_ANT_DIV
         struct ath_antdiv sc_antdiv;
 #endif
@@ -981,8 +970,6 @@ struct ath_softc {
         struct ath_descdma sc_rxdma;
         int sc_rxbufsize;       /* rx size based on mtu */
         u32 *sc_rxlink;         /* link ptr in last RX desc */
-        u32 sc_rxflush;         /* rx flush in progress */
-        u64 sc_lastrx;          /* tsf of last rx'd frame */
 
         /* TX */
         struct list_head sc_txbuf;
@@ -991,7 +978,7 @@ struct ath_softc {
         u32 sc_txqsetup;
         u32 sc_txintrperiod;    /* tx interrupt batching */
         int sc_haltype2q[ATH9K_WME_AC_VO+1]; /* HAL WME AC -> h/w qnum */
-        u32 sc_ant_tx[8];       /* recent tx frames/antenna */
+        u16 seq_no; /* TX sequence number */
 
         /* Beacon */
         struct ath9k_tx_queue_info sc_beacon_qi;
@@ -1015,7 +1002,6 @@ struct ath_softc {
         /* Channel, Band */
         struct ieee80211_channel channels[IEEE80211_NUM_BANDS][ATH_CHAN_MAX];
         struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
-        struct ath9k_channel sc_curchan;
 
         /* Locks */
         spinlock_t sc_rxflushlock;
@@ -1023,6 +1009,12 @@ struct ath_softc {
         spinlock_t sc_txbuflock;
         spinlock_t sc_resetlock;
         spinlock_t node_lock;
+
+        /* LEDs */
+        struct ath_led radio_led;
+        struct ath_led assoc_led;
+        struct ath_led tx_led;
+        struct ath_led rx_led;
 };
 
 int ath_init(u16 devid, struct ath_softc *sc);
@@ -1030,14 +1022,8 @@ void ath_deinit(struct ath_softc *sc);
 int ath_open(struct ath_softc *sc, struct ath9k_channel *initial_chan);
 int ath_suspend(struct ath_softc *sc);
 irqreturn_t ath_isr(int irq, void *dev);
-int ath_reset(struct ath_softc *sc);
-void ath_scan_start(struct ath_softc *sc);
-void ath_scan_end(struct ath_softc *sc);
+int ath_reset(struct ath_softc *sc, bool retry_tx);
 int ath_set_channel(struct ath_softc *sc, struct ath9k_channel *hchan);
-void ath_setup_rate(struct ath_softc *sc,
-                    enum wireless_mode wMode,
-                    enum RATE_TYPE type,
-                    const struct ath9k_rate_table *rt);
 
 /*********************/
 /* Utility Functions */
@@ -1056,17 +1042,5 @@ int ath_cabq_update(struct ath_softc *);
 void ath_get_currentCountry(struct ath_softc *sc,
         struct ath9k_country_entry *ctry);
 u64 ath_extend_tsf(struct ath_softc *sc, u32 rstamp);
-void ath_internal_reset(struct ath_softc *sc);
-u32 ath_chan2flags(struct ieee80211_channel *chan, struct ath_softc *sc);
-dma_addr_t ath_skb_map_single(struct ath_softc *sc,
-                              struct sk_buff *skb,
-                              int direction,
-                              dma_addr_t *pa);
-void ath_skb_unmap_single(struct ath_softc *sc,
-                          struct sk_buff *skb,
-                          int direction,
-                          dma_addr_t *pa);
-void ath_mcast_merge(struct ath_softc *sc, u32 mfilt[2]);
-enum ath9k_ht_macmode ath_cwm_macmode(struct ath_softc *sc);
 
 #endif /* CORE_H */
diff --git a/drivers/net/wireless/ath9k/hw.c b/drivers/net/wireless/ath9k/hw.c
index a17eb130f574..2578411c6019 100644
--- a/drivers/net/wireless/ath9k/hw.c
+++ b/drivers/net/wireless/ath9k/hw.c
@@ -85,29 +85,6 @@ static const struct hal_percal_data adc_init_dc_cal = {
         ath9k_hw_adc_dccal_calibrate
 };
 
-static const struct ath_hal ar5416hal = {
-        AR5416_MAGIC,
-        0,
-        0,
-        NULL,
-        NULL,
-        CTRY_DEFAULT,
-        0,
-        0,
-        0,
-        0,
-        0,
-        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-         0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
-        },
-};
-
 static struct ath9k_rate_table ar5416_11a_table = {
         8,
         {0},
@@ -371,7 +348,7 @@ static void ath9k_hw_set_defaults(struct ath_hal *ah)
         ah->ah_config.intr_mitigation = 0;
 }
 
-static inline void ath9k_hw_override_ini(struct ath_hal *ah,
+static void ath9k_hw_override_ini(struct ath_hal *ah,
                                          struct ath9k_channel *chan)
 {
         if (!AR_SREV_5416_V20_OR_LATER(ah)
@@ -381,8 +358,8 @@ static inline void ath9k_hw_override_ini(struct ath_hal *ah,
         REG_WRITE(ah, 0x9800 + (651 << 2), 0x11);
 }
 
-static inline void ath9k_hw_init_bb(struct ath_hal *ah,
-                                    struct ath9k_channel *chan)
+static void ath9k_hw_init_bb(struct ath_hal *ah,
+                             struct ath9k_channel *chan)
 {
         u32 synthDelay;
 
@@ -397,8 +374,8 @@ static inline void ath9k_hw_init_bb(struct ath_hal *ah,
         udelay(synthDelay + BASE_ACTIVATE_DELAY);
 }
 
-static inline void ath9k_hw_init_interrupt_masks(struct ath_hal *ah,
-                                                 enum ath9k_opmode opmode)
+static void ath9k_hw_init_interrupt_masks(struct ath_hal *ah,
+                                          enum ath9k_opmode opmode)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
 
@@ -428,7 +405,7 @@ static inline void ath9k_hw_init_interrupt_masks(struct ath_hal *ah,
         }
 }
 
-static inline void ath9k_hw_init_qos(struct ath_hal *ah)
+static void ath9k_hw_init_qos(struct ath_hal *ah)
 {
         REG_WRITE(ah, AR_MIC_QOS_CONTROL, 0x100aa);
         REG_WRITE(ah, AR_MIC_QOS_SELECT, 0x3210);
@@ -523,7 +500,7 @@ static inline bool ath9k_hw_nvram_read(struct ath_hal *ah,
                 return ath9k_hw_eeprom_read(ah, off, data);
 }
 
-static inline bool ath9k_hw_fill_eeprom(struct ath_hal *ah)
+static bool ath9k_hw_fill_eeprom(struct ath_hal *ah)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
         struct ar5416_eeprom *eep = &ahp->ah_eeprom;
@@ -790,7 +767,7 @@ ath9k_hw_eeprom_set_board_values(struct ath_hal *ah,
         return true;
 }
 
-static inline int ath9k_hw_check_eeprom(struct ath_hal *ah)
+static int ath9k_hw_check_eeprom(struct ath_hal *ah)
 {
         u32 sum = 0, el;
         u16 *eepdata;
@@ -1196,11 +1173,12 @@ static struct ath_hal_5416 *ath9k_hw_newstate(u16 devid,
 
         ah = &ahp->ah;
 
-        memcpy(&ahp->ah, &ar5416hal, sizeof(struct ath_hal));
-
         ah->ah_sc = sc;
         ah->ah_sh = mem;
 
+        ah->ah_magic = AR5416_MAGIC;
+        ah->ah_countryCode = CTRY_DEFAULT;
+
         ah->ah_devid = devid;
         ah->ah_subvendorid = 0;
 
@@ -1294,7 +1272,7 @@ u32 ath9k_hw_get_eeprom(struct ath_hal_5416 *ahp,
         }
 }
 
-static inline int ath9k_hw_get_radiorev(struct ath_hal *ah)
+static int ath9k_hw_get_radiorev(struct ath_hal *ah)
 {
         u32 val;
         int i;
@@ -1307,7 +1285,7 @@ static inline int ath9k_hw_get_radiorev(struct ath_hal *ah)
         return ath9k_hw_reverse_bits(val, 8);
 }
 
-static inline int ath9k_hw_init_macaddr(struct ath_hal *ah)
+static int ath9k_hw_init_macaddr(struct ath_hal *ah)
 {
         u32 sum;
         int i;
@@ -1389,7 +1367,7 @@ static u16 ath9k_hw_eeprom_get_spur_chan(struct ath_hal *ah,
         return spur_val;
 }
 
-static inline int ath9k_hw_rfattach(struct ath_hal *ah)
+static int ath9k_hw_rfattach(struct ath_hal *ah)
 {
         bool rfStatus = false;
         int ecode = 0;
@@ -1434,8 +1412,8 @@ static int ath9k_hw_rf_claim(struct ath_hal *ah)
         return 0;
 }
 
-static inline void ath9k_hw_init_pll(struct ath_hal *ah,
-                                     struct ath9k_channel *chan)
+static void ath9k_hw_init_pll(struct ath_hal *ah,
+                              struct ath9k_channel *chan)
 {
         u32 pll;
 
@@ -1553,7 +1531,7 @@ static void ath9k_hw_set_operating_mode(struct ath_hal *ah, int opmode)
         }
 }
 
-static inline void
+static void
 ath9k_hw_set_rfmode(struct ath_hal *ah, struct ath9k_channel *chan)
 {
         u32 rfMode = 0;
@@ -1623,7 +1601,7 @@ static bool ath9k_hw_set_reset(struct ath_hal *ah, int type)
         return true;
 }
 
-static inline bool ath9k_hw_set_reset_power_on(struct ath_hal *ah)
+static bool ath9k_hw_set_reset_power_on(struct ath_hal *ah)
 {
         REG_WRITE(ah, AR_RTC_FORCE_WAKE, AR_RTC_FORCE_WAKE_EN |
                   AR_RTC_FORCE_WAKE_ON_INT);
@@ -1664,7 +1642,7 @@ static bool ath9k_hw_set_reset_reg(struct ath_hal *ah,
         }
 }
 
-static inline
+static
 struct ath9k_channel *ath9k_hw_check_chan(struct ath_hal *ah,
                                           struct ath9k_channel *chan)
 {
@@ -2098,7 +2076,7 @@ static void ath9k_hw_ani_attach(struct ath_hal *ah)
                 ahp->ah_procPhyErr |= HAL_PROCESS_ANI;
 }
 
-static inline void ath9k_hw_ani_setup(struct ath_hal *ah)
+static void ath9k_hw_ani_setup(struct ath_hal *ah)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
         int i;
@@ -2822,32 +2800,11 @@ static void ath9k_hw_gpio_cfg_output_mux(struct ath_hal *ah,
         }
 }
 
-static bool ath9k_hw_cfg_output(struct ath_hal *ah, u32 gpio,
-                                enum ath9k_gpio_output_mux_type
-                                halSignalType)
+void ath9k_hw_cfg_output(struct ath_hal *ah, u32 gpio,
+                         u32 ah_signal_type)
 {
-        u32 ah_signal_type;
         u32 gpio_shift;
 
-        static u32 MuxSignalConversionTable[] = {
-
-                AR_GPIO_OUTPUT_MUX_AS_OUTPUT,
-
-                AR_GPIO_OUTPUT_MUX_AS_PCIE_ATTENTION_LED,
-
-                AR_GPIO_OUTPUT_MUX_AS_PCIE_POWER_LED,
-
-                AR_GPIO_OUTPUT_MUX_AS_MAC_NETWORK_LED,
-
-                AR_GPIO_OUTPUT_MUX_AS_MAC_POWER_LED,
-        };
-
-        if ((halSignalType >= 0)
-            && (halSignalType < ARRAY_SIZE(MuxSignalConversionTable)))
-                ah_signal_type = MuxSignalConversionTable[halSignalType];
-        else
-                return false;
-
         ath9k_hw_gpio_cfg_output_mux(ah, gpio, ah_signal_type);
 
         gpio_shift = 2 * gpio;
@@ -2856,16 +2813,12 @@ static bool ath9k_hw_cfg_output(struct ath_hal *ah, u32 gpio,
                 AR_GPIO_OE_OUT,
                 (AR_GPIO_OE_OUT_DRV_ALL << gpio_shift),
                 (AR_GPIO_OE_OUT_DRV << gpio_shift));
-
-        return true;
 }
 
-static bool ath9k_hw_set_gpio(struct ath_hal *ah, u32 gpio,
-                              u32 val)
+void ath9k_hw_set_gpio(struct ath_hal *ah, u32 gpio, u32 val)
 {
         REG_RMW(ah, AR_GPIO_IN_OUT, ((val & 1) << gpio),
                 AR_GPIO_BIT(gpio));
-        return true;
 }
 
 static u32 ath9k_hw_gpio_get(struct ath_hal *ah, u32 gpio)
@@ -2883,7 +2836,7 @@ static u32 ath9k_hw_gpio_get(struct ath_hal *ah, u32 gpio)
         }
 }
 
-static inline int ath9k_hw_post_attach(struct ath_hal *ah)
+static int ath9k_hw_post_attach(struct ath_hal *ah)
 {
         int ecode;
 
@@ -3595,7 +3548,7 @@ static inline bool ath9k_hw_fill_vpd_table(u8 pwrMin,
         return true;
 }
 
-static inline void
+static void
 ath9k_hw_get_gain_boundaries_pdadcs(struct ath_hal *ah,
                                     struct ath9k_channel *chan,
                                     struct cal_data_per_freq *pRawDataSet,
@@ -3777,7 +3730,7 @@ ath9k_hw_get_gain_boundaries_pdadcs(struct ath_hal *ah,
         return;
 }
 
-static inline bool
+static bool
 ath9k_hw_set_power_cal_table(struct ath_hal *ah,
                              struct ar5416_eeprom *pEepData,
                              struct ath9k_channel *chan,
@@ -3980,7 +3933,7 @@ void ath9k_hw_configpcipowersave(struct ath_hal *ah, int restore)
         }
 }
 
-static inline void
+static void
 ath9k_hw_get_legacy_target_powers(struct ath_hal *ah,
                                   struct ath9k_channel *chan,
                                   struct cal_target_power_leg *powInfo,
@@ -4046,7 +3999,7 @@ ath9k_hw_get_legacy_target_powers(struct ath_hal *ah,
         }
 }
 
-static inline void
+static void
 ath9k_hw_get_target_powers(struct ath_hal *ah,
                            struct ath9k_channel *chan,
                            struct cal_target_power_ht *powInfo,
@@ -4113,7 +4066,7 @@ ath9k_hw_get_target_powers(struct ath_hal *ah,
         }
 }
 
-static inline u16
+static u16
 ath9k_hw_get_max_edge_power(u16 freq,
                             struct cal_ctl_edges *pRdEdgesPower,
                             bool is2GHz)
@@ -4143,7 +4096,7 @@ ath9k_hw_get_max_edge_power(u16 freq,
         return twiceMaxEdgePower;
 }
 
-static inline bool
+static bool
 ath9k_hw_set_power_per_rate_table(struct ath_hal *ah,
                                   struct ar5416_eeprom *pEepData,
                                   struct ath9k_channel *chan,
@@ -5122,7 +5075,7 @@ static void ath9k_hw_spur_mitigate(struct ath_hal *ah,
         REG_WRITE(ah, AR_PHY_MASK2_P_61_45, tmp_mask);
 }
 
-static inline void ath9k_hw_init_chain_masks(struct ath_hal *ah)
+static void ath9k_hw_init_chain_masks(struct ath_hal *ah)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
         int rx_chainmask, tx_chainmask;
@@ -5326,7 +5279,7 @@ bool ath9k_hw_setslottime(struct ath_hal *ah, u32 us)
         }
 }
 
-static inline void ath9k_hw_init_user_settings(struct ath_hal *ah)
+static void ath9k_hw_init_user_settings(struct ath_hal *ah)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
 
@@ -5345,7 +5298,7 @@ static inline void ath9k_hw_init_user_settings(struct ath_hal *ah)
                 ath9k_hw_set_global_txtimeout(ah, ahp->ah_globaltxtimeout);
 }
 
-static inline int
+static int
 ath9k_hw_process_ini(struct ath_hal *ah,
                      struct ath9k_channel *chan,
                      enum ath9k_ht_macmode macmode)
@@ -5476,7 +5429,7 @@ ath9k_hw_process_ini(struct ath_hal *ah,
         return 0;
 }
 
-static inline void ath9k_hw_setup_calibration(struct ath_hal *ah,
+static void ath9k_hw_setup_calibration(struct ath_hal *ah,
                                               struct hal_cal_list *currCal)
 {
         REG_RMW_FIELD(ah, AR_PHY_TIMING_CTRL4(0),
@@ -5512,8 +5465,8 @@ static inline void ath9k_hw_setup_calibration(struct ath_hal *ah,
                     AR_PHY_TIMING_CTRL4_DO_CAL);
 }
 
-static inline void ath9k_hw_reset_calibration(struct ath_hal *ah,
-                                              struct hal_cal_list *currCal)
+static void ath9k_hw_reset_calibration(struct ath_hal *ah,
+                                       struct hal_cal_list *currCal)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
         int i;
@@ -5532,7 +5485,7 @@ static inline void ath9k_hw_reset_calibration(struct ath_hal *ah,
         ahp->ah_CalSamples = 0;
 }
 
-static inline void
+static void
 ath9k_hw_per_calibration(struct ath_hal *ah,
                          struct ath9k_channel *ichan,
                          u8 rxchainmask,
@@ -5622,7 +5575,7 @@ static inline bool ath9k_hw_run_init_cals(struct ath_hal *ah,
         return true;
 }
 
-static inline bool
+static bool
 ath9k_hw_channel_change(struct ath_hal *ah,
                         struct ath9k_channel *chan,
                         enum ath9k_ht_macmode macmode)
@@ -5799,8 +5752,8 @@ static bool ath9k_hw_iscal_supported(struct ath_hal *ah,
         return retval;
 }
 
-static inline bool ath9k_hw_init_cal(struct ath_hal *ah,
-                                     struct ath9k_channel *chan)
+static bool ath9k_hw_init_cal(struct ath_hal *ah,
+                              struct ath9k_channel *chan)
 {
         struct ath_hal_5416 *ahp = AH5416(ah);
         struct ath9k_channel *ichan =
@@ -5861,7 +5814,7 @@ static inline bool ath9k_hw_init_cal(struct ath_hal *ah,
 }
 
 
-bool ath9k_hw_reset(struct ath_hal *ah, enum ath9k_opmode opmode,
+bool ath9k_hw_reset(struct ath_hal *ah,
                     struct ath9k_channel *chan,
                     enum ath9k_ht_macmode macmode,
                     u8 txchainmask, u8 rxchainmask,
@@ -5945,7 +5898,7 @@ bool ath9k_hw_reset(struct ath_hal *ah, enum ath9k_opmode opmode,
                         else
                                 ath9k_hw_set_gpio(ah, 9, 1);
                 }
-                ath9k_hw_cfg_output(ah, 9, ATH9K_GPIO_OUTPUT_MUX_AS_OUTPUT);
+                ath9k_hw_cfg_output(ah, 9, AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
         }
 
         ecode = ath9k_hw_process_ini(ah, chan, macmode);
@@ -5975,7 +5928,7 @@ bool ath9k_hw_reset(struct ath_hal *ah, enum ath9k_opmode opmode,
                   | (ah->ah_config.
                      ack_6mb ? AR_STA_ID1_ACKCTS_6MB : 0)
                   | ahp->ah_staId1Defaults);
-        ath9k_hw_set_operating_mode(ah, opmode);
+        ath9k_hw_set_operating_mode(ah, ah->ah_opmode);
 
         REG_WRITE(ah, AR_BSSMSKL, get_unaligned_le32(ahp->ah_bssidmask));
         REG_WRITE(ah, AR_BSSMSKU, get_unaligned_le16(ahp->ah_bssidmask + 4));
@@ -6005,13 +5958,11 @@ bool ath9k_hw_reset(struct ath_hal *ah, enum ath9k_opmode opmode,
         for (i = 0; i < ah->ah_caps.total_queues; i++)
                 ath9k_hw_resettxqueue(ah, i);
 
-        ath9k_hw_init_interrupt_masks(ah, opmode);
+        ath9k_hw_init_interrupt_masks(ah, ah->ah_opmode);
         ath9k_hw_init_qos(ah);
 
         ath9k_hw_init_user_settings(ah);
 
-        ah->ah_opmode = opmode;
-
         REG_WRITE(ah, AR_STA_ID1,
                   REG_READ(ah, AR_STA_ID1) | AR_STA_ID1_PRESERVE_SEQNUM);
 
@@ -7678,8 +7629,7 @@ bool ath9k_hw_resettxqueue(struct ath_hal *ah, u32 q)
         REG_WRITE(ah, AR_DRETRY_LIMIT(q),
                   SM(INIT_SSH_RETRY, AR_D_RETRY_LIMIT_STA_SH)
                   | SM(INIT_SLG_RETRY, AR_D_RETRY_LIMIT_STA_LG)
-                  | SM(qi->tqi_shretry, AR_D_RETRY_LIMIT_FR_SH)
-                );
+                  | SM(qi->tqi_shretry, AR_D_RETRY_LIMIT_FR_SH));
 
         REG_WRITE(ah, AR_QMISC(q), AR_Q_MISC_DCU_EARLY_TERM_REQ);
         REG_WRITE(ah, AR_DMISC(q),
@@ -8324,15 +8274,7 @@ struct ath_hal *ath9k_hw_attach(u16 devid,
                 *error = -ENXIO;
                 break;
         }
-        if (ah != NULL) {
-                ah->ah_devid = ah->ah_devid;
-                ah->ah_subvendorid = ah->ah_subvendorid;
-                ah->ah_macVersion = ah->ah_macVersion;
-                ah->ah_macRev = ah->ah_macRev;
-                ah->ah_phyRev = ah->ah_phyRev;
-                ah->ah_analog5GhzRev = ah->ah_analog5GhzRev;
-                ah->ah_analog2GhzRev = ah->ah_analog2GhzRev;
-        }
+
         return ah;
 }
 
diff --git a/drivers/net/wireless/ath9k/hw.h b/drivers/net/wireless/ath9k/hw.h
index ae680f21ba7e..2113818ee934 100644
--- a/drivers/net/wireless/ath9k/hw.h
+++ b/drivers/net/wireless/ath9k/hw.h
@@ -314,14 +314,11 @@ struct ar5416_desc {
 #define RXSTATUS_RATE(ah, ads)  (AR_SREV_5416_V20_OR_LATER(ah) ?        \
                                  MS(ads->ds_rxstatus0, AR_RxRate) :     \
                                  (ads->ds_rxstatus3 >> 2) & 0xFF)
-#define RXSTATUS_DUPLICATE(ah, ads)  (AR_SREV_5416_V20_OR_LATER(ah) ?   \
-                                      MS(ads->ds_rxstatus3, AR_Parallel40) : \
-                                      (ads->ds_rxstatus3 >> 10) & 0x1)
 
-#define set11nTries(_series, _index)                            \
+#define set11nTries(_series, _index) \
         (SM((_series)[_index].Tries, AR_XmitDataTries##_index))
 
-#define set11nRate(_series, _index)                             \
+#define set11nRate(_series, _index) \
         (SM((_series)[_index].Rate, AR_XmitRate##_index))
 
 #define set11nPktDurRTSCTS(_series, _index)                             \
@@ -330,11 +327,11 @@ struct ar5416_desc {
                 AR_RTSCTSQual##_index : 0))
 
 #define set11nRateFlags(_series, _index)                                \
-        (((_series)[_index].RateFlags & ATH9K_RATESERIES_2040 ? \
-                AR_2040_##_index : 0) \
-        |((_series)[_index].RateFlags & ATH9K_RATESERIES_HALFGI ? \
-                AR_GI##_index : 0) \
-        |SM((_series)[_index].ChSel, AR_ChainSel##_index))
+        (((_series)[_index].RateFlags & ATH9K_RATESERIES_2040 ?         \
+          AR_2040_##_index : 0)                                         \
+         |((_series)[_index].RateFlags & ATH9K_RATESERIES_HALFGI ?      \
+           AR_GI##_index : 0)                                           \
+         |SM((_series)[_index].ChSel, AR_ChainSel##_index))
 
 #define AR_SREV_9100(ah) ((ah->ah_macVersion) == AR_SREV_VERSION_9100)
 
@@ -346,9 +343,6 @@ struct ar5416_desc {
 #define MAX_TX_FIFO_THRESHOLD   ((4096 / 64) - 1)
 #define INIT_TX_FIFO_THRESHOLD  MIN_TX_FIFO_THRESHOLD
 
-#define NUM_CORNER_FIX_BITS_2133    7
-#define CCK_OFDM_GAIN_DELTA         15
-
 struct ar5416AniState {
         struct ath9k_channel c;
         u8 noiseImmunityLevel;
@@ -377,11 +371,8 @@ struct ar5416AniState {
 };
 
 #define HAL_PROCESS_ANI     0x00000001
-#define HAL_RADAR_EN        0x80000000
-#define HAL_AR_EN           0x40000000
-
 #define DO_ANI(ah) \
-    ((AH5416(ah)->ah_procPhyErr & HAL_PROCESS_ANI))
+        ((AH5416(ah)->ah_procPhyErr & HAL_PROCESS_ANI))
 
 struct ar5416Stats {
         u32 ast_ani_niup;
@@ -425,7 +416,6 @@ struct ar5416Stats {
 #define AR5416_EEP_MINOR_VER_7       0x7
 #define AR5416_EEP_MINOR_VER_9       0x9
 
-#define AR5416_EEP_START_LOC            256
 #define AR5416_NUM_5G_CAL_PIERS         8
 #define AR5416_NUM_2G_CAL_PIERS         4
 #define AR5416_NUM_5G_20_TARGET_POWERS  8
@@ -441,25 +431,10 @@ struct ar5416Stats {
 #define AR5416_EEPROM_MODAL_SPURS       5
 #define AR5416_MAX_RATE_POWER           63
 #define AR5416_NUM_PDADC_VALUES         128
-#define AR5416_NUM_RATES                16
 #define AR5416_BCHAN_UNUSED             0xFF
 #define AR5416_MAX_PWR_RANGE_IN_HALF_DB 64
-#define AR5416_EEPMISC_BIG_ENDIAN       0x01
 #define AR5416_MAX_CHAINS               3
-#define AR5416_ANT_16S                  25
-
-#define AR5416_NUM_ANT_CHAIN_FIELDS     7
-#define AR5416_NUM_ANT_COMMON_FIELDS    4
-#define AR5416_SIZE_ANT_CHAIN_FIELD     3
-#define AR5416_SIZE_ANT_COMMON_FIELD    4
-#define AR5416_ANT_CHAIN_MASK           0x7
-#define AR5416_ANT_COMMON_MASK          0xf
-#define AR5416_CHAIN_0_IDX              0
-#define AR5416_CHAIN_1_IDX              1
-#define AR5416_CHAIN_2_IDX              2
-
 #define AR5416_PWR_TABLE_OFFSET         -5
-#define AR5416_LEGACY_CHAINMASK         1
 
 enum eeprom_param {
         EEP_NFTHRESH_5,
@@ -633,7 +608,7 @@ struct ar5416IniArray {
 };
 
 #define INIT_INI_ARRAY(iniarray, array, rows, columns) do {     \
-                (iniarray)->ia_array = (u32 *)(array);    \
+                (iniarray)->ia_array = (u32 *)(array);          \
                 (iniarray)->ia_rows = (rows);                   \
                 (iniarray)->ia_columns = (columns);             \
         } while (0)
@@ -641,16 +616,16 @@ struct ar5416IniArray {
 #define INI_RA(iniarray, row, column) \
         (((iniarray)->ia_array)[(row) * ((iniarray)->ia_columns) + (column)])
 
-#define INIT_CAL(_perCal) do { \
-                (_perCal)->calState = CAL_WAITING; \
-                (_perCal)->calNext = NULL; \
+#define INIT_CAL(_perCal) do {                          \
+                (_perCal)->calState = CAL_WAITING;      \
+                (_perCal)->calNext = NULL;              \
         } while (0)
 
 #define INSERT_CAL(_ahp, _perCal)                                       \
         do {                                                            \
                 if ((_ahp)->ah_cal_list_last == NULL) {                 \
-                        (_ahp)->ah_cal_list = \
-                                (_ahp)->ah_cal_list_last = (_perCal); \
+                        (_ahp)->ah_cal_list =                           \
+                                (_ahp)->ah_cal_list_last = (_perCal);   \
                         ((_ahp)->ah_cal_list_last)->calNext = (_perCal); \
                 } else {                                                \
                         ((_ahp)->ah_cal_list_last)->calNext = (_perCal); \
@@ -696,25 +671,29 @@ struct hal_cal_list {
 struct ath_hal_5416 {
         struct ath_hal ah;
         struct ar5416_eeprom ah_eeprom;
+        struct ar5416Stats ah_stats;
+        struct ath9k_tx_queue_info ah_txq[ATH9K_NUM_TX_QUEUES];
+        void __iomem *ah_cal_mem;
+
         u8 ah_macaddr[ETH_ALEN];
         u8 ah_bssid[ETH_ALEN];
         u8 ah_bssidmask[ETH_ALEN];
         u16 ah_assocId;
+
         int16_t ah_curchanRadIndex;
         u32 ah_maskReg;
-        struct ar5416Stats ah_stats;
-        u32 ah_txDescMask;
         u32 ah_txOkInterruptMask;
         u32 ah_txErrInterruptMask;
         u32 ah_txDescInterruptMask;
         u32 ah_txEolInterruptMask;
         u32 ah_txUrnInterruptMask;
-        struct ath9k_tx_queue_info ah_txq[ATH9K_NUM_TX_QUEUES];
-        enum ath9k_power_mode ah_powerMode;
         bool ah_chipFullSleep;
         u32 ah_atimWindow;
-        enum ath9k_ant_setting ah_diversityControl;
         u16 ah_antennaSwitchSwap;
+        enum ath9k_power_mode ah_powerMode;
+        enum ath9k_ant_setting ah_diversityControl;
+
+        /* Calibration */
         enum hal_cal_types ah_suppCals;
         struct hal_cal_list ah_iqCalData;
         struct hal_cal_list ah_adcGainCalData;
@@ -751,16 +730,16 @@ struct ath_hal_5416 {
                 int32_t sign[AR5416_MAX_CHAINS];
         } ah_Meas3;
         u16 ah_CalSamples;
-        u32 ah_tx6PowerInHalfDbm;
+
         u32 ah_staId1Defaults;
         u32 ah_miscMode;
-        bool ah_tpcEnabled;
-        u32 ah_beaconInterval;
         enum {
                 AUTO_32KHZ,
                 USE_32KHZ,
                 DONT_USE_32KHZ,
         } ah_enable32kHzClock;
+
+        /* RF */
         u32 *ah_analogBank0Data;
         u32 *ah_analogBank1Data;
         u32 *ah_analogBank2Data;
@@ -770,8 +749,9 @@ struct ath_hal_5416 {
         u32 *ah_analogBank7Data;
         u32 *ah_addac5416_21;
         u32 *ah_bank6Temp;
-        u32 ah_ofdmTxPower;
+
         int16_t ah_txPowerIndexOffset;
+        u32 ah_beaconInterval;
         u32 ah_slottime;
         u32 ah_acktimeout;
         u32 ah_ctstimeout;
@@ -780,7 +760,8 @@ struct ath_hal_5416 {
         u32 ah_gpioSelect;
         u32 ah_polarity;
         u32 ah_gpioBit;
-        bool ah_eepEnabled;
+
+        /* ANI */
         u32 ah_procPhyErr;
         bool ah_hasHwPhyCounters;
         u32 ah_aniPeriod;
@@ -790,18 +771,14 @@ struct ath_hal_5416 {
         int ah_coarseHigh[5];
         int ah_coarseLow[5];
         int ah_firpwr[5];
-        u16 ah_ratesArray[16];
+        enum ath9k_ani_cmd ah_ani_function;
+
         u32 ah_intrTxqs;
         bool ah_intrMitigation;
-        u32 ah_cycleCount;
-        u32 ah_ctlBusy;
-        u32 ah_extBusy;
         enum ath9k_ht_extprotspacing ah_extprotspacing;
         u8 ah_txchainmask;
         u8 ah_rxchainmask;
-        int ah_hwp;
-        void __iomem *ah_cal_mem;
-        enum ath9k_ani_cmd ah_ani_function;
+
         struct ar5416IniArray ah_iniModes;
         struct ar5416IniArray ah_iniCommon;
         struct ar5416IniArray ah_iniBank0;
@@ -820,10 +797,6 @@ struct ath_hal_5416 {
 
 #define FREQ2FBIN(x, y) ((y) ? ((x) - 2300) : (((x) - 4800) / 5))
 
-#define IS_5416_EMU(ah)                                 \
-        ((ah->ah_devid == AR5416_DEVID_EMU) ||          \
-         (ah->ah_devid == AR5416_DEVID_EMU_PCIE))
-
 #define ar5416RfDetach(ah) do {                                 \
                 if (AH5416(ah)->ah_rfHal.rfDetach != NULL)      \
                         AH5416(ah)->ah_rfHal.rfDetach(ah);      \
@@ -841,8 +814,8 @@ struct ath_hal_5416 {
 #define REG_WRITE_ARRAY(iniarray, column, regWr) do {                   \
                 int r;                                                  \
                 for (r = 0; r < ((iniarray)->ia_rows); r++) {           \
-                        REG_WRITE(ah, INI_RA((iniarray), (r), 0), \
-                                INI_RA((iniarray), r, (column))); \
+                        REG_WRITE(ah, INI_RA((iniarray), (r), 0),       \
+                                  INI_RA((iniarray), r, (column)));     \
                         DO_DELAY(regWr);                                \
                 }                                                       \
         } while (0)
@@ -852,30 +825,21 @@ struct ath_hal_5416 {
 #define COEF_SCALE_S                24
 #define HT40_CHANNEL_CENTER_SHIFT   10
 
-#define ar5416CheckOpMode(_opmode)                                      \
-        ((_opmode == ATH9K_M_STA) || (_opmode == ATH9K_M_IBSS) ||       \
-         (_opmode == ATH9K_M_HOSTAP) || (_opmode == ATH9K_M_MONITOR))
-
 #define AR5416_EEPROM_MAGIC_OFFSET  0x0
 
 #define AR5416_EEPROM_S             2
 #define AR5416_EEPROM_OFFSET        0x2000
-#define AR5416_EEPROM_START_ADDR                        \
+#define AR5416_EEPROM_START_ADDR \
         (AR_SREV_9100(ah)) ? 0x1fff1000 : 0x503f1200
 #define AR5416_EEPROM_MAX           0xae0
-#define ar5416_get_eep_ver(_ahp)                                \
+#define ar5416_get_eep_ver(_ahp) \
         (((_ahp)->ah_eeprom.baseEepHeader.version >> 12) & 0xF)
-#define ar5416_get_eep_rev(_ahp)                                \
+#define ar5416_get_eep_rev(_ahp) \
         (((_ahp)->ah_eeprom.baseEepHeader.version) & 0xFFF)
-#define ar5416_get_ntxchains(_txchainmask)                              \
+#define ar5416_get_ntxchains(_txchainmask) \
         (((_txchainmask >> 2) & 1) + \
                 ((_txchainmask >> 1) & 1) + (_txchainmask & 1))
 
-#define IS_EEP_MINOR_V3(_ahp) \
-        (ath9k_hw_get_eeprom((_ahp), EEP_MINOR_REV)  >= AR5416_EEP_MINOR_VER_3)
-
-#define FIXED_CCA_THRESHOLD 15
-
 #ifdef __BIG_ENDIAN
 #define AR5416_EEPROM_MAGIC 0x5aa5
 #else
@@ -910,8 +874,6 @@ struct ath_hal_5416 {
 #define AR_GPIOD_MASK                   0x00001FFF
 #define AR_GPIO_BIT(_gpio)              (1 << (_gpio))
 
-#define MAX_ANALOG_START                319
-
 #define HAL_EP_RND(x, mul) \
         ((((x)%(mul)) >= ((mul)/2)) ? ((x) + ((mul) - 1)) / (mul) : (x)/(mul))
 #define BEACON_RSSI(ahp) \
@@ -923,8 +885,6 @@ struct ath_hal_5416 {
 #define AH_TIMEOUT         100000
 #define AH_TIME_QUANTUM        10
 
-#define IS(_c, _f)       (((_c)->channelFlags & _f) || 0)
-
 #define AR_KEYTABLE_SIZE 128
 #define POWER_UP_TIME    200000
 
@@ -964,6 +924,6 @@ struct ath_hal_5416 {
 #define OFDM_SYMBOL_TIME_QUARTER    16
 
 u32 ath9k_hw_get_eeprom(struct ath_hal_5416 *ahp,
-                              enum eeprom_param param);
+                        enum eeprom_param param);
 
 #endif
diff --git a/drivers/net/wireless/ath9k/main.c b/drivers/net/wireless/ath9k/main.c
index 2888778040e4..dc45eef3289a 100644
--- a/drivers/net/wireless/ath9k/main.c
+++ b/drivers/net/wireless/ath9k/main.c
@@ -22,8 +22,6 @@
 #define ATH_PCI_VERSION "0.1"
 
 #define IEEE80211_HTCAP_MAXRXAMPDU_FACTOR       13
-#define IEEE80211_ACTION_CAT_HT                 7
-#define IEEE80211_ACTION_HT_TXCHWIDTH           0
 
 static char *dev_info = "ath9k";
 
@@ -212,21 +210,16 @@ static int ath_key_config(struct ath_softc *sc,
 
 static void ath_key_delete(struct ath_softc *sc, struct ieee80211_key_conf *key)
 {
-#define ATH_MAX_NUM_KEYS 4
         int freeslot;
 
-        freeslot = (key->keyidx >= ATH_MAX_NUM_KEYS) ? 1 : 0;
+        freeslot = (key->keyidx >= 4) ? 1 : 0;
         ath_key_reset(sc, key->keyidx, freeslot);
-#undef ATH_MAX_NUM_KEYS
 }
 
 static void setup_ht_cap(struct ieee80211_ht_info *ht_info)
 {
-/* Until mac80211 includes these fields */
-
-#define IEEE80211_HT_CAP_DSSSCCK40 0x1000
-#define IEEE80211_HT_CAP_MAXRXAMPDU_65536 0x3   /* 2 ^ 16 */
-#define IEEE80211_HT_CAP_MPDUDENSITY_8 0x6      /* 8 usec */
+#define ATH9K_HT_CAP_MAXRXAMPDU_65536 0x3       /* 2 ^ 16 */
+#define ATH9K_HT_CAP_MPDUDENSITY_8 0x6          /* 8 usec */
 
         ht_info->ht_supported = 1;
         ht_info->cap = (u16)IEEE80211_HT_CAP_SUP_WIDTH
@@ -234,8 +227,8 @@ static void setup_ht_cap(struct ieee80211_ht_info *ht_info)
                         |(u16)IEEE80211_HT_CAP_SGI_40
                         |(u16)IEEE80211_HT_CAP_DSSSCCK40;
 
-        ht_info->ampdu_factor = IEEE80211_HT_CAP_MAXRXAMPDU_65536;
-        ht_info->ampdu_density = IEEE80211_HT_CAP_MPDUDENSITY_8;
+        ht_info->ampdu_factor = ATH9K_HT_CAP_MAXRXAMPDU_65536;
+        ht_info->ampdu_density = ATH9K_HT_CAP_MPDUDENSITY_8;
         /* setup supported mcs set */
         memset(ht_info->supp_mcs_set, 0, 16);
         ht_info->supp_mcs_set[0] = 0xff;
@@ -368,6 +361,20 @@ static int ath9k_tx(struct ieee80211_hw *hw,
 {
         struct ath_softc *sc = hw->priv;
         int hdrlen, padsize;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+        /*
+         * As a temporary workaround, assign seq# here; this will likely need
+         * to be cleaned up to work better with Beacon transmission and virtual
+         * BSSes.
+         */
+        if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+                struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+                if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
+                        sc->seq_no += 0x10;
+                hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
+                hdr->seq_ctrl |= cpu_to_le16(sc->seq_no);
+        }
 
         /* Add the padding after the header if this is not already done */
         hdrlen = ieee80211_get_hdrlen_from_skb(skb);
@@ -426,10 +433,13 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
         case IEEE80211_IF_TYPE_IBSS:
                 ic_opmode = ATH9K_M_IBSS;
                 break;
+        case IEEE80211_IF_TYPE_AP:
+                ic_opmode = ATH9K_M_HOSTAP;
+                break;
         default:
                 DPRINTF(sc, ATH_DBG_FATAL,
-                        "%s: Only STA and IBSS are supported currently\n",
-                        __func__);
+                        "%s: Interface type %d not yet supported\n",
+                        __func__, conf->type);
                 return -EOPNOTSUPP;
         }
 
@@ -472,7 +482,8 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
         ath_rate_newstate(sc, avp);
 
         /* Reclaim beacon resources */
-        if (sc->sc_opmode == ATH9K_M_HOSTAP || sc->sc_opmode == ATH9K_M_IBSS) {
+        if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP ||
+            sc->sc_ah->ah_opmode == ATH9K_M_IBSS) {
                 ath9k_hw_stoptxdma(sc->sc_ah, sc->sc_bhalq);
                 ath_beacon_return(sc, avp);
         }
@@ -480,7 +491,7 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
         /* Set interrupt mask */
         sc->sc_imask &= ~(ATH9K_INT_SWBA | ATH9K_INT_BMISS);
         ath9k_hw_set_interrupts(sc->sc_ah, sc->sc_imask & ~ATH9K_INT_GLOBAL);
-        sc->sc_beacons = 0;
+        sc->sc_flags &= ~SC_OP_BEACONS;
 
         error = ath_vap_detach(sc, 0);
         if (error)
@@ -529,6 +540,7 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
                                   struct ieee80211_if_conf *conf)
 {
         struct ath_softc *sc = hw->priv;
+        struct ath_hal *ah = sc->sc_ah;
         struct ath_vap *avp;
         u32 rfilt = 0;
         int error, i;
@@ -541,6 +553,17 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
                 return -EINVAL;
         }
 
+        /* TODO: Need to decide which hw opmode to use for multi-interface
+         * cases */
+        if (vif->type == IEEE80211_IF_TYPE_AP &&
+            ah->ah_opmode != ATH9K_M_HOSTAP) {
+                ah->ah_opmode = ATH9K_M_HOSTAP;
+                ath9k_hw_setopmode(ah);
+                ath9k_hw_write_associd(ah, sc->sc_myaddr, 0);
+                /* Request full reset to get hw opmode changed properly */
+                sc->sc_flags |= SC_OP_FULL_RESET;
+        }
+
         if ((conf->changed & IEEE80211_IFCC_BSSID) &&
             !is_zero_ether_addr(conf->bssid)) {
                 switch (vif->type) {
@@ -549,10 +572,6 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
                         /* Update ratectrl about the new state */
                         ath_rate_newstate(sc, avp);
 
-                        /* Set rx filter */
-                        rfilt = ath_calcrxfilter(sc);
-                        ath9k_hw_setrxfilter(sc->sc_ah, rfilt);
-
                         /* Set BSSID */
                         memcpy(sc->sc_curbssid, conf->bssid, ETH_ALEN);
                         sc->sc_curaid = 0;
@@ -585,7 +604,7 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
                                 print_mac(mac, sc->sc_curbssid), sc->sc_curaid);
 
                         /* need to reconfigure the beacon */
-                        sc->sc_beacons = 0;
+                        sc->sc_flags &= ~SC_OP_BEACONS ;
 
                         break;
                 default:
@@ -594,7 +613,8 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
         }
 
         if ((conf->changed & IEEE80211_IFCC_BEACON) &&
-            (vif->type == IEEE80211_IF_TYPE_IBSS)) {
+            ((vif->type == IEEE80211_IF_TYPE_IBSS) ||
+             (vif->type == IEEE80211_IF_TYPE_AP))) {
                 /*
                  * Allocate and setup the beacon frame.
                  *
@@ -636,8 +656,7 @@ static int ath9k_config_interface(struct ieee80211_hw *hw,
         FIF_BCN_PRBRESP_PROMISC |               \
         FIF_FCSFAIL)
 
-/* Accept unicast, bcast and mcast frames */
-
+/* FIXME: sc->sc_full_reset ? */
 static void ath9k_configure_filter(struct ieee80211_hw *hw,
                                    unsigned int changed_flags,
                                    unsigned int *total_flags,
@@ -645,16 +664,22 @@ static void ath9k_configure_filter(struct ieee80211_hw *hw,
                                    struct dev_mc_list *mclist)
 {
         struct ath_softc *sc = hw->priv;
+        u32 rfilt;
 
         changed_flags &= SUPPORTED_FILTERS;
         *total_flags &= SUPPORTED_FILTERS;
 
+        sc->rx_filter = *total_flags;
+        rfilt = ath_calcrxfilter(sc);
+        ath9k_hw_setrxfilter(sc->sc_ah, rfilt);
+
         if (changed_flags & FIF_BCN_PRBRESP_PROMISC) {
                 if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
-                        ath_scan_start(sc);
-                else
-                        ath_scan_end(sc);
+                        ath9k_hw_write_associd(sc->sc_ah, ath_bcast_mac, 0);
         }
+
+        DPRINTF(sc, ATH_DBG_CONFIG, "%s: Set HW RX filter: 0x%x\n",
+                __func__, sc->rx_filter);
 }
 
 static void ath9k_sta_notify(struct ieee80211_hw *hw,
@@ -831,7 +856,7 @@ static void ath9k_bss_assoc_info(struct ath_softc *sc,
 
                 /* Configure the beacon */
                 ath_beacon_config(sc, 0);
-                sc->sc_beacons = 1;
+                sc->sc_flags |= SC_OP_BEACONS;
 
                 /* Reset rssi stats */
                 sc->sc_halstats.ns_avgbrssi = ATH_RSSI_DUMMY_MARKER;
@@ -894,9 +919,9 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
                         __func__,
                         bss_conf->use_short_preamble);
                 if (bss_conf->use_short_preamble)
-                        sc->sc_flags |= ATH_PREAMBLE_SHORT;
+                        sc->sc_flags |= SC_OP_PREAMBLE_SHORT;
                 else
-                        sc->sc_flags &= ~ATH_PREAMBLE_SHORT;
+                        sc->sc_flags &= ~SC_OP_PREAMBLE_SHORT;
         }
 
         if (changed & BSS_CHANGED_ERP_CTS_PROT) {
@@ -905,9 +930,9 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
                         bss_conf->use_cts_prot);
                 if (bss_conf->use_cts_prot &&
                     hw->conf.channel->band != IEEE80211_BAND_5GHZ)
-                        sc->sc_flags |= ATH_PROTECT_ENABLE;
+                        sc->sc_flags |= SC_OP_PROTECT_ENABLE;
                 else
-                        sc->sc_flags &= ~ATH_PROTECT_ENABLE;
+                        sc->sc_flags &= ~SC_OP_PROTECT_ENABLE;
         }
 
         if (changed & BSS_CHANGED_HT) {
@@ -1035,15 +1060,6 @@ void ath_get_beaconconfig(struct ath_softc *sc,
         conf->bmiss_timeout = ATH_DEFAULT_BMISS_LIMIT * conf->listen_interval;
 }
 
-int ath_update_beacon(struct ath_softc *sc,
-                      int if_id,
-                      struct ath_beacon_offset *bo,
-                      struct sk_buff *skb,
-                      int mcast)
-{
-        return 0;
-}
-
 void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
                      struct ath_xmit_status *tx_status, struct ath_node *an)
 {
@@ -1065,8 +1081,16 @@ void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
                 tx_info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
                 tx_status->flags &= ~ATH_TX_BAR;
         }
-        if (tx_status->flags)
-                tx_info->status.excessive_retries = 1;
+
+        if (tx_status->flags & (ATH_TX_ERROR | ATH_TX_XRETRY)) {
+                if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+                        /* Frame was not ACKed, but an ACK was expected */
+                        tx_info->status.excessive_retries = 1;
+                }
+        } else {
+                /* Frame was ACKed */
+                tx_info->flags |= IEEE80211_TX_STAT_ACK;
+        }
 
         tx_info->status.retry_count = tx_status->retries;
 
@@ -1075,7 +1099,7 @@ void ath_tx_complete(struct ath_softc *sc, struct sk_buff *skb,
                 ath_node_put(sc, an, ATH9K_BH_STATUS_CHANGE);
 }
 
-int ath__rx_indicate(struct ath_softc *sc,
+int _ath_rx_indicate(struct ath_softc *sc,
                      struct sk_buff *skb,
                      struct ath_recv_status *status,
                      u16 keyix)
@@ -1095,9 +1119,6 @@ int ath__rx_indicate(struct ath_softc *sc,
                 skb_pull(skb, padsize);
         }
 
-        /* remove FCS before passing up to protocol stack */
-        skb_trim(skb, (skb->len - FCS_LEN));
-
         /* Prepare rx status */
         ath9k_rx_prepare(sc, skb, status, &rx_status);
 
@@ -1146,9 +1167,119 @@ int ath_rx_subframe(struct ath_node *an,
         return 0;
 }
 
-enum ath9k_ht_macmode ath_cwm_macmode(struct ath_softc *sc)
+/********************************/
+/*       LED functions          */
+/********************************/
+
+static void ath_led_brightness(struct led_classdev *led_cdev,
+                               enum led_brightness brightness)
 {
-        return sc->sc_ht_info.tx_chan_width;
+        struct ath_led *led = container_of(led_cdev, struct ath_led, led_cdev);
+        struct ath_softc *sc = led->sc;
+
+        switch (brightness) {
+        case LED_OFF:
+                if (led->led_type == ATH_LED_ASSOC ||
+                    led->led_type == ATH_LED_RADIO)
+                        sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
+                ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN,
+                                (led->led_type == ATH_LED_RADIO) ? 1 :
+                                !!(sc->sc_flags & SC_OP_LED_ASSOCIATED));
+                break;
+        case LED_FULL:
+                if (led->led_type == ATH_LED_ASSOC)
+                        sc->sc_flags |= SC_OP_LED_ASSOCIATED;
+                ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 0);
+                break;
+        default:
+                break;
+        }
+}
+
+static int ath_register_led(struct ath_softc *sc, struct ath_led *led,
+                            char *trigger)
+{
+        int ret;
+
+        led->sc = sc;
+        led->led_cdev.name = led->name;
+        led->led_cdev.default_trigger = trigger;
+        led->led_cdev.brightness_set = ath_led_brightness;
+
+        ret = led_classdev_register(wiphy_dev(sc->hw->wiphy), &led->led_cdev);
+        if (ret)
+                DPRINTF(sc, ATH_DBG_FATAL,
+                        "Failed to register led:%s", led->name);
+        else
+                led->registered = 1;
+        return ret;
+}
+
+static void ath_unregister_led(struct ath_led *led)
+{
+        if (led->registered) {
+                led_classdev_unregister(&led->led_cdev);
+                led->registered = 0;
+        }
+}
+
+static void ath_deinit_leds(struct ath_softc *sc)
+{
+        ath_unregister_led(&sc->assoc_led);
+        sc->sc_flags &= ~SC_OP_LED_ASSOCIATED;
+        ath_unregister_led(&sc->tx_led);
+        ath_unregister_led(&sc->rx_led);
+        ath_unregister_led(&sc->radio_led);
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+}
+
+static void ath_init_leds(struct ath_softc *sc)
+{
+        char *trigger;
+        int ret;
+
+        /* Configure gpio 1 for output */
+        ath9k_hw_cfg_output(sc->sc_ah, ATH_LED_PIN,
+                            AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
+        /* LED off, active low */
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+
+        trigger = ieee80211_get_radio_led_name(sc->hw);
+        snprintf(sc->radio_led.name, sizeof(sc->radio_led.name),
+                "ath9k-%s:radio", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->radio_led, trigger);
+        sc->radio_led.led_type = ATH_LED_RADIO;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_assoc_led_name(sc->hw);
+        snprintf(sc->assoc_led.name, sizeof(sc->assoc_led.name),
+                "ath9k-%s:assoc", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->assoc_led, trigger);
+        sc->assoc_led.led_type = ATH_LED_ASSOC;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_tx_led_name(sc->hw);
+        snprintf(sc->tx_led.name, sizeof(sc->tx_led.name),
+                "ath9k-%s:tx", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->tx_led, trigger);
+        sc->tx_led.led_type = ATH_LED_TX;
+        if (ret)
+                goto fail;
+
+        trigger = ieee80211_get_rx_led_name(sc->hw);
+        snprintf(sc->rx_led.name, sizeof(sc->rx_led.name),
+                "ath9k-%s:rx", wiphy_name(sc->hw->wiphy));
+        ret = ath_register_led(sc, &sc->rx_led, trigger);
+        sc->rx_led.led_type = ATH_LED_RX;
+        if (ret)
+                goto fail;
+
+        return;
+
+fail:
+        ath_deinit_leds(sc);
 }
 
 static int ath_detach(struct ath_softc *sc)
@@ -1157,6 +1288,9 @@ static int ath_detach(struct ath_softc *sc)
 
         DPRINTF(sc, ATH_DBG_CONFIG, "%s: Detach ATH hw\n", __func__);
 
+        /* Deinit LED control */
+        ath_deinit_leds(sc);
+
         /* Unregister hw */
 
         ieee80211_unregister_hw(hw);
@@ -1250,18 +1384,21 @@ static int ath_attach(u16 devid,
                 goto bad;
         }
 
+        /* Initialize LED control */
+        ath_init_leds(sc);
+
         /* initialize tx/rx engine */
 
         error = ath_tx_init(sc, ATH_TXBUF);
         if (error != 0)
-                goto bad1;
+                goto detach;
 
         error = ath_rx_init(sc, ATH_RXBUF);
         if (error != 0)
-                goto bad1;
+                goto detach;
 
         return 0;
-bad1:
+detach:
         ath_detach(sc);
 bad:
         return error;
@@ -1340,7 +1477,9 @@ static int ath_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
                 goto bad2;
         }
 
-        hw->flags = IEEE80211_HW_SIGNAL_DBM |
+        hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+                IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+                IEEE80211_HW_SIGNAL_DBM |
                 IEEE80211_HW_NOISE_DBM;
 
         SET_IEEE80211_DEV(hw, &pdev->dev);
@@ -1404,6 +1543,10 @@ static void ath_pci_remove(struct pci_dev *pdev)
 
 static int ath_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 {
+        struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+        struct ath_softc *sc = hw->priv;
+
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
         pci_save_state(pdev);
         pci_disable_device(pdev);
         pci_set_power_state(pdev, 3);
@@ -1413,6 +1556,8 @@ static int ath_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 
 static int ath_pci_resume(struct pci_dev *pdev)
 {
+        struct ieee80211_hw *hw = pci_get_drvdata(pdev);
+        struct ath_softc *sc = hw->priv;
         u32 val;
         int err;
 
@@ -1429,6 +1574,11 @@ static int ath_pci_resume(struct pci_dev *pdev)
         if ((val & 0x0000ff00) != 0)
                 pci_write_config_dword(pdev, 0x40, val & 0xffff00ff);
 
+        /* Enable LED */
+        ath9k_hw_cfg_output(sc->sc_ah, ATH_LED_PIN,
+                            AR_GPIO_OUTPUT_MUX_AS_OUTPUT);
+        ath9k_hw_set_gpio(sc->sc_ah, ATH_LED_PIN, 1);
+
         return 0;
 }
 
diff --git a/drivers/net/wireless/ath9k/phy.h b/drivers/net/wireless/ath9k/phy.h
index 0cd399a5344a..14702344448b 100644
--- a/drivers/net/wireless/ath9k/phy.h
+++ b/drivers/net/wireless/ath9k/phy.h
@@ -18,19 +18,19 @@
 #define PHY_H
 
 bool ath9k_hw_ar9280_set_channel(struct ath_hal *ah,
-                                          struct ath9k_channel
-                                          *chan);
+                                 struct ath9k_channel
+                                 *chan);
 bool ath9k_hw_set_channel(struct ath_hal *ah,
-                                   struct ath9k_channel *chan);
+                          struct ath9k_channel *chan);
 void ath9k_hw_write_regs(struct ath_hal *ah, u32 modesIndex,
                          u32 freqIndex, int regWrites);
 bool ath9k_hw_set_rf_regs(struct ath_hal *ah,
-                                   struct ath9k_channel *chan,
-                                   u16 modesIndex);
+                          struct ath9k_channel *chan,
+                          u16 modesIndex);
 void ath9k_hw_decrease_chain_power(struct ath_hal *ah,
                                    struct ath9k_channel *chan);
 bool ath9k_hw_init_rf(struct ath_hal *ah,
-                               int *status);
+                      int *status);
 
 #define AR_PHY_BASE     0x9800
 #define AR_PHY(_n)      (AR_PHY_BASE + ((_n)<<2))
diff --git a/drivers/net/wireless/ath9k/rc.c b/drivers/net/wireless/ath9k/rc.c
index 73c460ad355f..390019ed398e 100644
--- a/drivers/net/wireless/ath9k/rc.c
+++ b/drivers/net/wireless/ath9k/rc.c
@@ -653,8 +653,8 @@ ath_rc_sib_init_validrates(struct ath_rate_node *ath_rc_priv,
         rate_ctrl = (struct ath_tx_ratectrl *)(ath_rc_priv);
         for (i = 0; i < rate_table->rate_cnt; i++) {
                 valid = (ath_rc_priv->single_stream ?
-                                rate_table->info[i].valid_single_stream :
-                                rate_table->info[i].valid);
+                         rate_table->info[i].valid_single_stream :
+                         rate_table->info[i].valid);
                 if (valid == TRUE) {
                         u32 phy = rate_table->info[i].phy;
                         u8 valid_rate_count = 0;
@@ -740,14 +740,14 @@ ath_rc_sib_setvalid_htrates(struct ath_rate_node *ath_rc_priv,
                 for (j = 0; j < rate_table->rate_cnt; j++) {
                         u32 phy = rate_table->info[j].phy;
                         u32 valid = (ath_rc_priv->single_stream ?
-                                rate_table->info[j].valid_single_stream :
-                                rate_table->info[j].valid);
+                                     rate_table->info[j].valid_single_stream :
+                                     rate_table->info[j].valid);
 
                         if (((((struct ath_rateset *)
-                                mcs_set)->rs_rates[i] & 0x7F) !=
-                                (rate_table->info[j].dot11rate & 0x7F)) ||
-                                        !WLAN_RC_PHY_HT(phy) ||
-                                        !WLAN_RC_PHY_HT_VALID(valid, capflag))
+                               mcs_set)->rs_rates[i] & 0x7F) !=
+                             (rate_table->info[j].dot11rate & 0x7F)) ||
+                            !WLAN_RC_PHY_HT(phy) ||
+                            !WLAN_RC_PHY_HT_VALID(valid, capflag))
                                 continue;
 
                         if (!ath_rc_valid_phyrate(phy, capflag, FALSE))
@@ -847,9 +847,9 @@ void ath_rate_newstate(struct ath_softc *sc, struct ath_vap *avp)
         /* For half and quarter rate channles use different
          * rate tables
          */
-        if (sc->sc_curchan.channelFlags & CHANNEL_HALF)
+        if (sc->sc_ah->ah_curchan->channelFlags & CHANNEL_HALF)
                 ar5416_sethalf_ratetable(asc);
-        else if (sc->sc_curchan.channelFlags & CHANNEL_QUARTER)
+        else if (sc->sc_ah->ah_curchan->channelFlags & CHANNEL_QUARTER)
                 ar5416_setquarter_ratetable(asc);
         else /* full rate */
                 ar5416_setfull_ratetable(asc);
@@ -866,10 +866,10 @@ void ath_rate_newstate(struct ath_softc *sc, struct ath_vap *avp)
 }
 
 static u8 ath_rc_ratefind_ht(struct ath_softc *sc,
-                                   struct ath_rate_node *ath_rc_priv,
-                                   const struct ath_rate_table *rate_table,
-                                   int probe_allowed, int *is_probing,
-                                   int is_retry)
+                             struct ath_rate_node *ath_rc_priv,
+                             const struct ath_rate_table *rate_table,
+                             int probe_allowed, int *is_probing,
+                             int is_retry)
 {
         u32 dt, best_thruput, this_thruput, now_msec;
         u8 rate, next_rate, best_rate, maxindex, minindex;
@@ -997,8 +997,8 @@ static u8 ath_rc_ratefind_ht(struct ath_softc *sc,
                 rate = rate_ctrl->rate_table_size - 1;
 
         ASSERT((rate_table->info[rate].valid && !ath_rc_priv->single_stream) ||
-                (rate_table->info[rate].valid_single_stream &&
-                        ath_rc_priv->single_stream));
+               (rate_table->info[rate].valid_single_stream &&
+                ath_rc_priv->single_stream));
 
         return rate;
 }
@@ -1023,10 +1023,10 @@ static void ath_rc_rate_set_series(const struct ath_rate_table *rate_table ,
 }
 
 static u8 ath_rc_rate_getidx(struct ath_softc *sc,
-                                   struct ath_rate_node *ath_rc_priv,
-                                   const struct ath_rate_table *rate_table,
-                                   u8 rix, u16 stepdown,
-                                   u16 min_rate)
+                             struct ath_rate_node *ath_rc_priv,
+                             const struct ath_rate_table *rate_table,
+                             u8 rix, u16 stepdown,
+                             u16 min_rate)
 {
         u32 j;
         u8 nextindex;
@@ -1066,8 +1066,8 @@ static void ath_rc_ratefind(struct ath_softc *sc,
         rate_table =
                 (struct ath_rate_table *)asc->hw_rate_table[sc->sc_curmode];
         rix = ath_rc_ratefind_ht(sc, ath_rc_priv, rate_table,
-                                (rcflag & ATH_RC_PROBE_ALLOWED) ? 1 : 0,
-                                is_probe, is_retry);
+                                 (rcflag & ATH_RC_PROBE_ALLOWED) ? 1 : 0,
+                                 is_probe, is_retry);
         nrix = rix;
 
         if ((rcflag & ATH_RC_PROBE_ALLOWED) && (*is_probe)) {
@@ -1099,13 +1099,13 @@ static void ath_rc_ratefind(struct ath_softc *sc,
                 try_num = ((i + 1) == num_rates) ?
                         num_tries - (try_per_rate * i) : try_per_rate ;
                 min_rate = (((i + 1) == num_rates) &&
-                        (rcflag & ATH_RC_MINRATE_LASTRATE)) ? 1 : 0;
+                            (rcflag & ATH_RC_MINRATE_LASTRATE)) ? 1 : 0;
 
                 nrix = ath_rc_rate_getidx(sc, ath_rc_priv,
-                        rate_table, nrix, 1, min_rate);
+                                          rate_table, nrix, 1, min_rate);
                 /* All other rates in the series have RTS enabled */
                 ath_rc_rate_set_series(rate_table,
-                        &series[i], try_num, nrix, TRUE);
+                                       &series[i], try_num, nrix, TRUE);
         }
 
         /*
@@ -1124,13 +1124,13 @@ static void ath_rc_ratefind(struct ath_softc *sc,
          * above conditions.
          */
         if ((sc->sc_curmode == ATH9K_MODE_11NG_HT20) ||
-                        (sc->sc_curmode == ATH9K_MODE_11NG_HT40PLUS) ||
-                        (sc->sc_curmode == ATH9K_MODE_11NG_HT40MINUS)) {
+            (sc->sc_curmode == ATH9K_MODE_11NG_HT40PLUS) ||
+            (sc->sc_curmode == ATH9K_MODE_11NG_HT40MINUS)) {
                 u8  dot11rate = rate_table->info[rix].dot11rate;
                 u8 phy = rate_table->info[rix].phy;
                 if (i == 4 &&
                     ((dot11rate == 2 && phy == WLAN_RC_PHY_HT_40_SS) ||
-                    (dot11rate == 3 && phy == WLAN_RC_PHY_HT_20_SS))) {
+                     (dot11rate == 3 && phy == WLAN_RC_PHY_HT_20_SS))) {
                         series[3].rix = series[2].rix;
                         series[3].flags = series[2].flags;
                         series[3].max_4ms_framelen = series[2].max_4ms_framelen;
@@ -1141,18 +1141,19 @@ static void ath_rc_ratefind(struct ath_softc *sc,
 /*
  * Return the Tx rate series.
  */
-void ath_rate_findrate(struct ath_softc *sc,
-                       struct ath_rate_node *ath_rc_priv,
-                       int num_tries,
-                       int num_rates,
-                       unsigned int rcflag,
-                       struct ath_rc_series series[],
-                       int *is_probe,
-                       int is_retry)
+static void ath_rate_findrate(struct ath_softc *sc,
+                              struct ath_rate_node *ath_rc_priv,
+                              int num_tries,
+                              int num_rates,
+                              unsigned int rcflag,
+                              struct ath_rc_series series[],
+                              int *is_probe,
+                              int is_retry)
 {
         struct ath_vap *avp = ath_rc_priv->avp;
 
-        DPRINTF(sc, ATH_DBG_RATE, "%s", __func__);
+        DPRINTF(sc, ATH_DBG_RATE, "%s\n", __func__);
+
         if (!num_rates || !num_tries)
                 return;
 
@@ -1174,9 +1175,8 @@ void ath_rate_findrate(struct ath_softc *sc,
                         unsigned int    mcs;
                         u8 series_rix = 0;
 
-                        series[idx].tries =
-                                IEEE80211_RATE_IDX_ENTRY(
-                                        avp->av_config.av_fixed_retryset, idx);
+                        series[idx].tries = IEEE80211_RATE_IDX_ENTRY(
+                                avp->av_config.av_fixed_retryset, idx);
 
                         mcs = IEEE80211_RATE_IDX_ENTRY(
                                 avp->av_config.av_fixed_rateset, idx);
@@ -1228,7 +1228,7 @@ static void ath_rc_update_ht(struct ath_softc *sc,
         u32 now_msec = jiffies_to_msecs(jiffies);
         int state_change = FALSE, rate, count;
         u8 last_per;
-        struct ath_rate_softc  *asc = (struct ath_rate_softc *)sc->sc_rc;
+        struct ath_rate_softc *asc = (struct ath_rate_softc *)sc->sc_rc;
         struct ath_rate_table *rate_table =
                 (struct ath_rate_table *)asc->hw_rate_table[sc->sc_curmode];
 
@@ -1272,14 +1272,14 @@ static void ath_rc_update_ht(struct ath_softc *sc,
                 } else {
                         /* xretries == 2 */
                         count = sizeof(nretry_to_per_lookup) /
-                                        sizeof(nretry_to_per_lookup[0]);
+                                sizeof(nretry_to_per_lookup[0]);
                         if (retries >= count)
                                 retries = count - 1;
                         /* new_PER = 7/8*old_PER + 1/8*(currentPER) */
                         rate_ctrl->state[tx_rate].per =
                                 (u8)(rate_ctrl->state[tx_rate].per -
-                                (rate_ctrl->state[tx_rate].per >> 3) +
-                                ((100) >> 3));
+                                     (rate_ctrl->state[tx_rate].per >> 3) +
+                                     ((100) >> 3));
                 }
 
                 /* xretries == 1 or 2 */
@@ -1295,8 +1295,7 @@ static void ath_rc_update_ht(struct ath_softc *sc,
                 if (retries >= count)
                         retries = count - 1;
                 if (info_priv->n_bad_frames) {
-                        /* new_PER = 7/8*old_PER + 1/8*(currentPER)  */
-                        /*
+                        /* new_PER = 7/8*old_PER + 1/8*(currentPER)
                          * Assuming that n_frames is not 0.  The current PER
                          * from the retries is 100 * retries / (retries+1),
                          * since the first retries attempts failed, and the
@@ -1386,7 +1385,7 @@ static void ath_rc_update_ht(struct ath_softc *sc,
                          * rssi_ack values.
                          */
                         if (tx_rate == rate_ctrl->rate_max_phy &&
-                                        rate_ctrl->hw_maxretry_pktcnt < 255) {
+                            rate_ctrl->hw_maxretry_pktcnt < 255) {
                                 rate_ctrl->hw_maxretry_pktcnt++;
                         }
 
@@ -1418,7 +1417,7 @@ static void ath_rc_update_ht(struct ath_softc *sc,
                                         /* Now reduce the current
                                          * rssi threshold. */
                                         if ((rssi_ackAvg < rssi_thres + 2) &&
-                                                (rssi_thres > rssi_ack_vmin)) {
+                                            (rssi_thres > rssi_ack_vmin)) {
                                                 rate_ctrl->state[tx_rate].
                                                         rssi_thres--;
                                         }
@@ -1436,10 +1435,10 @@ static void ath_rc_update_ht(struct ath_softc *sc,
          * a while (except if we are probing).
          */
         if (rate_ctrl->state[tx_rate].per >= 55 && tx_rate > 0 &&
-                        rate_table->info[tx_rate].ratekbps <=
-                        rate_table->info[rate_ctrl->rate_max_phy].ratekbps) {
+            rate_table->info[tx_rate].ratekbps <=
+            rate_table->info[rate_ctrl->rate_max_phy].ratekbps) {
                 ath_rc_get_nextlowervalid_txrate(rate_table, rate_ctrl,
-                                (u8) tx_rate, &rate_ctrl->rate_max_phy);
+                                 (u8) tx_rate, &rate_ctrl->rate_max_phy);
 
                 /* Don't probe for a little while. */
                 rate_ctrl->probe_time = now_msec;
@@ -1460,43 +1459,43 @@ static void ath_rc_update_ht(struct ath_softc *sc,
                                 break;
 
                         if (rate_ctrl->state[rate].rssi_thres +
-                                rate_table->info[rate].rssi_ack_deltamin >
-                                        rate_ctrl->state[rate+1].rssi_thres) {
+                            rate_table->info[rate].rssi_ack_deltamin >
+                            rate_ctrl->state[rate+1].rssi_thres) {
                                 rate_ctrl->state[rate+1].rssi_thres =
                                         rate_ctrl->state[rate].
-                                                rssi_thres +
+                                        rssi_thres +
                                         rate_table->info[rate].
-                                                rssi_ack_deltamin;
+                                        rssi_ack_deltamin;
                         }
                 }
 
                 /* Make sure the rates below this have lower rssi thresholds. */
                 for (rate = tx_rate - 1; rate >= 0; rate--) {
                         if (rate_table->info[rate].phy !=
-                                rate_table->info[tx_rate].phy)
+                            rate_table->info[tx_rate].phy)
                                 break;
 
                         if (rate_ctrl->state[rate].rssi_thres +
-                                rate_table->info[rate].rssi_ack_deltamin >
-                                        rate_ctrl->state[rate+1].rssi_thres) {
+                            rate_table->info[rate].rssi_ack_deltamin >
+                            rate_ctrl->state[rate+1].rssi_thres) {
                                 if (rate_ctrl->state[rate+1].rssi_thres <
-                                        rate_table->info[rate].
-                                        rssi_ack_deltamin)
+                                    rate_table->info[rate].
+                                    rssi_ack_deltamin)
                                         rate_ctrl->state[rate].rssi_thres = 0;
                                 else {
                                         rate_ctrl->state[rate].rssi_thres =
                                                 rate_ctrl->state[rate+1].
-                                                        rssi_thres -
-                                                        rate_table->info[rate].
-                                                        rssi_ack_deltamin;
+                                                rssi_thres -
+                                                rate_table->info[rate].
+                                                rssi_ack_deltamin;
                                 }
 
                                 if (rate_ctrl->state[rate].rssi_thres <
-                                        rate_table->info[rate].
-                                                rssi_ack_validmin) {
+                                    rate_table->info[rate].
+                                    rssi_ack_validmin) {
                                         rate_ctrl->state[rate].rssi_thres =
                                                 rate_table->info[rate].
-                                                        rssi_ack_validmin;
+                                                rssi_ack_validmin;
                                 }
                         }
                 }
@@ -1507,11 +1506,11 @@ static void ath_rc_update_ht(struct ath_softc *sc,
         if (rate_ctrl->state[tx_rate].per < last_per) {
                 for (rate = tx_rate - 1; rate >= 0; rate--) {
                         if (rate_table->info[rate].phy !=
-                                rate_table->info[tx_rate].phy)
+                            rate_table->info[tx_rate].phy)
                                 break;
 
                         if (rate_ctrl->state[rate].per >
-                                        rate_ctrl->state[rate+1].per) {
+                            rate_ctrl->state[rate+1].per) {
                                 rate_ctrl->state[rate].per =
                                         rate_ctrl->state[rate+1].per;
                         }
@@ -1528,11 +1527,11 @@ static void ath_rc_update_ht(struct ath_softc *sc,
         /* Every so often, we reduce the thresholds and
          * PER (different for CCK and OFDM). */
         if (now_msec - rate_ctrl->rssi_down_time >=
-                rate_table->rssi_reduce_interval) {
+            rate_table->rssi_reduce_interval) {
 
                 for (rate = 0; rate < rate_ctrl->rate_table_size; rate++) {
                         if (rate_ctrl->state[rate].rssi_thres >
-                                rate_table->info[rate].rssi_ack_validmin)
+                            rate_table->info[rate].rssi_ack_validmin)
                                 rate_ctrl->state[rate].rssi_thres -= 1;
                 }
                 rate_ctrl->rssi_down_time = now_msec;
@@ -1541,7 +1540,7 @@ static void ath_rc_update_ht(struct ath_softc *sc,
         /* Every so often, we reduce the thresholds
          * and PER (different for CCK and OFDM). */
         if (now_msec - rate_ctrl->per_down_time >=
-                rate_table->rssi_reduce_interval) {
+            rate_table->rssi_reduce_interval) {
                 for (rate = 0; rate < rate_ctrl->rate_table_size; rate++) {
                         rate_ctrl->state[rate].per =
                                 7 * rate_ctrl->state[rate].per / 8;
@@ -1560,7 +1559,7 @@ static void ath_rc_update(struct ath_softc *sc,
                           struct ath_tx_info_priv *info_priv, int final_ts_idx,
                           int xretries, int long_retry)
 {
-        struct ath_rate_softc  *asc = (struct ath_rate_softc *)sc->sc_rc;
+        struct ath_rate_softc *asc = (struct ath_rate_softc *)sc->sc_rc;
         struct ath_rate_table *rate_table;
         struct ath_tx_ratectrl *rate_ctrl;
         struct ath_rc_series rcs[4];
@@ -1637,7 +1636,6 @@ static void ath_rc_update(struct ath_softc *sc,
                 xretries, long_retry);
 }
 
-
 /*
  * Process a tx descriptor for a completed transmit (success or failure).
  */
@@ -1651,13 +1649,13 @@ static void ath_rate_tx_complete(struct ath_softc *sc,
         struct ath_vap *avp;
 
         avp = rc_priv->avp;
-        if ((avp->av_config.av_fixed_rateset != IEEE80211_FIXED_RATE_NONE)
-                        || info_priv->tx.ts_status & ATH9K_TXERR_FILT)
+        if ((avp->av_config.av_fixed_rateset != IEEE80211_FIXED_RATE_NONE) ||
+            (info_priv->tx.ts_status & ATH9K_TXERR_FILT))
                 return;
 
         if (info_priv->tx.ts_rssi > 0) {
                 ATH_RSSI_LPF(an->an_chainmask_sel.tx_avgrssi,
-                                info_priv->tx.ts_rssi);
+                             info_priv->tx.ts_rssi);
         }
 
         /*
@@ -1682,7 +1680,6 @@ static void ath_rate_tx_complete(struct ath_softc *sc,
                       info_priv->tx.ts_longretry);
 }
 
-
 /*
  *  Update the SIB's rate control information
  *
@@ -1701,8 +1698,8 @@ static void ath_rc_sib_update(struct ath_softc *sc,
         struct ath_rate_softc *asc = (struct ath_rate_softc *)sc->sc_rc;
         struct ath_rateset *rateset = negotiated_rates;
         u8 *ht_mcs = (u8 *)negotiated_htrates;
-        struct ath_tx_ratectrl *rate_ctrl  = (struct ath_tx_ratectrl *)
-                (ath_rc_priv);
+        struct ath_tx_ratectrl *rate_ctrl =
+                (struct ath_tx_ratectrl *)ath_rc_priv;
         u8 i, j, k, hi = 0, hthi = 0;
 
         rate_table = (struct ath_rate_table *)
@@ -1824,7 +1821,8 @@ static void ath_setup_rates(struct ieee80211_local *local, struct sta_info *sta)
         struct ath_rate_node *rc_priv = sta->rate_ctrl_priv;
         int i, j = 0;
 
-        DPRINTF(sc, ATH_DBG_RATE, "%s", __func__);
+        DPRINTF(sc, ATH_DBG_RATE, "%s\n", __func__);
+
         sband =  local->hw.wiphy->bands[local->hw.conf.channel->band];
         for (i = 0; i < sband->n_bitrates; i++) {
                 if (sta->supp_rates[local->hw.conf.channel->band] & BIT(i)) {
@@ -1903,7 +1901,7 @@ static void ath_tx_aggr_resp(struct ath_softc *sc,
         int state;
         DECLARE_MAC_BUF(mac);
 
-        if (!sc->sc_txaggr)
+        if (!(sc->sc_flags & SC_OP_TXAGGR))
                 return;
 
         txtid = ATH_AN_2_TID(an, tidno);
@@ -1944,7 +1942,7 @@ static void ath_get_rate(void *priv, struct net_device *dev,
         struct ath_rate_node *ath_rc_priv;
         struct ath_node *an;
         struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-        int is_probe, chk, ret;
+        int is_probe = FALSE, chk, ret;
         s8 lowest_idx;
         __le16 fc = hdr->frame_control;
         u8 *qc, tid;
@@ -1962,7 +1960,7 @@ static void ath_get_rate(void *priv, struct net_device *dev,
         tx_info_priv->min_rate = (sband->bitrates[lowest_idx].bitrate * 2) / 10;
         /* lowest rate for management and multicast/broadcast frames */
         if (!ieee80211_is_data(fc) ||
-                        is_multicast_ether_addr(hdr->addr1) || !sta) {
+            is_multicast_ether_addr(hdr->addr1) || !sta) {
                 sel->rate_idx = lowest_idx;
                 return;
         }
@@ -1978,7 +1976,7 @@ static void ath_get_rate(void *priv, struct net_device *dev,
                           false);
         if (is_probe)
                 sel->probe_idx = ((struct ath_tx_ratectrl *)
-                        sta->rate_ctrl_priv)->probe_rate;
+                                  sta->rate_ctrl_priv)->probe_rate;
 
         /* Ratecontrol sometimes returns invalid rate index */
         if (tx_info_priv->rcs[0].rix != 0xff)
@@ -2035,6 +2033,7 @@ static void ath_rate_init(void *priv, void *priv_sta,
         struct ieee80211_hw *hw = local_to_hw(local);
         struct ieee80211_conf *conf = &local->hw.conf;
         struct ath_softc *sc = hw->priv;
+        struct ath_rate_node *ath_rc_priv = priv_sta;
         int i, j = 0;
 
         DPRINTF(sc, ATH_DBG_RATE, "%s\n", __func__);
@@ -2046,12 +2045,11 @@ static void ath_rate_init(void *priv, void *priv_sta,
         if (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) {
                 for (i = 0; i < MCS_SET_SIZE; i++) {
                         if (conf->ht_conf.supp_mcs_set[i/8] & (1<<(i%8)))
-                                ((struct ath_rate_node *)
-                                priv_sta)->neg_ht_rates.rs_rates[j++] = i;
+                                ath_rc_priv->neg_ht_rates.rs_rates[j++] = i;
                         if (j == ATH_RATE_MAX)
                                 break;
                 }
-                ((struct ath_rate_node *)priv_sta)->neg_ht_rates.rs_nrates = j;
+                ath_rc_priv->neg_ht_rates.rs_nrates = j;
         }
         ath_rc_node_update(hw, priv_sta);
 }
@@ -2066,7 +2064,7 @@ static void *ath_rate_alloc(struct ieee80211_local *local)
         struct ieee80211_hw *hw = local_to_hw(local);
         struct ath_softc *sc = hw->priv;
 
-        DPRINTF(sc, ATH_DBG_RATE, "%s", __func__);
+        DPRINTF(sc, ATH_DBG_RATE, "%s\n", __func__);
         return local->hw.priv;
 }
 
@@ -2081,14 +2079,17 @@ static void *ath_rate_alloc_sta(void *priv, gfp_t gfp)
         struct ath_vap *avp = sc->sc_vaps[0];
         struct ath_rate_node *rate_priv;
 
-        DPRINTF(sc, ATH_DBG_RATE, "%s", __func__);
+        DPRINTF(sc, ATH_DBG_RATE, "%s\n", __func__);
+
         rate_priv = ath_rate_node_alloc(avp, sc->sc_rc, gfp);
         if (!rate_priv) {
-                DPRINTF(sc, ATH_DBG_FATAL, "%s:Unable to allocate"
-                                "private rate control structure", __func__);
+                DPRINTF(sc, ATH_DBG_FATAL,
+                        "%s: Unable to allocate private rc structure\n",
+                        __func__);
                 return NULL;
         }
         ath_rc_sib_init(rate_priv);
+
         return rate_priv;
 }
 
diff --git a/drivers/net/wireless/ath9k/rc.h b/drivers/net/wireless/ath9k/rc.h
index 71aef9c75232..b95b41508b98 100644
--- a/drivers/net/wireless/ath9k/rc.h
+++ b/drivers/net/wireless/ath9k/rc.h
@@ -71,9 +71,6 @@ enum ieee80211_fixed_rate_mode {
  */
 #define IEEE80211_RATE_IDX_ENTRY(val, idx) (((val&(0xff<<(idx*8)))>>(idx*8)))
 
-#define SHORT_PRE 1
-#define LONG_PRE 0
-
 #define WLAN_PHY_HT_20_SS       WLAN_RC_PHY_HT_20_SS
 #define WLAN_PHY_HT_20_DS       WLAN_RC_PHY_HT_20_DS
 #define WLAN_PHY_HT_20_DS_HGI   WLAN_RC_PHY_HT_20_DS_HGI
@@ -102,18 +99,18 @@ enum {
         WLAN_RC_PHY_MAX
 };
 
-#define WLAN_RC_PHY_DS(_phy)   ((_phy == WLAN_RC_PHY_HT_20_DS)           \
-        || (_phy == WLAN_RC_PHY_HT_40_DS)        \
-        || (_phy == WLAN_RC_PHY_HT_20_DS_HGI)    \
-        || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
-#define WLAN_RC_PHY_40(_phy)   ((_phy == WLAN_RC_PHY_HT_40_SS)           \
-        || (_phy == WLAN_RC_PHY_HT_40_DS)        \
-        || (_phy == WLAN_RC_PHY_HT_40_SS_HGI)    \
-        || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
+#define WLAN_RC_PHY_DS(_phy)   ((_phy == WLAN_RC_PHY_HT_20_DS)          \
+                                || (_phy == WLAN_RC_PHY_HT_40_DS)       \
+                                || (_phy == WLAN_RC_PHY_HT_20_DS_HGI)   \
+                                || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
+#define WLAN_RC_PHY_40(_phy)   ((_phy == WLAN_RC_PHY_HT_40_SS)          \
+                                || (_phy == WLAN_RC_PHY_HT_40_DS)       \
+                                || (_phy == WLAN_RC_PHY_HT_40_SS_HGI)   \
+                                || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
 #define WLAN_RC_PHY_SGI(_phy)  ((_phy == WLAN_RC_PHY_HT_20_SS_HGI)      \
-        || (_phy == WLAN_RC_PHY_HT_20_DS_HGI)   \
-        || (_phy == WLAN_RC_PHY_HT_40_SS_HGI)   \
-        || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
+                                || (_phy == WLAN_RC_PHY_HT_20_DS_HGI)   \
+                                || (_phy == WLAN_RC_PHY_HT_40_SS_HGI)   \
+                                || (_phy == WLAN_RC_PHY_HT_40_DS_HGI))
 
 #define WLAN_RC_PHY_HT(_phy)    (_phy >= WLAN_RC_PHY_HT_20_SS)
 
@@ -135,56 +132,59 @@ enum {
 #define WLAN_RC_SGI_FLAG        (0x04)
 #define WLAN_RC_HT_FLAG         (0x08)
 
-/* Index into the rate table */
-#define INIT_RATE_MAX_20        23
-#define INIT_RATE_MAX_40        40
-
 #define RATE_TABLE_SIZE         64
 
-/* XXX: Convert to kdoc */
+/**
+ * struct ath_rate_table - Rate Control table
+ * @valid: valid for use in rate control
+ * @valid_single_stream: valid for use in rate control for
+ *      single stream operation
+ * @phy: CCK/OFDM
+ * @ratekbps: rate in Kbits per second
+ * @user_ratekbps: user rate in Kbits per second
+ * @ratecode: rate that goes into HW descriptors
+ * @short_preamble: Mask for enabling short preamble in ratecode for CCK
+ * @dot11rate: value that goes into supported
+ *      rates info element of MLME
+ * @ctrl_rate: Index of next lower basic rate, used for duration computation
+ * @max_4ms_framelen: maximum frame length(bytes) for tx duration
+ * @probe_interval: interval for rate control to probe for other rates
+ * @rssi_reduce_interval: interval for rate control to reduce rssi
+ * @initial_ratemax: initial ratemax value used in ath_rc_sib_update()
+ */
 struct ath_rate_table {
         int rate_cnt;
         struct {
-                int valid;            /* Valid for use in rate control */
-                int valid_single_stream;/* Valid for use in rate control
-                                        for single stream operation */
-                u8 phy;              /* CCK/OFDM/TURBO/XR */
-                u32 ratekbps;         /* Rate in Kbits per second */
-                u32 user_ratekbps;     /* User rate in KBits per second */
-                u8 ratecode;         /* rate that goes into
-                                        hw descriptors */
-                u8 short_preamble;    /* Mask for enabling short preamble
-                                                in rate code for CCK */
-                u8 dot11rate;        /* Value that goes into supported
-                                        rates info element of MLME */
-                u8 ctrl_rate;      /* Index of next lower basic rate,
-                                        used for duration computation */
-                int8_t rssi_ack_validmin;  /* Rate control related */
-                int8_t rssi_ack_deltamin;  /* Rate control related */
-                u8 base_index;        /* base rate index */
-                u8 cw40index;        /* 40cap rate index */
-                u8 sgi_index;         /* shortgi rate index */
-                u8 ht_index;          /* shortgi rate index */
-                u32 max_4ms_framelen;   /* Maximum frame length(bytes)
-                                                for 4ms tx duration */
+                int valid;
+                int valid_single_stream;
+                u8 phy;
+                u32 ratekbps;
+                u32 user_ratekbps;
+                u8 ratecode;
+                u8 short_preamble;
+                u8 dot11rate;
+                u8 ctrl_rate;
+                int8_t rssi_ack_validmin;
+                int8_t rssi_ack_deltamin;
+                u8 base_index;
+                u8 cw40index;
+                u8 sgi_index;
+                u8 ht_index;
+                u32 max_4ms_framelen;
         } info[RATE_TABLE_SIZE];
-        u32 probe_interval;        /* interval for ratectrl to
-                                        probe for other rates */
-        u32 rssi_reduce_interval;   /* interval for ratectrl
-                                                to reduce RSSI */
-        u8 initial_ratemax;   /* the initial ratemax value used
-                                        in ath_rc_sib_update() */
+        u32 probe_interval;
+        u32 rssi_reduce_interval;
+        u8 initial_ratemax;
 };
 
 #define ATH_RC_PROBE_ALLOWED            0x00000001
 #define ATH_RC_MINRATE_LASTRATE         0x00000002
-#define ATH_RC_SHORT_PREAMBLE           0x00000004
 
 struct ath_rc_series {
-        u8    rix;
-        u8    tries;
-        u8    flags;
-        u32   max_4ms_framelen;
+        u8 rix;
+        u8 tries;
+        u8 flags;
+        u32 max_4ms_framelen;
 };
 
 /* rcs_flags definition */
@@ -201,42 +201,56 @@ struct ath_rc_series {
 #define MAX_TX_RATE_PHY         48
 
 struct ath_tx_ratectrl_state {
-        int8_t rssi_thres; /* required rssi for this rate (dB) */
-        u8 per; /* recent estimate of packet error rate (%) */
+        int8_t rssi_thres;      /* required rssi for this rate (dB) */
+        u8 per;                 /* recent estimate of packet error rate (%) */
 };
 
+/**
+ * struct ath_tx_ratectrl - TX Rate control Information
+ * @state: RC state
+ * @rssi_last: last ACK rssi
+ * @rssi_last_lookup: last ACK rssi used for lookup
+ * @rssi_last_prev: previous last ACK rssi
+ * @rssi_last_prev2: 2nd previous last ACK rssi
+ * @rssi_sum_cnt: count of rssi_sum for averaging
+ * @rssi_sum_rate: rate that we are averaging
+ * @rssi_sum: running sum of rssi for averaging
+ * @probe_rate: rate we are probing at
+ * @rssi_time: msec timestamp for last ack rssi
+ * @rssi_down_time: msec timestamp for last down step
+ * @probe_time: msec timestamp for last probe
+ * @hw_maxretry_pktcnt: num of packets since we got HW max retry error
+ * @max_valid_rate: maximum number of valid rate
+ * @per_down_time: msec timestamp for last PER down step
+ * @valid_phy_ratecnt: valid rate count
+ * @rate_max_phy: phy index for the max rate
+ * @probe_interval: interval for ratectrl to probe for other rates
+ */
 struct ath_tx_ratectrl {
-        struct ath_tx_ratectrl_state state[MAX_TX_RATE_TBL]; /* state */
-        int8_t rssi_last;            /* last ack rssi */
-        int8_t rssi_last_lookup;        /* last ack rssi used for lookup */
-        int8_t rssi_last_prev;  /* previous last ack rssi */
-        int8_t rssi_last_prev2; /* 2nd previous last ack rssi */
-        int32_t rssi_sum_cnt;        /* count of rssi_sum for averaging */
-        int32_t rssi_sum_rate;       /* rate that we are averaging */
-        int32_t rssi_sum;           /* running sum of rssi for averaging */
-        u32 valid_txrate_mask;   /* mask of valid rates */
-        u8 rate_table_size;      /* rate table size */
-        u8 rate_max;            /* max rate that has recently worked */
-        u8 probe_rate;          /* rate we are probing at */
-        u32 rssi_time;          /* msec timestamp for last ack rssi */
-        u32 rssi_down_time;      /* msec timestamp for last down step */
-        u32 probe_time;         /* msec timestamp for last probe */
-        u8 hw_maxretry_pktcnt;   /* num packets since we got
-                                        HW max retry error */
-        u8 max_valid_rate;       /* maximum number of valid rate */
-        u8 valid_rate_index[MAX_TX_RATE_TBL]; /* valid rate index */
-        u32 per_down_time;       /* msec timstamp for last
-                                        PER down step */
+        struct ath_tx_ratectrl_state state[MAX_TX_RATE_TBL];
+        int8_t rssi_last;
+        int8_t rssi_last_lookup;
+        int8_t rssi_last_prev;
+        int8_t rssi_last_prev2;
+        int32_t rssi_sum_cnt;
+        int32_t rssi_sum_rate;
+        int32_t rssi_sum;
+        u8 rate_table_size;
+        u8 probe_rate;
+        u32 rssi_time;
+        u32 rssi_down_time;
+        u32 probe_time;
+        u8 hw_maxretry_pktcnt;
+        u8 max_valid_rate;
+        u8 valid_rate_index[MAX_TX_RATE_TBL];
+        u32 per_down_time;
 
         /* 11n state */
-        u8  valid_phy_ratecnt[WLAN_RC_PHY_MAX]; /* valid rate count */
-        u8  valid_phy_rateidx[WLAN_RC_PHY_MAX][MAX_TX_RATE_TBL];
-        u8  rc_phy_mode;
-        u8  rate_max_phy;        /* Phy index for the max rate */
-        u32 rate_max_lastused;   /* msec timstamp of when we
-                                        last used rateMaxPhy */
-        u32 probe_interval;     /* interval for ratectrl to probe
-                                        for other rates */
+        u8 valid_phy_ratecnt[WLAN_RC_PHY_MAX];
+        u8 valid_phy_rateidx[WLAN_RC_PHY_MAX][MAX_TX_RATE_TBL];
+        u8 rc_phy_mode;
+        u8 rate_max_phy;
+        u32 probe_interval;
 };
 
 struct ath_rateset {
@@ -248,29 +262,32 @@ struct ath_rateset {
 struct ath_rate_softc {
         /* phy tables that contain rate control data */
         const void *hw_rate_table[ATH9K_MODE_MAX];
-        int fixedrix;   /* -1 or index of fixed rate */
+
+        /* -1 or index of fixed rate */
+        int fixedrix;
 };
 
 /* per-node state */
 struct ath_rate_node {
-        struct ath_tx_ratectrl tx_ratectrl;     /* rate control state proper */
-        u32 prev_data_rix;      /* rate idx of last data frame */
+        struct ath_tx_ratectrl tx_ratectrl;
 
-        /* map of rate ix -> negotiated rate set ix */
-        u8 rixmap[MAX_TX_RATE_TBL];
+        /* rate idx of last data frame */
+        u32 prev_data_rix;
 
-        /* map of ht rate ix -> negotiated rate set ix */
-        u8 ht_rixmap[MAX_TX_RATE_TBL];
+        /* ht capabilities */
+        u8 ht_cap;
 
-        u8 ht_cap;              /* ht capabilities */
-        u8 ant_tx;              /* current transmit antenna */
+        /* When TRUE, only single stream Tx possible */
+        u8 single_stream;
 
-        u8 single_stream;   /* When TRUE, only single
-                                stream Tx possible */
-        struct ath_rateset neg_rates;   /* Negotiated rates */
-        struct ath_rateset neg_ht_rates;        /* Negotiated HT rates */
-        struct ath_rate_softc *asc; /* back pointer to atheros softc */
-        struct ath_vap *avp;    /* back pointer to vap */
+        /* Negotiated rates */
+        struct ath_rateset neg_rates;
+
+        /* Negotiated HT rates */
+        struct ath_rateset neg_ht_rates;
+
+        struct ath_rate_softc *asc;
+        struct ath_vap *avp;
 };
 
 /* Driver data of ieee80211_tx_info */
@@ -297,17 +314,10 @@ void ath_rc_node_update(struct ieee80211_hw *hw, struct ath_rate_node *rc_priv);
 void ath_rate_newstate(struct ath_softc *sc, struct ath_vap *avp);
 
 /*
- * Return the tx rate series.
- */
-void ath_rate_findrate(struct ath_softc *sc, struct ath_rate_node *ath_rc_priv,
-                       int num_tries, int num_rates,
-                       unsigned int rcflag, struct ath_rc_series[],
-                       int *is_probe, int isretry);
-/*
  * Return rate index for given Dot11 Rate.
  */
 u8 ath_rate_findrateix(struct ath_softc *sc,
-                             u8 dot11_rate);
+                       u8 dot11_rate);
 
 /* Routines to register/unregister rate control algorithm */
 int ath_rate_control_register(void);
diff --git a/drivers/net/wireless/ath9k/recv.c b/drivers/net/wireless/ath9k/recv.c
index 2fe806175c01..6e13c638cc0b 100644
--- a/drivers/net/wireless/ath9k/recv.c
+++ b/drivers/net/wireless/ath9k/recv.c
@@ -184,7 +184,7 @@ static int ath_ampdu_input(struct ath_softc *sc,
                 tid = qc[0] & 0xf;
         }
 
-        if (sc->sc_opmode == ATH9K_M_STA) {
+        if (sc->sc_ah->ah_opmode == ATH9K_M_STA) {
                 /* Drop the frame not belonging to me. */
                 if (memcmp(hdr->addr1, sc->sc_myaddr, ETH_ALEN)) {
                         dev_kfree_skb(skb);
@@ -448,17 +448,16 @@ static int ath_rx_indicate(struct ath_softc *sc,
         int type;
 
         /* indicate frame to the stack, which will free the old skb. */
-        type = ath__rx_indicate(sc, skb, status, keyix);
+        type = _ath_rx_indicate(sc, skb, status, keyix);
 
         /* allocate a new skb and queue it to for H/W processing */
         nskb = ath_rxbuf_alloc(sc, sc->sc_rxbufsize);
         if (nskb != NULL) {
                 bf->bf_mpdu = nskb;
-                bf->bf_buf_addr = ath_skb_map_single(sc,
-                        nskb,
-                        PCI_DMA_FROMDEVICE,
-                        /* XXX: Remove get_dma_mem_context() */
-                        get_dma_mem_context(bf, bf_dmacontext));
+                bf->bf_buf_addr = pci_map_single(sc->pdev, nskb->data,
+                                         skb_end_pointer(nskb) - nskb->head,
+                                         PCI_DMA_FROMDEVICE);
+                bf->bf_dmacontext = bf->bf_buf_addr;
                 ATH_RX_CONTEXT(nskb)->ctx_rxbuf = bf;
 
                 /* queue the new wbuf to H/W */
@@ -504,7 +503,7 @@ int ath_rx_init(struct ath_softc *sc, int nbufs)
 
         do {
                 spin_lock_init(&sc->sc_rxflushlock);
-                sc->sc_rxflush = 0;
+                sc->sc_flags &= ~SC_OP_RXFLUSH;
                 spin_lock_init(&sc->sc_rxbuflock);
 
                 /*
@@ -541,9 +540,10 @@ int ath_rx_init(struct ath_softc *sc, int nbufs)
                         }
 
                         bf->bf_mpdu = skb;
-                        bf->bf_buf_addr =
-                                ath_skb_map_single(sc, skb, PCI_DMA_FROMDEVICE,
-                                       get_dma_mem_context(bf, bf_dmacontext));
+                        bf->bf_buf_addr = pci_map_single(sc->pdev, skb->data,
+                                         skb_end_pointer(skb) - skb->head,
+                                         PCI_DMA_FROMDEVICE);
+                        bf->bf_dmacontext = bf->bf_buf_addr;
                         ATH_RX_CONTEXT(skb)->ctx_rxbuf = bf;
                 }
                 sc->sc_rxlink = NULL;
@@ -597,6 +597,7 @@ void ath_rx_cleanup(struct ath_softc *sc)
 u32 ath_calcrxfilter(struct ath_softc *sc)
 {
 #define RX_FILTER_PRESERVE (ATH9K_RX_FILTER_PHYERR | ATH9K_RX_FILTER_PHYRADAR)
+
         u32 rfilt;
 
         rfilt = (ath9k_hw_getrxfilter(sc->sc_ah) & RX_FILTER_PRESERVE)
@@ -604,25 +605,29 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
                 | ATH9K_RX_FILTER_MCAST;
 
         /* If not a STA, enable processing of Probe Requests */
-        if (sc->sc_opmode != ATH9K_M_STA)
+        if (sc->sc_ah->ah_opmode != ATH9K_M_STA)
                 rfilt |= ATH9K_RX_FILTER_PROBEREQ;
 
         /* Can't set HOSTAP into promiscous mode */
-        if (sc->sc_opmode == ATH9K_M_MONITOR) {
+        if (((sc->sc_ah->ah_opmode != ATH9K_M_HOSTAP) &&
+             (sc->rx_filter & FIF_PROMISC_IN_BSS)) ||
+            (sc->sc_ah->ah_opmode == ATH9K_M_MONITOR)) {
                 rfilt |= ATH9K_RX_FILTER_PROM;
                 /* ??? To prevent from sending ACK */
                 rfilt &= ~ATH9K_RX_FILTER_UCAST;
         }
 
-        if (sc->sc_opmode == ATH9K_M_STA || sc->sc_opmode == ATH9K_M_IBSS ||
-            sc->sc_scanning)
+        if (((sc->sc_ah->ah_opmode == ATH9K_M_STA) &&
+             (sc->rx_filter & FIF_BCN_PRBRESP_PROMISC)) ||
+            (sc->sc_ah->ah_opmode == ATH9K_M_IBSS))
                 rfilt |= ATH9K_RX_FILTER_BEACON;
 
         /* If in HOSTAP mode, want to enable reception of PSPOLL frames
            & beacon frames */
-        if (sc->sc_opmode == ATH9K_M_HOSTAP)
+        if (sc->sc_ah->ah_opmode == ATH9K_M_HOSTAP)
                 rfilt |= (ATH9K_RX_FILTER_BEACON | ATH9K_RX_FILTER_PSPOLL);
         return rfilt;
+
 #undef RX_FILTER_PRESERVE
 }
 
@@ -702,11 +707,11 @@ void ath_flushrecv(struct ath_softc *sc)
          * progress (see references to sc_rxflush)
          */
         spin_lock_bh(&sc->sc_rxflushlock);
-        sc->sc_rxflush = 1;
+        sc->sc_flags |= SC_OP_RXFLUSH;
 
         ath_rx_tasklet(sc, 1);
 
-        sc->sc_rxflush = 0;
+        sc->sc_flags &= ~SC_OP_RXFLUSH;
         spin_unlock_bh(&sc->sc_rxflushlock);
 }
 
@@ -719,7 +724,7 @@ int ath_rx_input(struct ath_softc *sc,
                  struct ath_recv_status *rx_status,
                  enum ATH_RX_TYPE *status)
 {
-        if (is_ampdu && sc->sc_rxaggr) {
+        if (is_ampdu && (sc->sc_flags & SC_OP_RXAGGR)) {
                 *status = ATH_RX_CONSUMED;
                 return ath_ampdu_input(sc, an, skb, rx_status);
         } else {
@@ -750,7 +755,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush)
 
         do {
                 /* If handling rx interrupt and flush is in progress => exit */
-                if (sc->sc_rxflush && (flush == 0))
+                if ((sc->sc_flags & SC_OP_RXFLUSH) && (flush == 0))
                         break;
 
                 spin_lock_bh(&sc->sc_rxbuflock);
@@ -900,7 +905,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush)
                          * Enable this if you want to see
                          * error frames in Monitor mode.
                          */
-                        if (sc->sc_opmode != ATH9K_M_MONITOR)
+                        if (sc->sc_ah->ah_opmode != ATH9K_M_MONITOR)
                                 goto rx_next;
 #endif
                         /* fall thru for monitor mode handling... */
@@ -945,7 +950,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush)
                          * decryption and MIC failures. For monitor mode,
                          * we also ignore the CRC error.
                          */
-                        if (sc->sc_opmode == ATH9K_M_MONITOR) {
+                        if (sc->sc_ah->ah_opmode == ATH9K_M_MONITOR) {
                                 if (ds->ds_rxstat.rs_status &
                                     ~(ATH9K_RXERR_DECRYPT | ATH9K_RXERR_MIC |
                                         ATH9K_RXERR_CRC))
@@ -1089,7 +1094,7 @@ rx_next:
                         "%s: Reset rx chain mask. "
                         "Do internal reset\n", __func__);
                 ASSERT(flush == 0);
-                ath_internal_reset(sc);
+                ath_reset(sc, false);
         }
 
         return 0;
@@ -1127,7 +1132,7 @@ int ath_rx_aggr_start(struct ath_softc *sc,
         rxtid = &an->an_aggr.rx.tid[tid];
 
         spin_lock_bh(&rxtid->tidlock);
-        if (sc->sc_rxaggr) {
+        if (sc->sc_flags & SC_OP_RXAGGR) {
                 /* Allow aggregation reception
                  * Adjust rx BA window size. Peer might indicate a
                  * zero buffer size for a _dont_care_ condition.
@@ -1227,7 +1232,7 @@ void ath_rx_aggr_teardown(struct ath_softc *sc,
 
 void ath_rx_node_init(struct ath_softc *sc, struct ath_node *an)
 {
-        if (sc->sc_rxaggr) {
+        if (sc->sc_flags & SC_OP_RXAGGR) {
                 struct ath_arx_tid *rxtid;
                 int tidno;
 
@@ -1259,7 +1264,7 @@ void ath_rx_node_init(struct ath_softc *sc, struct ath_node *an)
 
 void ath_rx_node_cleanup(struct ath_softc *sc, struct ath_node *an)
 {
-        if (sc->sc_rxaggr) {
+        if (sc->sc_flags & SC_OP_RXAGGR) {
                 struct ath_arx_tid *rxtid;
                 int tidno, i;
 
@@ -1292,27 +1297,3 @@ void ath_rx_node_free(struct ath_softc *sc, struct ath_node *an)
 {
         ath_rx_node_cleanup(sc, an);
 }
-
-dma_addr_t ath_skb_map_single(struct ath_softc *sc,
-                              struct sk_buff *skb,
-                              int direction,
-                              dma_addr_t *pa)
-{
-        /*
-         * NB: do NOT use skb->len, which is 0 on initialization.
-         * Use skb's entire data area instead.
-         */
-        *pa = pci_map_single(sc->pdev, skb->data,
-                skb_end_pointer(skb) - skb->head, direction);
-        return *pa;
-}
-
-void ath_skb_unmap_single(struct ath_softc *sc,
-                          struct sk_buff *skb,
-                          int direction,
-                          dma_addr_t *pa)
-{
-        /* Unmap skb's entire data area */
-        pci_unmap_single(sc->pdev, *pa,
-                skb_end_pointer(skb) - skb->head, direction);
-}
diff --git a/drivers/net/wireless/ath9k/reg.h b/drivers/net/wireless/ath9k/reg.h
index 42b0890a4685..60617ae66209 100644
--- a/drivers/net/wireless/ath9k/reg.h
+++ b/drivers/net/wireless/ath9k/reg.h
@@ -899,12 +899,6 @@ enum {
 #define AR_GPIO_OUTPUT_MUX2                      0x4064
 #define AR_GPIO_OUTPUT_MUX3                      0x4068
 
-#define AR_GPIO_OUTPUT_MUX_AS_OUTPUT             0
-#define AR_GPIO_OUTPUT_MUX_AS_PCIE_ATTENTION_LED 1
-#define AR_GPIO_OUTPUT_MUX_AS_PCIE_POWER_LED     2
-#define AR_GPIO_OUTPUT_MUX_AS_MAC_NETWORK_LED    5
-#define AR_GPIO_OUTPUT_MUX_AS_MAC_POWER_LED      6
-
 #define AR_INPUT_STATE                           0x406c
 
 #define AR_EEPROM_STATUS_DATA                    0x407c
diff --git a/drivers/net/wireless/ath9k/xmit.c b/drivers/net/wireless/ath9k/xmit.c
index 157f830ee6b8..3fc6641e8bf7 100644
--- a/drivers/net/wireless/ath9k/xmit.c
+++ b/drivers/net/wireless/ath9k/xmit.c
@@ -60,79 +60,6 @@ static u32 bits_per_symbol[][2] = {
 #define IS_HT_RATE(_rate)     ((_rate) & 0x80)
 
 /*
- * Insert a chain of ath_buf (descriptors) on a multicast txq
- * but do NOT start tx DMA on this queue.
- * NB: must be called with txq lock held
- */
-
-static void ath_tx_mcastqaddbuf(struct ath_softc *sc,
-                                struct ath_txq *txq,
-                                struct list_head *head)
-{
-        struct ath_hal *ah = sc->sc_ah;
-        struct ath_buf *bf;
-
-        if (list_empty(head))
-                return;
-
-        /*
-         * Insert the frame on the outbound list and
-         * pass it on to the hardware.
-         */
-        bf = list_first_entry(head, struct ath_buf, list);
-
-        /*
-         * The CAB queue is started from the SWBA handler since
-         * frames only go out on DTIM and to avoid possible races.
-         */
-        ath9k_hw_set_interrupts(ah, 0);
-
-        /*
-         * If there is anything in the mcastq, we want to set
-         * the "more data" bit in the last item in the queue to
-         * indicate that there is "more data". It makes sense to add
-         * it here since you are *always* going to have
-         * more data when adding to this queue, no matter where
-         * you call from.
-         */
-
-        if (txq->axq_depth) {
-                struct ath_buf *lbf;
-                struct ieee80211_hdr *hdr;
-
-                /*
-                 * Add the "more data flag" to the last frame
-                 */
-
-                lbf = list_entry(txq->axq_q.prev, struct ath_buf, list);
-                hdr = (struct ieee80211_hdr *)
-                        ((struct sk_buff *)(lbf->bf_mpdu))->data;
-                hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
-        }
-
-        /*
-         * Now, concat the frame onto the queue
-         */
-        list_splice_tail_init(head, &txq->axq_q);
-        txq->axq_depth++;
-        txq->axq_totalqueued++;
-        txq->axq_linkbuf = list_entry(txq->axq_q.prev, struct ath_buf, list);
-
-        DPRINTF(sc, ATH_DBG_QUEUE,
-                "%s: txq depth = %d\n", __func__, txq->axq_depth);
-        if (txq->axq_link != NULL) {
-                *txq->axq_link = bf->bf_daddr;
-                DPRINTF(sc, ATH_DBG_XMIT,
-                        "%s: link[%u](%p)=%llx (%p)\n",
-                        __func__,
-                        txq->axq_qnum, txq->axq_link,
-                        ito64(bf->bf_daddr), bf->bf_desc);
-        }
-        txq->axq_link = &(bf->bf_lastbf->bf_desc->ds_link);
-        ath9k_hw_set_interrupts(ah, sc->sc_imask);
-}
-
-/*
  * Insert a chain of ath_buf (descriptors) on a txq and
  * assume the descriptors are already chained together by caller.
  * NB: must be called with txq lock held
@@ -277,8 +204,6 @@ static int ath_tx_prepare(struct ath_softc *sc,
         __le16 fc;
         u8 *qc;
 
-        memset(txctl, 0, sizeof(struct ath_tx_control));
-
         txctl->dev = sc;
         hdr = (struct ieee80211_hdr *)skb->data;
         hdrlen = ieee80211_get_hdrlen_from_skb(skb);
@@ -302,7 +227,6 @@ static int ath_tx_prepare(struct ath_softc *sc,
         }
 
         txctl->if_id = 0;
-        txctl->nextfraglen = 0;
         txctl->frmlen = skb->len + FCS_LEN - (hdrlen & 3);
         txctl->txpower = MAX_RATE_POWER; /* FIXME */
 
@@ -329,12 +253,18 @@ static int ath_tx_prepare(struct ath_softc *sc,
 
         /* Fill qnum */
 
-        txctl->qnum = ath_get_hal_qnum(skb_get_queue_mapping(skb), sc);
-        txq = &sc->sc_txq[txctl->qnum];
+        if (unlikely(txctl->flags & ATH9K_TXDESC_CAB)) {
+                txctl->qnum = 0;
+                txq = sc->sc_cabq;
+        } else {
+                txctl->qnum = ath_get_hal_qnum(skb_get_queue_mapping(skb), sc);
+                txq = &sc->sc_txq[txctl->qnum];
+        }
         spin_lock_bh(&txq->axq_lock);
 
         /* Try to avoid running out of descriptors */
-        if (txq->axq_depth >= (ATH_TXBUF - 20)) {
+        if (txq->axq_depth >= (ATH_TXBUF - 20) &&
+            !(txctl->flags & ATH9K_TXDESC_CAB)) {
                 DPRINTF(sc, ATH_DBG_FATAL,
                         "%s: TX queue: %d is full, depth: %d\n",
                         __func__,
@@ -354,12 +284,12 @@ static int ath_tx_prepare(struct ath_softc *sc,
 
         /* Fill flags */
 
-        txctl->flags = ATH9K_TXDESC_CLRDMASK;    /* needed for crypto errors */
+        txctl->flags |= ATH9K_TXDESC_CLRDMASK; /* needed for crypto errors */
 
         if (tx_info->flags & IEEE80211_TX_CTL_NO_ACK)
-                tx_info->flags |= ATH9K_TXDESC_NOACK;
+                txctl->flags |= ATH9K_TXDESC_NOACK;
         if (tx_info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
-                tx_info->flags |= ATH9K_TXDESC_RTSENA;
+                txctl->flags |= ATH9K_TXDESC_RTSENA;
 
         /*
          * Setup for rate calculations.
@@ -392,7 +322,7 @@ static int ath_tx_prepare(struct ath_softc *sc,
                  * incremented by the fragmentation routine.
                  */
                 if (likely(!(txctl->flags & ATH9K_TXDESC_FRAG_IS_ON)) &&
-                        txctl->ht && sc->sc_txaggr) {
+                    txctl->ht && (sc->sc_flags & SC_OP_TXAGGR)) {
                         struct ath_atx_tid *tid;
 
                         tid = ATH_AN_2_TID(txctl->an, txctl->tidno);
@@ -413,50 +343,18 @@ static int ath_tx_prepare(struct ath_softc *sc,
         }
         rix = rcs[0].rix;
 
-        /*
-         * Calculate duration.  This logically belongs in the 802.11
-         * layer but it lacks sufficient information to calculate it.
-         */
-        if ((txctl->flags & ATH9K_TXDESC_NOACK) == 0 && !ieee80211_is_ctl(fc)) {
-                u16 dur;
+        if (ieee80211_has_morefrags(fc) ||
+            (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG)) {
                 /*
-                 * XXX not right with fragmentation.
-                 */
-                if (sc->sc_flags & ATH_PREAMBLE_SHORT)
-                        dur = rt->info[rix].spAckDuration;
-                else
-                        dur = rt->info[rix].lpAckDuration;
-
-                if (le16_to_cpu(hdr->frame_control) &
-                                IEEE80211_FCTL_MOREFRAGS) {
-                        dur += dur;  /* Add additional 'SIFS + ACK' */
-
-                        /*
-                        ** Compute size of next fragment in order to compute
-                        ** durations needed to update NAV.
-                        ** The last fragment uses the ACK duration only.
-                        ** Add time for next fragment.
-                        */
-                        dur += ath9k_hw_computetxtime(sc->sc_ah, rt,
-                                        txctl->nextfraglen,
-                                        rix, sc->sc_flags & ATH_PREAMBLE_SHORT);
-                }
-
-                if (ieee80211_has_morefrags(fc) ||
-                     (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_FRAG)) {
-                        /*
-                        **  Force hardware to use computed duration for next
-                        **  fragment by disabling multi-rate retry, which
-                        **  updates duration based on the multi-rate
-                        **  duration table.
-                        */
-                        rcs[1].tries = rcs[2].tries = rcs[3].tries = 0;
-                        rcs[1].rix = rcs[2].rix = rcs[3].rix = 0;
-                        /* reset tries but keep rate index */
-                        rcs[0].tries = ATH_TXMAXTRY;
-                }
-
-                hdr->duration_id = cpu_to_le16(dur);
+                **  Force hardware to use computed duration for next
+                **  fragment by disabling multi-rate retry, which
+                **  updates duration based on the multi-rate
+                **  duration table.
+                */
+                rcs[1].tries = rcs[2].tries = rcs[3].tries = 0;
+                rcs[1].rix = rcs[2].rix = rcs[3].rix = 0;
+                /* reset tries but keep rate index */
+                rcs[0].tries = ATH_TXMAXTRY;
         }
 
         /*
@@ -484,12 +382,8 @@ static int ath_tx_prepare(struct ath_softc *sc,
         if (is_multicast_ether_addr(hdr->addr1)) {
                 antenna = sc->sc_mcastantenna + 1;
                 sc->sc_mcastantenna = (sc->sc_mcastantenna + 1) & 0x1;
-        } else
-                antenna = sc->sc_txantenna;
+        }
 
-#ifdef USE_LEGACY_HAL
-        txctl->antenna = antenna;
-#endif
         return 0;
 }
 
@@ -502,7 +396,6 @@ static void ath_tx_complete_buf(struct ath_softc *sc,
 {
         struct sk_buff *skb = bf->bf_mpdu;
         struct ath_xmit_status tx_status;
-        dma_addr_t *pa;
 
         /*
          * Set retry information.
@@ -518,13 +411,12 @@ static void ath_tx_complete_buf(struct ath_softc *sc,
         if (!txok) {
                 tx_status.flags |= ATH_TX_ERROR;
 
-                if (bf->bf_isxretried)
+                if (bf_isxretried(bf))
                         tx_status.flags |= ATH_TX_XRETRY;
         }
         /* Unmap this frame */
-        pa = get_dma_mem_context(bf, bf_dmacontext);
         pci_unmap_single(sc->pdev,
-                         *pa,
+                         bf->bf_dmacontext,
                          skb->len,
                          PCI_DMA_TODEVICE);
         /* complete this frame */
@@ -629,7 +521,7 @@ static int ath_tx_num_badfrms(struct ath_softc *sc,
         if (isnodegone || ds->ds_txstat.ts_flags == ATH9K_TX_SW_ABORTED)
                 return 0;
 
-        isaggr = bf->bf_isaggr;
+        isaggr = bf_isaggr(bf);
         if (isaggr) {
                 seq_st = ATH_DS_BA_SEQ(ds);
                 memcpy(ba, ATH_DS_BA_BITMAP(ds), WME_BA_BMP_SIZE >> 3);
@@ -651,7 +543,7 @@ static void ath_tx_set_retry(struct ath_softc *sc, struct ath_buf *bf)
         struct sk_buff *skb;
         struct ieee80211_hdr *hdr;
 
-        bf->bf_isretried = 1;
+        bf->bf_state.bf_type |= BUF_RETRY;
         bf->bf_retries++;
 
         skb = bf->bf_mpdu;
@@ -698,7 +590,7 @@ static u32 ath_pkt_duration(struct ath_softc *sc,
         u8 rc;
         int streams, pktlen;
 
-        pktlen = bf->bf_isaggr ? bf->bf_al : bf->bf_frmlen;
+        pktlen = bf_isaggr(bf) ? bf->bf_al : bf->bf_frmlen;
         rc = rt->info[rix].rateCode;
 
         /*
@@ -742,7 +634,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
         int i, flags, rtsctsena = 0, dynamic_mimops = 0;
         u32 ctsduration = 0;
         u8 rix = 0, cix, ctsrate = 0;
-        u32 aggr_limit_with_rts = sc->sc_rtsaggrlimit;
+        u32 aggr_limit_with_rts = ah->ah_caps.rts_aggr_limit;
         struct ath_node *an = (struct ath_node *) bf->bf_node;
 
         /*
@@ -781,7 +673,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
          * let rate series flags determine which rates will actually
          * use RTS.
          */
-        if ((ah->ah_caps.hw_caps & ATH9K_HW_CAP_HT) && bf->bf_isdata) {
+        if ((ah->ah_caps.hw_caps & ATH9K_HW_CAP_HT) && bf_isdata(bf)) {
                 BUG_ON(!an);
                 /*
                  * 802.11g protection not needed, use our default behavior
@@ -793,7 +685,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                  * and the second aggregate should have any protection at all.
                  */
                 if (an->an_smmode == ATH_SM_PWRSAV_DYNAMIC) {
-                        if (!bf->bf_aggrburst) {
+                        if (!bf_isaggrburst(bf)) {
                                 flags = ATH9K_TXDESC_RTSENA;
                                 dynamic_mimops = 1;
                         } else {
@@ -806,7 +698,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
          * Set protection if aggregate protection on
          */
         if (sc->sc_config.ath_aggr_prot &&
-            (!bf->bf_isaggr || (bf->bf_isaggr && bf->bf_al < 8192))) {
+            (!bf_isaggr(bf) || (bf_isaggr(bf) && bf->bf_al < 8192))) {
                 flags = ATH9K_TXDESC_RTSENA;
                 cix = rt->info[sc->sc_protrix].controlRate;
                 rtsctsena = 1;
@@ -815,7 +707,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
         /*
          *  For AR5416 - RTS cannot be followed by a frame larger than 8K.
          */
-        if (bf->bf_isaggr && (bf->bf_al > aggr_limit_with_rts)) {
+        if (bf_isaggr(bf) && (bf->bf_al > aggr_limit_with_rts)) {
                 /*
                  * Ensure that in the case of SM Dynamic power save
                  * while we are bursting the second aggregate the
@@ -832,7 +724,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
         /* NB: cix is set above where RTS/CTS is enabled */
         BUG_ON(cix == 0xff);
         ctsrate = rt->info[cix].rateCode |
-                (bf->bf_shpreamble ? rt->info[cix].shortPreamble : 0);
+                (bf_isshpreamble(bf) ? rt->info[cix].shortPreamble : 0);
 
         /*
          * Setup HAL rate series
@@ -846,7 +738,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                 rix = bf->bf_rcs[i].rix;
 
                 series[i].Rate = rt->info[rix].rateCode |
-                        (bf->bf_shpreamble ? rt->info[rix].shortPreamble : 0);
+                        (bf_isshpreamble(bf) ? rt->info[rix].shortPreamble : 0);
 
                 series[i].Tries = bf->bf_rcs[i].tries;
 
@@ -862,7 +754,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                         sc, rix, bf,
                         (bf->bf_rcs[i].flags & ATH_RC_CW40_FLAG) != 0,
                         (bf->bf_rcs[i].flags & ATH_RC_SGI_FLAG),
-                        bf->bf_shpreamble);
+                        bf_isshpreamble(bf));
 
                 if ((an->an_smmode == ATH_SM_PWRSAV_STATIC) &&
                     (bf->bf_rcs[i].flags & ATH_RC_DS_FLAG) == 0) {
@@ -875,7 +767,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                          */
                         series[i].ChSel = sc->sc_tx_chainmask;
                 } else {
-                        if (bf->bf_ht)
+                        if (bf_isht(bf))
                                 series[i].ChSel =
                                         ath_chainmask_sel_logic(sc, an);
                         else
@@ -908,7 +800,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                  *     use the precalculated ACK durations.
                  */
                 if (flags & ATH9K_TXDESC_RTSENA) {    /* SIFS + CTS */
-                        ctsduration += bf->bf_shpreamble ?
+                        ctsduration += bf_isshpreamble(bf) ?
                                 rt->info[cix].spAckDuration :
                                 rt->info[cix].lpAckDuration;
                 }
@@ -916,7 +808,7 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
                 ctsduration += series[0].PktDuration;
 
                 if ((bf->bf_flags & ATH9K_TXDESC_NOACK) == 0) { /* SIFS + ACK */
-                        ctsduration += bf->bf_shpreamble ?
+                        ctsduration += bf_isshpreamble(bf) ?
                                 rt->info[rix].spAckDuration :
                                 rt->info[rix].lpAckDuration;
                 }
@@ -932,10 +824,10 @@ static void ath_buf_set_rate(struct ath_softc *sc, struct ath_buf *bf)
          * set dur_update_en for l-sig computation except for PS-Poll frames
          */
         ath9k_hw_set11n_ratescenario(ah, ds, lastds,
-                                    !bf->bf_ispspoll,
-                                    ctsrate,
-                                    ctsduration,
-                                    series, 4, flags);
+                                     !bf_ispspoll(bf),
+                                     ctsrate,
+                                     ctsduration,
+                                     series, 4, flags);
         if (sc->sc_config.ath_aggr_prot && flags)
                 ath9k_hw_set11n_burstduration(ah, ds, 8192);
 }
@@ -958,7 +850,7 @@ static int ath_tx_send_normal(struct ath_softc *sc,
         BUG_ON(list_empty(bf_head));
 
         bf = list_first_entry(bf_head, struct ath_buf, list);
-        bf->bf_isampdu = 0; /* regular HT frame */
+        bf->bf_state.bf_type &= ~BUF_AMPDU; /* regular HT frame */
 
         skb = (struct sk_buff *)bf->bf_mpdu;
         tx_info = IEEE80211_SKB_CB(skb);
@@ -998,7 +890,7 @@ static void ath_tx_flush_tid(struct ath_softc *sc, struct ath_atx_tid *tid)
 
         while (!list_empty(&tid->buf_q)) {
                 bf = list_first_entry(&tid->buf_q, struct ath_buf, list);
-                ASSERT(!bf->bf_isretried);
+                ASSERT(!bf_isretried(bf));
                 list_cut_position(&bf_head, &tid->buf_q, &bf->bf_lastfrm->list);
                 ath_tx_send_normal(sc, txq, tid, &bf_head);
         }
@@ -1025,7 +917,7 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
         int isaggr, txfail, txpending, sendbar = 0, needreset = 0;
         int isnodegone = (an->an_flags & ATH_NODE_CLEAN);
 
-        isaggr = bf->bf_isaggr;
+        isaggr = bf_isaggr(bf);
         if (isaggr) {
                 if (txok) {
                         if (ATH_DS_TX_BA(ds)) {
@@ -1047,7 +939,7 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
                                  * when perform internal reset in this routine.
                                  * Only enable reset in STA mode for now.
                                  */
-                                if (sc->sc_opmode == ATH9K_M_STA)
+                                if (sc->sc_ah->ah_opmode == ATH9K_M_STA)
                                         needreset = 1;
                         }
                 } else {
@@ -1075,7 +967,7 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
                                         ath_tx_set_retry(sc, bf);
                                         txpending = 1;
                                 } else {
-                                        bf->bf_isxretried = 1;
+                                        bf->bf_state.bf_type |= BUF_XRETRY;
                                         txfail = 1;
                                         sendbar = 1;
                                 }
@@ -1175,11 +1067,8 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
                                                 tbf->bf_lastfrm->bf_desc);
 
                                         /* copy the DMA context */
-                                        copy_dma_mem_context(
-                                                get_dma_mem_context(tbf,
-                                                        bf_dmacontext),
-                                                get_dma_mem_context(bf_last,
-                                                        bf_dmacontext));
+                                        tbf->bf_dmacontext =
+                                                bf_last->bf_dmacontext;
                                 }
                                 list_add_tail(&tbf->list, &bf_head);
                         } else {
@@ -1188,7 +1077,7 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
                                  * software retry
                                  */
                                 ath9k_hw_cleartxdesc(sc->sc_ah,
-                                        bf->bf_lastfrm->bf_desc);
+                                                     bf->bf_lastfrm->bf_desc);
                         }
 
                         /*
@@ -1242,7 +1131,7 @@ static void ath_tx_complete_aggr_rifs(struct ath_softc *sc,
         }
 
         if (needreset)
-                ath_internal_reset(sc);
+                ath_reset(sc, false);
 
         return;
 }
@@ -1331,7 +1220,7 @@ static int ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
 
                 txq->axq_depth--;
 
-                if (bf->bf_isaggr)
+                if (bf_isaggr(bf))
                         txq->axq_aggr_depth--;
 
                 txok = (ds->ds_txstat.ts_status == 0);
@@ -1345,14 +1234,14 @@ static int ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
                         spin_unlock_bh(&sc->sc_txbuflock);
                 }
 
-                if (!bf->bf_isampdu) {
+                if (!bf_isampdu(bf)) {
                         /*
                          * This frame is sent out as a single frame.
                          * Use hardware retry status for this frame.
                          */
                         bf->bf_retries = ds->ds_txstat.ts_longretry;
                         if (ds->ds_txstat.ts_status & ATH9K_TXERR_XRETRY)
-                                bf->bf_isxretried = 1;
+                                bf->bf_state.bf_type |= BUF_XRETRY;
                         nbad = 0;
                 } else {
                         nbad = ath_tx_num_badfrms(sc, bf, txok);
@@ -1368,7 +1257,7 @@ static int ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
                         if (ds->ds_txstat.ts_status == 0)
                                 nacked++;
 
-                        if (bf->bf_isdata) {
+                        if (bf_isdata(bf)) {
                                 if (isrifs)
                                         tmp_ds = bf->bf_rifslast->bf_desc;
                                 else
@@ -1384,7 +1273,7 @@ static int ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
                 /*
                  * Complete this transmit unit
                  */
-                if (bf->bf_isampdu)
+                if (bf_isampdu(bf))
                         ath_tx_complete_aggr_rifs(sc, txq, bf, &bf_head, txok);
                 else
                         ath_tx_complete_buf(sc, bf, &bf_head, txok, 0);
@@ -1406,7 +1295,7 @@ static int ath_tx_processq(struct ath_softc *sc, struct ath_txq *txq)
                 /*
                  * schedule any pending packets if aggregation is enabled
                  */
-                if (sc->sc_txaggr)
+                if (sc->sc_flags & SC_OP_TXAGGR)
                         ath_txq_schedule(sc, txq);
                 spin_unlock_bh(&txq->axq_lock);
         }
@@ -1430,10 +1319,9 @@ static void ath_drain_txdataq(struct ath_softc *sc, bool retry_tx)
         struct ath_hal *ah = sc->sc_ah;
         int i;
         int npend = 0;
-        enum ath9k_ht_macmode ht_macmode = ath_cwm_macmode(sc);
 
         /* XXX return value */
-        if (!sc->sc_invalid) {
+        if (!(sc->sc_flags & SC_OP_INVALID)) {
                 for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
                         if (ATH_TXQ_SETUP(sc, i)) {
                                 ath_tx_stopdma(sc, &sc->sc_txq[i]);
@@ -1454,10 +1342,11 @@ static void ath_drain_txdataq(struct ath_softc *sc, bool retry_tx)
                         "%s: Unable to stop TxDMA. Reset HAL!\n", __func__);
 
                 spin_lock_bh(&sc->sc_resetlock);
-                if (!ath9k_hw_reset(ah, sc->sc_opmode,
-                        &sc->sc_curchan, ht_macmode,
-                        sc->sc_tx_chainmask, sc->sc_rx_chainmask,
-                        sc->sc_ht_extprotspacing, true, &status)) {
+                if (!ath9k_hw_reset(ah,
+                                    sc->sc_ah->ah_curchan,
+                                    sc->sc_ht_info.tx_chan_width,
+                                    sc->sc_tx_chainmask, sc->sc_rx_chainmask,
+                                    sc->sc_ht_extprotspacing, true, &status)) {
 
                         DPRINTF(sc, ATH_DBG_FATAL,
                                 "%s: unable to reset hardware; hal status %u\n",
@@ -1481,7 +1370,7 @@ static void ath_tx_addto_baw(struct ath_softc *sc,
 {
         int index, cindex;
 
-        if (bf->bf_isretried)
+        if (bf_isretried(bf))
                 return;
 
         index  = ATH_BA_INDEX(tid->seq_start, bf->bf_seqno);
@@ -1516,7 +1405,7 @@ static int ath_tx_send_ampdu(struct ath_softc *sc,
         BUG_ON(list_empty(bf_head));
 
         bf = list_first_entry(bf_head, struct ath_buf, list);
-        bf->bf_isampdu = 1;
+        bf->bf_state.bf_type |= BUF_AMPDU;
         bf->bf_seqno = txctl->seqno; /* save seqno and tidno in buffer */
         bf->bf_tidno = txctl->tidno;
 
@@ -1860,7 +1749,7 @@ static void ath_tx_sched_aggr(struct ath_softc *sc,
                 if (bf->bf_nframes == 1) {
                         ASSERT(bf->bf_lastfrm == bf_last);
 
-                        bf->bf_isaggr = 0;
+                        bf->bf_state.bf_type &= ~BUF_AGGR;
                         /*
                          * clear aggr bits for every descriptor
                          * XXX TODO: is there a way to optimize it?
@@ -1877,7 +1766,7 @@ static void ath_tx_sched_aggr(struct ath_softc *sc,
                 /*
                  * setup first desc with rate and aggr info
                  */
-                bf->bf_isaggr  = 1;
+                bf->bf_state.bf_type |= BUF_AGGR;
                 ath_buf_set_rate(sc, bf);
                 ath9k_hw_set11n_aggr_first(sc->sc_ah, bf->bf_desc, bf->bf_al);
 
@@ -1925,7 +1814,7 @@ static void ath_tid_drain(struct ath_softc *sc,
                 list_cut_position(&bf_head, &tid->buf_q, &bf->bf_lastfrm->list);
 
                 /* update baw for software retried frame */
-                if (bf->bf_isretried)
+                if (bf_isretried(bf))
                         ath_tx_update_baw(sc, tid, bf->bf_seqno);
 
                 /*
@@ -1990,13 +1879,18 @@ static int ath_tx_start_dma(struct ath_softc *sc,
         struct list_head bf_head;
         struct ath_desc *ds;
         struct ath_hal *ah = sc->sc_ah;
-        struct ath_txq *txq = &sc->sc_txq[txctl->qnum];
+        struct ath_txq *txq;
         struct ath_tx_info_priv *tx_info_priv;
         struct ath_rc_series *rcs;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
         struct ieee80211_tx_info *tx_info =  IEEE80211_SKB_CB(skb);
         __le16 fc = hdr->frame_control;
 
+        if (unlikely(txctl->flags & ATH9K_TXDESC_CAB))
+                txq = sc->sc_cabq;
+        else
+                txq = &sc->sc_txq[txctl->qnum];
+
         /* For each sglist entry, allocate an ath_buf for DMA */
         INIT_LIST_HEAD(&bf_head);
         spin_lock_bh(&sc->sc_txbuflock);
@@ -2014,11 +1908,21 @@ static int ath_tx_start_dma(struct ath_softc *sc,
         /* set up this buffer */
         ATH_TXBUF_RESET(bf);
         bf->bf_frmlen = txctl->frmlen;
-        bf->bf_isdata = ieee80211_is_data(fc);
-        bf->bf_isbar = ieee80211_is_back_req(fc);
-        bf->bf_ispspoll = ieee80211_is_pspoll(fc);
+
+        ieee80211_is_data(fc) ?
+                (bf->bf_state.bf_type |= BUF_DATA) :
+                (bf->bf_state.bf_type &= ~BUF_DATA);
+        ieee80211_is_back_req(fc) ?
+                (bf->bf_state.bf_type |= BUF_BAR) :
+                (bf->bf_state.bf_type &= ~BUF_BAR);
+        ieee80211_is_pspoll(fc) ?
+                (bf->bf_state.bf_type |= BUF_PSPOLL) :
+                (bf->bf_state.bf_type &= ~BUF_PSPOLL);
+        (sc->sc_flags & SC_OP_PREAMBLE_SHORT) ?
+                (bf->bf_state.bf_type |= BUF_SHORT_PREAMBLE) :
+                (bf->bf_state.bf_type &= ~BUF_SHORT_PREAMBLE);
+
         bf->bf_flags = txctl->flags;
-        bf->bf_shpreamble = sc->sc_flags & ATH_PREAMBLE_SHORT;
         bf->bf_keytype = txctl->keytype;
         tx_info_priv = (struct ath_tx_info_priv *)tx_info->driver_data[0];
         rcs = tx_info_priv->rcs;
@@ -2038,8 +1942,7 @@ static int ath_tx_start_dma(struct ath_softc *sc,
         /*
          * Save the DMA context in the first ath_buf
          */
-        copy_dma_mem_context(get_dma_mem_context(bf, bf_dmacontext),
-                             get_dma_mem_context(txctl, dmacontext));
+        bf->bf_dmacontext = txctl->dmacontext;
 
         /*
          * Formulate first tx descriptor with tx controls.
@@ -2060,11 +1963,13 @@ static int ath_tx_start_dma(struct ath_softc *sc,
                             ds);                /* first descriptor */
 
         bf->bf_lastfrm = bf;
-        bf->bf_ht = txctl->ht;
+        (txctl->ht) ?
+                (bf->bf_state.bf_type |= BUF_HT) :
+                (bf->bf_state.bf_type &= ~BUF_HT);
 
         spin_lock_bh(&txq->axq_lock);
 
-        if (txctl->ht && sc->sc_txaggr) {
+        if (txctl->ht && (sc->sc_flags & SC_OP_TXAGGR)) {
                 struct ath_atx_tid *tid = ATH_AN_2_TID(an, txctl->tidno);
                 if (ath_aggr_query(sc, an, txctl->tidno)) {
                         /*
@@ -2090,27 +1995,7 @@ static int ath_tx_start_dma(struct ath_softc *sc,
                         bf->bf_tidno = txctl->tidno;
                 }
 
-                if (is_multicast_ether_addr(hdr->addr1)) {
-                        struct ath_vap *avp = sc->sc_vaps[txctl->if_id];
-
-                        /*
-                         * When servicing one or more stations in power-save
-                         * mode (or) if there is some mcast data waiting on
-                         * mcast queue (to prevent out of order delivery of
-                         * mcast,bcast packets) multicast frames must be
-                         * buffered until after the beacon. We use the private
-                         * mcast queue for that.
-                         */
-                        /* XXX? more bit in 802.11 frame header */
-                        spin_lock_bh(&avp->av_mcastq.axq_lock);
-                        if (txctl->ps || avp->av_mcastq.axq_depth)
-                                ath_tx_mcastqaddbuf(sc,
-                                        &avp->av_mcastq, &bf_head);
-                        else
-                                ath_tx_txqaddbuf(sc, txq, &bf_head);
-                        spin_unlock_bh(&avp->av_mcastq.axq_lock);
-                } else
-                        ath_tx_txqaddbuf(sc, txq, &bf_head);
+                ath_tx_txqaddbuf(sc, txq, &bf_head);
         }
         spin_unlock_bh(&txq->axq_lock);
         return 0;
@@ -2118,30 +2003,31 @@ static int ath_tx_start_dma(struct ath_softc *sc,
 
 static void xmit_map_sg(struct ath_softc *sc,
                         struct sk_buff *skb,
-                        dma_addr_t *pa,
                         struct ath_tx_control *txctl)
 {
         struct ath_xmit_status tx_status;
         struct ath_atx_tid *tid;
         struct scatterlist sg;
 
-        *pa = pci_map_single(sc->pdev, skb->data, skb->len, PCI_DMA_TODEVICE);
+        txctl->dmacontext = pci_map_single(sc->pdev, skb->data,
+                                           skb->len, PCI_DMA_TODEVICE);
 
         /* setup S/G list */
         memset(&sg, 0, sizeof(struct scatterlist));
-        sg_dma_address(&sg) = *pa;
+        sg_dma_address(&sg) = txctl->dmacontext;
         sg_dma_len(&sg) = skb->len;
 
         if (ath_tx_start_dma(sc, skb, &sg, 1, txctl) != 0) {
                 /*
                  *  We have to do drop frame here.
                  */
-                pci_unmap_single(sc->pdev, *pa, skb->len, PCI_DMA_TODEVICE);
+                pci_unmap_single(sc->pdev, txctl->dmacontext,
+                                 skb->len, PCI_DMA_TODEVICE);
 
                 tx_status.retries = 0;
                 tx_status.flags = ATH_TX_ERROR;
 
-                if (txctl->ht && sc->sc_txaggr) {
+                if (txctl->ht && (sc->sc_flags & SC_OP_TXAGGR)) {
                         /* Reclaim the seqno. */
                         tid = ATH_AN_2_TID((struct ath_node *)
                                 txctl->an, txctl->tidno);
@@ -2162,7 +2048,7 @@ int ath_tx_init(struct ath_softc *sc, int nbufs)
 
                 /* Setup tx descriptors */
                 error = ath_descdma_setup(sc, &sc->sc_txdma, &sc->sc_txbuf,
-                        "tx", nbufs * ATH_FRAG_PER_MSDU, ATH_TXDESC);
+                        "tx", nbufs, 1);
                 if (error != 0) {
                         DPRINTF(sc, ATH_DBG_FATAL,
                                 "%s: failed to allocate tx descriptors: %d\n",
@@ -2403,6 +2289,7 @@ int ath_tx_start(struct ath_softc *sc, struct sk_buff *skb)
         struct ath_tx_control txctl;
         int error = 0;
 
+        memset(&txctl, 0, sizeof(struct ath_tx_control));
         error = ath_tx_prepare(sc, skb, &txctl);
         if (error == 0)
                 /*
@@ -2410,9 +2297,7 @@ int ath_tx_start(struct ath_softc *sc, struct sk_buff *skb)
                  * ath_tx_start_dma() will be called either synchronously
                  * or asynchrounsly once DMA is complete.
                  */
-                xmit_map_sg(sc, skb,
-                            get_dma_mem_context(&txctl, dmacontext),
-                            &txctl);
+                xmit_map_sg(sc, skb, &txctl);
         else
                 ath_node_put(sc, txctl.an, ATH9K_BH_STATUS_CHANGE);
 
@@ -2424,8 +2309,7 @@ int ath_tx_start(struct ath_softc *sc, struct sk_buff *skb)
 
 void ath_tx_tasklet(struct ath_softc *sc)
 {
-        u64 tsf = ath9k_hw_gettsf64(sc->sc_ah);
-        int i, nacked = 0;
+        int i;
         u32 qcumask = ((1 << ATH9K_NUM_TX_QUEUES) - 1);
 
         ath9k_hw_gettxintrtxqs(sc->sc_ah, &qcumask);
@@ -2435,10 +2319,8 @@ void ath_tx_tasklet(struct ath_softc *sc)
          */
         for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
                 if (ATH_TXQ_SETUP(sc, i) && (qcumask & (1 << i)))
-                        nacked += ath_tx_processq(sc, &sc->sc_txq[i]);
+                        ath_tx_processq(sc, &sc->sc_txq[i]);
         }
-        if (nacked)
-                sc->sc_lastrx = tsf;
 }
 
 void ath_tx_draintxq(struct ath_softc *sc,
@@ -2486,14 +2368,14 @@ void ath_tx_draintxq(struct ath_softc *sc,
 
                 spin_unlock_bh(&txq->axq_lock);
 
-                if (bf->bf_isampdu)
+                if (bf_isampdu(bf))
                         ath_tx_complete_aggr_rifs(sc, txq, bf, &bf_head, 0);
                 else
                         ath_tx_complete_buf(sc, bf, &bf_head, 0, 0);
         }
 
         /* flush any pending frames if aggregation is enabled */
-        if (sc->sc_txaggr) {
+        if (sc->sc_flags & SC_OP_TXAGGR) {
                 if (!retry_tx) {
                         spin_lock_bh(&txq->axq_lock);
                         ath_txq_drain_pending_buffers(sc, txq,
@@ -2509,7 +2391,7 @@ void ath_draintxq(struct ath_softc *sc, bool retry_tx)
 {
         /* stop beacon queue. The beacon will be freed when
          * we go to INIT state */
-        if (!sc->sc_invalid) {
+        if (!(sc->sc_flags & SC_OP_INVALID)) {
                 (void) ath9k_hw_stoptxdma(sc->sc_ah, sc->sc_bhalq);
                 DPRINTF(sc, ATH_DBG_XMIT, "%s: beacon queue %x\n", __func__,
                         ath9k_hw_gettxbuf(sc->sc_ah, sc->sc_bhalq));
@@ -2536,7 +2418,7 @@ enum ATH_AGGR_CHECK ath_tx_aggr_check(struct ath_softc *sc,
         struct ath_atx_tid *txtid;
         DECLARE_MAC_BUF(mac);
 
-        if (!sc->sc_txaggr)
+        if (!(sc->sc_flags & SC_OP_TXAGGR))
                 return AGGR_NOT_REQUIRED;
 
         /* ADDBA exchange must be completed before sending aggregates */
@@ -2583,7 +2465,7 @@ int ath_tx_aggr_start(struct ath_softc *sc,
                 return -1;
         }
 
-        if (sc->sc_txaggr) {
+        if (sc->sc_flags & SC_OP_TXAGGR) {
                 txtid = ATH_AN_2_TID(an, tid);
                 txtid->addba_exchangeinprogress = 1;
                 ath_tx_pause_tid(sc, txtid);
@@ -2647,7 +2529,7 @@ void ath_tx_aggr_teardown(struct ath_softc *sc,
         spin_lock_bh(&txq->axq_lock);
         while (!list_empty(&txtid->buf_q)) {
                 bf = list_first_entry(&txtid->buf_q, struct ath_buf, list);
-                if (!bf->bf_isretried) {
+                if (!bf_isretried(bf)) {
                         /*
                          * NB: it's based on the assumption that
                          * software retried frame will always stay
@@ -2743,7 +2625,7 @@ void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq)
 
 void ath_tx_node_init(struct ath_softc *sc, struct ath_node *an)
 {
-        if (sc->sc_txaggr) {
+        if (sc->sc_flags & SC_OP_TXAGGR) {
                 struct ath_atx_tid *tid;
                 struct ath_atx_ac *ac;
                 int tidno, acno;
@@ -2855,7 +2737,7 @@ void ath_tx_node_cleanup(struct ath_softc *sc,
 
 void ath_tx_node_free(struct ath_softc *sc, struct ath_node *an)
 {
-        if (sc->sc_txaggr) {
+        if (sc->sc_flags & SC_OP_TXAGGR) {
                 struct ath_atx_tid *tid;
                 int tidno, i;
 
@@ -2869,3 +2751,57 @@ void ath_tx_node_free(struct ath_softc *sc, struct ath_node *an)
                 }
         }
 }
+
+void ath_tx_cabq(struct ath_softc *sc, struct sk_buff *skb)
+{
+        int hdrlen, padsize;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ath_tx_control txctl;
+
+        /*
+         * As a temporary workaround, assign seq# here; this will likely need
+         * to be cleaned up to work better with Beacon transmission and virtual
+         * BSSes.
+         */
+        if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+                struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+                if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
+                        sc->seq_no += 0x10;
+                hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
+                hdr->seq_ctrl |= cpu_to_le16(sc->seq_no);
+        }
+
+        /* Add the padding after the header if this is not already done */
+        hdrlen = ieee80211_get_hdrlen_from_skb(skb);
+        if (hdrlen & 3) {
+                padsize = hdrlen % 4;
+                if (skb_headroom(skb) < padsize) {
+                        DPRINTF(sc, ATH_DBG_XMIT, "%s: TX CABQ padding "
+                                "failed\n", __func__);
+                        dev_kfree_skb_any(skb);
+                        return;
+                }
+                skb_push(skb, padsize);
+                memmove(skb->data, skb->data + padsize, hdrlen);
+        }
+
+        DPRINTF(sc, ATH_DBG_XMIT, "%s: transmitting CABQ packet, skb: %p\n",
+                __func__,
+                skb);
+
+        memset(&txctl, 0, sizeof(struct ath_tx_control));
+        txctl.flags = ATH9K_TXDESC_CAB;
+        if (ath_tx_prepare(sc, skb, &txctl) == 0) {
+                /*
+                 * Start DMA mapping.
+                 * ath_tx_start_dma() will be called either synchronously
+                 * or asynchrounsly once DMA is complete.
+                 */
+                xmit_map_sg(sc, skb, &txctl);
+        } else {
+                ath_node_put(sc, txctl.an, ATH9K_BH_STATUS_CHANGE);
+                DPRINTF(sc, ATH_DBG_XMIT, "%s: TX CABQ failed\n", __func__);
+                dev_kfree_skb_any(skb);
+        }
+}
+
diff --git a/drivers/net/wireless/b43/Makefile b/drivers/net/wireless/b43/Makefile
index 8c52b0b9862a..fb6ffce03f0a 100644
--- a/drivers/net/wireless/b43/Makefile
+++ b/drivers/net/wireless/b43/Makefile
@@ -1,7 +1,9 @@
 b43-y                           += main.o
 b43-y                           += tables.o
 b43-$(CONFIG_B43_NPHY)          += tables_nphy.o
-b43-y                           += phy.o
+b43-y                           += phy_common.o
+b43-y                           += phy_g.o
+b43-y                           += phy_a.o
 b43-$(CONFIG_B43_NPHY)          += nphy.o
 b43-y                           += sysfs.o
 b43-y                           += xmit.o
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index edcdfa366452..f9c8161671d9 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -12,7 +12,7 @@
 #include "leds.h"
 #include "rfkill.h"
 #include "lo.h"
-#include "phy.h"
+#include "phy_common.h"
 
 
 /* The unique identifier of the firmware that's officially supported by
@@ -173,6 +173,11 @@ enum {
 #define B43_SHM_SH_CHAN                 0x00A0  /* Current channel (low 8bit only) */
 #define  B43_SHM_SH_CHAN_5GHZ           0x0100  /* Bit set, if 5Ghz channel */
 #define B43_SHM_SH_BCMCFIFOID           0x0108  /* Last posted cookie to the bcast/mcast FIFO */
+/* TSSI information */
+#define B43_SHM_SH_TSSI_CCK             0x0058  /* TSSI for last 4 CCK frames (32bit) */
+#define B43_SHM_SH_TSSI_OFDM_A          0x0068  /* TSSI for last 4 OFDM frames (32bit) */
+#define B43_SHM_SH_TSSI_OFDM_G          0x0070  /* TSSI for last 4 OFDM frames (32bit) */
+#define  B43_TSSI_MAX                   0x7F    /* Max value for one TSSI value */
 /* SHM_SHARED TX FIFO variables */
 #define B43_SHM_SH_SIZE01               0x0098  /* TX FIFO size for FIFO 0 (low) and 1 (high) */
 #define B43_SHM_SH_SIZE23               0x009A  /* TX FIFO size for FIFO 2 and 3 */
@@ -508,122 +513,6 @@ struct b43_iv {
 } __attribute__((__packed__));
 
 
-struct b43_phy {
-        /* Band support flags. */
-        bool supports_2ghz;
-        bool supports_5ghz;
-
-        /* GMODE bit enabled? */
-        bool gmode;
-
-        /* Analog Type */
-        u8 analog;
-        /* B43_PHYTYPE_ */
-        u8 type;
-        /* PHY revision number. */
-        u8 rev;
-
-        /* Radio versioning */
-        u16 radio_manuf;        /* Radio manufacturer */
-        u16 radio_ver;          /* Radio version */
-        u8 radio_rev;           /* Radio revision */
-
-        bool dyn_tssi_tbl;      /* tssi2dbm is kmalloc()ed. */
-
-        /* ACI (adjacent channel interference) flags. */
-        bool aci_enable;
-        bool aci_wlan_automatic;
-        bool aci_hw_rssi;
-
-        /* Radio switched on/off */
-        bool radio_on;
-        struct {
-                /* Values saved when turning the radio off.
-                 * They are needed when turning it on again. */
-                bool valid;
-                u16 rfover;
-                u16 rfoverval;
-        } radio_off_context;
-
-        u16 minlowsig[2];
-        u16 minlowsigpos[2];
-
-        /* TSSI to dBm table in use */
-        const s8 *tssi2dbm;
-        /* Target idle TSSI */
-        int tgt_idle_tssi;
-        /* Current idle TSSI */
-        int cur_idle_tssi;
-
-        /* LocalOscillator control values. */
-        struct b43_txpower_lo_control *lo_control;
-        /* Values from b43_calc_loopback_gain() */
-        s16 max_lb_gain;        /* Maximum Loopback gain in hdB */
-        s16 trsw_rx_gain;       /* TRSW RX gain in hdB */
-        s16 lna_lod_gain;       /* LNA lod */
-        s16 lna_gain;           /* LNA */
-        s16 pga_gain;           /* PGA */
-
-        /* Desired TX power level (in dBm).
-         * This is set by the user and adjusted in b43_phy_xmitpower(). */
-        u8 power_level;
-        /* A-PHY TX Power control value. */
-        u16 txpwr_offset;
-
-        /* Current TX power level attenuation control values */
-        struct b43_bbatt bbatt;
-        struct b43_rfatt rfatt;
-        u8 tx_control;          /* B43_TXCTL_XXX */
-
-        /* Hardware Power Control enabled? */
-        bool hardware_power_control;
-
-        /* Current Interference Mitigation mode */
-        int interfmode;
-        /* 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 B43_INTERFSTACK_SIZE    26
-        u32 interfstack[B43_INTERFSTACK_SIZE];  //FIXME: use a data structure
-
-        /* Saved values from the NRSSI Slope calculation */
-        s16 nrssi[2];
-        s32 nrssislope;
-        /* In memory nrssi lookup table. */
-        s8 nrssi_lt[64];
-
-        /* current channel */
-        u8 channel;
-
-        u16 lofcal;
-
-        u16 initval;            //FIXME rename?
-
-        /* PHY TX errors counter. */
-        atomic_t txerr_cnt;
-
-        /* The device does address auto increment for the OFDM tables.
-         * We cache the previously used address here and omit the address
-         * write on the next table access, if possible. */
-        u16 ofdmtab_addr; /* The address currently set in hardware. */
-        enum { /* The last data flow direction. */
-                B43_OFDMTAB_DIRECTION_UNKNOWN = 0,
-                B43_OFDMTAB_DIRECTION_READ,
-                B43_OFDMTAB_DIRECTION_WRITE,
-        } ofdmtab_addr_direction;
-
-#if B43_DEBUG
-        /* Manual TX-power control enabled? */
-        bool manual_txpower_control;
-        /* PHY registers locked by b43_phy_lock()? */
-        bool phy_locked;
-#endif /* B43_DEBUG */
-};
-
 /* Data structures for DMA transmission, per 80211 core. */
 struct b43_dma {
         struct b43_dmaring *tx_ring_AC_BK; /* Background */
@@ -764,6 +653,11 @@ struct b43_wl {
         struct b43_qos_params qos_params[4];
         /* Workqueue for updating QOS parameters in hardware. */
         struct work_struct qos_update_work;
+
+        /* Work for adjustment of the transmission power.
+         * This is scheduled when we determine that the actual TX output
+         * power doesn't match what we want. */
+        struct work_struct txpower_adjust_work;
 };
 
 /* In-memory representation of a cached microcode file. */
@@ -908,6 +802,15 @@ static inline int b43_is_mode(struct b43_wl *wl, int type)
         return (wl->operating && wl->if_type == type);
 }
 
+/**
+ * b43_current_band - Returns the currently used band.
+ * Returns one of IEEE80211_BAND_2GHZ and IEEE80211_BAND_5GHZ.
+ */
+static inline enum ieee80211_band b43_current_band(struct b43_wl *wl)
+{
+        return wl->hw->conf.channel->band;
+}
+
 static inline u16 b43_read16(struct b43_wldev *dev, u16 offset)
 {
         return ssb_read16(dev->dev, offset);
diff --git a/drivers/net/wireless/b43/debugfs.c b/drivers/net/wireless/b43/debugfs.c
index 29851bc1101f..06a01da80160 100644
--- a/drivers/net/wireless/b43/debugfs.c
+++ b/drivers/net/wireless/b43/debugfs.c
@@ -443,76 +443,6 @@ out_unlock:
         return count;
 }
 
-static ssize_t txpower_g_read_file(struct b43_wldev *dev,
-                                   char *buf, size_t bufsize)
-{
-        ssize_t count = 0;
-
-        if (dev->phy.type != B43_PHYTYPE_G) {
-                fappend("Device is not a G-PHY\n");
-                goto out;
-        }
-        fappend("Control:               %s\n", dev->phy.manual_txpower_control ?
-                "MANUAL" : "AUTOMATIC");
-        fappend("Baseband attenuation:  %u\n", dev->phy.bbatt.att);
-        fappend("Radio attenuation:     %u\n", dev->phy.rfatt.att);
-        fappend("TX Mixer Gain:         %s\n",
-                (dev->phy.tx_control & B43_TXCTL_TXMIX) ? "ON" : "OFF");
-        fappend("PA Gain 2dB:           %s\n",
-                (dev->phy.tx_control & B43_TXCTL_PA2DB) ? "ON" : "OFF");
-        fappend("PA Gain 3dB:           %s\n",
-                (dev->phy.tx_control & B43_TXCTL_PA3DB) ? "ON" : "OFF");
-        fappend("\n\n");
-        fappend("You can write to this file:\n");
-        fappend("Writing \"auto\" enables automatic txpower control.\n");
-        fappend
-            ("Writing the attenuation values as \"bbatt rfatt txmix pa2db pa3db\" "
-             "enables manual txpower control.\n");
-        fappend("Example: 5 4 0 0 1\n");
-        fappend("Enables manual control with Baseband attenuation 5, "
-                "Radio attenuation 4, No TX Mixer Gain, "
-                "No PA Gain 2dB, With PA Gain 3dB.\n");
-out:
-        return count;
-}
-
-static int txpower_g_write_file(struct b43_wldev *dev,
-                                const char *buf, size_t count)
-{
-        if (dev->phy.type != B43_PHYTYPE_G)
-                return -ENODEV;
-        if ((count >= 4) && (memcmp(buf, "auto", 4) == 0)) {
-                /* Automatic control */
-                dev->phy.manual_txpower_control = 0;
-                b43_phy_xmitpower(dev);
-        } else {
-                int bbatt = 0, rfatt = 0, txmix = 0, pa2db = 0, pa3db = 0;
-                /* Manual control */
-                if (sscanf(buf, "%d %d %d %d %d", &bbatt, &rfatt,
-                           &txmix, &pa2db, &pa3db) != 5)
-                        return -EINVAL;
-                b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
-                dev->phy.manual_txpower_control = 1;
-                dev->phy.bbatt.att = bbatt;
-                dev->phy.rfatt.att = rfatt;
-                dev->phy.tx_control = 0;
-                if (txmix)
-                        dev->phy.tx_control |= B43_TXCTL_TXMIX;
-                if (pa2db)
-                        dev->phy.tx_control |= B43_TXCTL_PA2DB;
-                if (pa3db)
-                        dev->phy.tx_control |= B43_TXCTL_PA3DB;
-                b43_phy_lock(dev);
-                b43_radio_lock(dev);
-                b43_set_txpower_g(dev, &dev->phy.bbatt,
-                                  &dev->phy.rfatt, dev->phy.tx_control);
-                b43_radio_unlock(dev);
-                b43_phy_unlock(dev);
-        }
-
-        return 0;
-}
-
 /* wl->irq_lock is locked */
 static int restart_write_file(struct b43_wldev *dev,
                               const char *buf, size_t count)
@@ -560,7 +490,7 @@ static ssize_t loctls_read_file(struct b43_wldev *dev,
                 err = -ENODEV;
                 goto out;
         }
-        lo = phy->lo_control;
+        lo = phy->g->lo_control;
         fappend("-- Local Oscillator calibration data --\n\n");
         fappend("HW-power-control enabled: %d\n",
                 dev->phy.hardware_power_control);
@@ -578,8 +508,8 @@ static ssize_t loctls_read_file(struct b43_wldev *dev,
         list_for_each_entry(cal, &lo->calib_list, list) {
                 bool active;
 
-                active = (b43_compare_bbatt(&cal->bbatt, &phy->bbatt) &&
-                          b43_compare_rfatt(&cal->rfatt, &phy->rfatt));
+                active = (b43_compare_bbatt(&cal->bbatt, &phy->g->bbatt) &&
+                          b43_compare_rfatt(&cal->rfatt, &phy->g->rfatt));
                 fappend("BB(%d), RF(%d,%d)  ->  I=%d, Q=%d  "
                         "(expires in %lu sec)%s\n",
                         cal->bbatt.att,
@@ -763,7 +693,6 @@ B43_DEBUGFS_FOPS(mmio32read, mmio32read__read_file, mmio32read__write_file, 1);
 B43_DEBUGFS_FOPS(mmio32write, NULL, mmio32write__write_file, 1);
 B43_DEBUGFS_FOPS(tsf, tsf_read_file, tsf_write_file, 1);
 B43_DEBUGFS_FOPS(txstat, txstat_read_file, NULL, 0);
-B43_DEBUGFS_FOPS(txpower_g, txpower_g_read_file, txpower_g_write_file, 0);
 B43_DEBUGFS_FOPS(restart, NULL, restart_write_file, 1);
 B43_DEBUGFS_FOPS(loctls, loctls_read_file, NULL, 0);
 
@@ -877,7 +806,6 @@ void b43_debugfs_add_device(struct b43_wldev *dev)
         ADD_FILE(mmio32write, 0200);
         ADD_FILE(tsf, 0600);
         ADD_FILE(txstat, 0400);
-        ADD_FILE(txpower_g, 0600);
         ADD_FILE(restart, 0200);
         ADD_FILE(loctls, 0400);
 
@@ -907,7 +835,6 @@ void b43_debugfs_remove_device(struct b43_wldev *dev)
         debugfs_remove(e->file_mmio32write.dentry);
         debugfs_remove(e->file_tsf.dentry);
         debugfs_remove(e->file_txstat.dentry);
-        debugfs_remove(e->file_txpower_g.dentry);
         debugfs_remove(e->file_restart.dentry);
         debugfs_remove(e->file_loctls.dentry);
 
diff --git a/drivers/net/wireless/b43/lo.c b/drivers/net/wireless/b43/lo.c
index 9c854d6aae36..6a18a1470465 100644
--- a/drivers/net/wireless/b43/lo.c
+++ b/drivers/net/wireless/b43/lo.c
@@ -29,7 +29,7 @@
 
 #include "b43.h"
 #include "lo.h"
-#include "phy.h"
+#include "phy_g.h"
 #include "main.h"
 
 #include <linux/delay.h>
@@ -174,7 +174,8 @@ static u16 lo_txctl_register_table(struct b43_wldev *dev,
 static void lo_measure_txctl_values(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
         u16 reg, mask;
         u16 trsw_rx, pga;
         u16 radio_pctl_reg;
@@ -195,7 +196,7 @@ static void lo_measure_txctl_values(struct b43_wldev *dev)
                 int lb_gain;    /* Loopback gain (in dB) */
 
                 trsw_rx = 0;
-                lb_gain = phy->max_lb_gain / 2;
+                lb_gain = gphy->max_lb_gain / 2;
                 if (lb_gain > 10) {
                         radio_pctl_reg = 0;
                         pga = abs(10 - lb_gain) / 6;
@@ -226,7 +227,7 @@ static void lo_measure_txctl_values(struct b43_wldev *dev)
         }
         b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
                                       & 0xFFF0) | radio_pctl_reg);
-        b43_phy_set_baseband_attenuation(dev, 2);
+        b43_gphy_set_baseband_attenuation(dev, 2);
 
         reg = lo_txctl_register_table(dev, &mask, NULL);
         mask = ~mask;
@@ -277,7 +278,8 @@ static void lo_measure_txctl_values(struct b43_wldev *dev)
 static void lo_read_power_vector(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
         int i;
         u64 tmp;
         u64 power_vector = 0;
@@ -298,6 +300,7 @@ static void lo_measure_gain_values(struct b43_wldev *dev,
                                    s16 max_rx_gain, int use_trsw_rx)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         u16 tmp;
 
         if (max_rx_gain < 0)
@@ -308,7 +311,7 @@ static void lo_measure_gain_values(struct b43_wldev *dev,
                 int trsw_rx_gain;
 
                 if (use_trsw_rx) {
-                        trsw_rx_gain = phy->trsw_rx_gain / 2;
+                        trsw_rx_gain = gphy->trsw_rx_gain / 2;
                         if (max_rx_gain >= trsw_rx_gain) {
                                 trsw_rx_gain = max_rx_gain - trsw_rx_gain;
                                 trsw_rx = 0x20;
@@ -316,38 +319,38 @@ static void lo_measure_gain_values(struct b43_wldev *dev,
                 } else
                         trsw_rx_gain = max_rx_gain;
                 if (trsw_rx_gain < 9) {
-                        phy->lna_lod_gain = 0;
+                        gphy->lna_lod_gain = 0;
                 } else {
-                        phy->lna_lod_gain = 1;
+                        gphy->lna_lod_gain = 1;
                         trsw_rx_gain -= 8;
                 }
                 trsw_rx_gain = clamp_val(trsw_rx_gain, 0, 0x2D);
-                phy->pga_gain = trsw_rx_gain / 3;
-                if (phy->pga_gain >= 5) {
-                        phy->pga_gain -= 5;
-                        phy->lna_gain = 2;
+                gphy->pga_gain = trsw_rx_gain / 3;
+                if (gphy->pga_gain >= 5) {
+                        gphy->pga_gain -= 5;
+                        gphy->lna_gain = 2;
                 } else
-                        phy->lna_gain = 0;
+                        gphy->lna_gain = 0;
         } else {
-                phy->lna_gain = 0;
-                phy->trsw_rx_gain = 0x20;
+                gphy->lna_gain = 0;
+                gphy->trsw_rx_gain = 0x20;
                 if (max_rx_gain >= 0x14) {
-                        phy->lna_lod_gain = 1;
-                        phy->pga_gain = 2;
+                        gphy->lna_lod_gain = 1;
+                        gphy->pga_gain = 2;
                 } else if (max_rx_gain >= 0x12) {
-                        phy->lna_lod_gain = 1;
-                        phy->pga_gain = 1;
+                        gphy->lna_lod_gain = 1;
+                        gphy->pga_gain = 1;
                 } else if (max_rx_gain >= 0xF) {
-                        phy->lna_lod_gain = 1;
-                        phy->pga_gain = 0;
+                        gphy->lna_lod_gain = 1;
+                        gphy->pga_gain = 0;
                 } else {
-                        phy->lna_lod_gain = 0;
-                        phy->pga_gain = 0;
+                        gphy->lna_lod_gain = 0;
+                        gphy->pga_gain = 0;
                 }
         }
 
         tmp = b43_radio_read16(dev, 0x7A);
-        if (phy->lna_lod_gain == 0)
+        if (gphy->lna_lod_gain == 0)
                 tmp &= ~0x0008;
         else
                 tmp |= 0x0008;
@@ -392,10 +395,11 @@ static void lo_measure_setup(struct b43_wldev *dev,
 {
         struct ssb_sprom *sprom = &dev->dev->bus->sprom;
         struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
         u16 tmp;
 
-        if (b43_has_hardware_pctl(phy)) {
+        if (b43_has_hardware_pctl(dev)) {
                 sav->phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
                 sav->phy_extg_01 = b43_phy_read(dev, B43_PHY_EXTG(0x01));
                 sav->phy_dacctl_hwpctl = b43_phy_read(dev, B43_PHY_DACCTL);
@@ -496,7 +500,7 @@ static void lo_measure_setup(struct b43_wldev *dev,
                 b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x0802);
         if (phy->rev >= 2)
                 b43_dummy_transmission(dev);
-        b43_radio_selectchannel(dev, 6, 0);
+        b43_gphy_channel_switch(dev, 6, 0);
         b43_radio_read16(dev, 0x51);    /* dummy read */
         if (phy->type == B43_PHYTYPE_G)
                 b43_phy_write(dev, B43_PHY_CCK(0x2F), 0);
@@ -520,18 +524,19 @@ static void lo_measure_restore(struct b43_wldev *dev,
                                struct lo_g_saved_values *sav)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         u16 tmp;
 
         if (phy->rev >= 2) {
                 b43_phy_write(dev, B43_PHY_PGACTL, 0xE300);
-                tmp = (phy->pga_gain << 8);
+                tmp = (gphy->pga_gain << 8);
                 b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA0);
                 udelay(5);
                 b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA2);
                 udelay(2);
                 b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA3);
         } else {
-                tmp = (phy->pga_gain | 0xEFA0);
+                tmp = (gphy->pga_gain | 0xEFA0);
                 b43_phy_write(dev, B43_PHY_PGACTL, tmp);
         }
         if (phy->type == B43_PHYTYPE_G) {
@@ -572,7 +577,7 @@ static void lo_measure_restore(struct b43_wldev *dev,
                 b43_phy_write(dev, B43_PHY_CCK(0x3E), sav->phy_cck_3E);
                 b43_phy_write(dev, B43_PHY_CRS0, sav->phy_crs0);
         }
-        if (b43_has_hardware_pctl(phy)) {
+        if (b43_has_hardware_pctl(dev)) {
                 tmp = (sav->phy_lo_mask & 0xBFFF);
                 b43_phy_write(dev, B43_PHY_LO_MASK, tmp);
                 b43_phy_write(dev, B43_PHY_EXTG(0x01), sav->phy_extg_01);
@@ -580,7 +585,7 @@ static void lo_measure_restore(struct b43_wldev *dev,
                 b43_phy_write(dev, B43_PHY_CCK(0x14), sav->phy_cck_14);
                 b43_phy_write(dev, B43_PHY_HPWR_TSSICTL, sav->phy_hpwr_tssictl);
         }
-        b43_radio_selectchannel(dev, sav->old_channel, 1);
+        b43_gphy_channel_switch(dev, sav->old_channel, 1);
 }
 
 struct b43_lo_g_statemachine {
@@ -597,6 +602,7 @@ static int lo_probe_possible_loctls(struct b43_wldev *dev,
                                     struct b43_lo_g_statemachine *d)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         struct b43_loctl test_loctl;
         struct b43_loctl orig_loctl;
         struct b43_loctl prev_loctl = {
@@ -646,9 +652,9 @@ static int lo_probe_possible_loctls(struct b43_wldev *dev,
                      test_loctl.q != prev_loctl.q) &&
                     (abs(test_loctl.i) <= 16 && abs(test_loctl.q) <= 16)) {
                         b43_lo_write(dev, &test_loctl);
-                        feedth = lo_measure_feedthrough(dev, phy->lna_gain,
-                                                        phy->pga_gain,
-                                                        phy->trsw_rx_gain);
+                        feedth = lo_measure_feedthrough(dev, gphy->lna_gain,
+                                                        gphy->pga_gain,
+                                                        gphy->trsw_rx_gain);
                         if (feedth < d->lowest_feedth) {
                                 memcpy(probe_loctl, &test_loctl,
                                        sizeof(struct b43_loctl));
@@ -677,6 +683,7 @@ static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
                                          int *max_rx_gain)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         struct b43_lo_g_statemachine d;
         u16 feedth;
         int found_lower;
@@ -693,17 +700,17 @@ static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
                 max_repeat = 4;
         do {
                 b43_lo_write(dev, &d.min_loctl);
-                feedth = lo_measure_feedthrough(dev, phy->lna_gain,
-                                                phy->pga_gain,
-                                                phy->trsw_rx_gain);
+                feedth = lo_measure_feedthrough(dev, gphy->lna_gain,
+                                                gphy->pga_gain,
+                                                gphy->trsw_rx_gain);
                 if (feedth < 0x258) {
                         if (feedth >= 0x12C)
                                 *max_rx_gain += 6;
                         else
                                 *max_rx_gain += 3;
-                        feedth = lo_measure_feedthrough(dev, phy->lna_gain,
-                                                        phy->pga_gain,
-                                                        phy->trsw_rx_gain);
+                        feedth = lo_measure_feedthrough(dev, gphy->lna_gain,
+                                                        gphy->pga_gain,
+                                                        gphy->trsw_rx_gain);
                 }
                 d.lowest_feedth = feedth;
 
@@ -752,6 +759,7 @@ struct b43_lo_calib * b43_calibrate_lo_setting(struct b43_wldev *dev,
                                                const struct b43_rfatt *rfatt)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         struct b43_loctl loctl = {
                 .i = 0,
                 .q = 0,
@@ -782,11 +790,11 @@ struct b43_lo_calib * b43_calibrate_lo_setting(struct b43_wldev *dev,
         if (rfatt->with_padmix)
                 max_rx_gain -= pad_mix_gain;
         if (has_loopback_gain(phy))
-                max_rx_gain += phy->max_lb_gain;
+                max_rx_gain += gphy->max_lb_gain;
         lo_measure_gain_values(dev, max_rx_gain,
                                has_loopback_gain(phy));
 
-        b43_phy_set_baseband_attenuation(dev, bbatt->att);
+        b43_gphy_set_baseband_attenuation(dev, bbatt->att);
         lo_probe_loctls_statemachine(dev, &loctl, &max_rx_gain);
 
         lo_measure_restore(dev, &saved_regs);
@@ -820,7 +828,7 @@ struct b43_lo_calib * b43_get_calib_lo_settings(struct b43_wldev *dev,
                                                 const struct b43_bbatt *bbatt,
                                                 const struct b43_rfatt *rfatt)
 {
-        struct b43_txpower_lo_control *lo = dev->phy.lo_control;
+        struct b43_txpower_lo_control *lo = dev->phy.g->lo_control;
         struct b43_lo_calib *c;
 
         c = b43_find_lo_calib(lo, bbatt, rfatt);
@@ -839,7 +847,8 @@ struct b43_lo_calib * b43_get_calib_lo_settings(struct b43_wldev *dev,
 void b43_gphy_dc_lt_init(struct b43_wldev *dev, bool update_all)
 {
         struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
         int i;
         int rf_offset, bb_offset;
         const struct b43_rfatt *rfatt;
@@ -917,14 +926,14 @@ static inline void b43_lo_fixup_rfatt(struct b43_rfatt *rf)
 
 void b43_lo_g_adjust(struct b43_wldev *dev)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = dev->phy.g;
         struct b43_lo_calib *cal;
         struct b43_rfatt rf;
 
-        memcpy(&rf, &phy->rfatt, sizeof(rf));
+        memcpy(&rf, &gphy->rfatt, sizeof(rf));
         b43_lo_fixup_rfatt(&rf);
 
-        cal = b43_get_calib_lo_settings(dev, &phy->bbatt, &rf);
+        cal = b43_get_calib_lo_settings(dev, &gphy->bbatt, &rf);
         if (!cal)
                 return;
         b43_lo_write(dev, &cal->ctl);
@@ -952,7 +961,8 @@ void b43_lo_g_adjust_to(struct b43_wldev *dev,
 void b43_lo_g_maintanance_work(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
         unsigned long now;
         unsigned long expire;
         struct b43_lo_calib *cal, *tmp;
@@ -962,7 +972,7 @@ void b43_lo_g_maintanance_work(struct b43_wldev *dev)
         if (!lo)
                 return;
         now = jiffies;
-        hwpctl = b43_has_hardware_pctl(phy);
+        hwpctl = b43_has_hardware_pctl(dev);
 
         if (hwpctl) {
                 /* Read the power vector and update it, if needed. */
@@ -983,8 +993,8 @@ void b43_lo_g_maintanance_work(struct b43_wldev *dev)
                 if (!time_before(cal->calib_time, expire))
                         continue;
                 /* This item expired. */
-                if (b43_compare_bbatt(&cal->bbatt, &phy->bbatt) &&
-                    b43_compare_rfatt(&cal->rfatt, &phy->rfatt)) {
+                if (b43_compare_bbatt(&cal->bbatt, &gphy->bbatt) &&
+                    b43_compare_rfatt(&cal->rfatt, &gphy->rfatt)) {
                         B43_WARN_ON(current_item_expired);
                         current_item_expired = 1;
                 }
@@ -1002,7 +1012,7 @@ void b43_lo_g_maintanance_work(struct b43_wldev *dev)
                 /* Recalibrate currently used LO setting. */
                 if (b43_debug(dev, B43_DBG_LO))
                         b43dbg(dev->wl, "LO: Recalibrating current LO setting\n");
-                cal = b43_calibrate_lo_setting(dev, &phy->bbatt, &phy->rfatt);
+                cal = b43_calibrate_lo_setting(dev, &gphy->bbatt, &gphy->rfatt);
                 if (cal) {
                         list_add(&cal->list, &lo->calib_list);
                         b43_lo_write(dev, &cal->ctl);
@@ -1013,7 +1023,7 @@ void b43_lo_g_maintanance_work(struct b43_wldev *dev)
 
 void b43_lo_g_cleanup(struct b43_wldev *dev)
 {
-        struct b43_txpower_lo_control *lo = dev->phy.lo_control;
+        struct b43_txpower_lo_control *lo = dev->phy.g->lo_control;
         struct b43_lo_calib *cal, *tmp;
 
         if (!lo)
@@ -1027,9 +1037,7 @@ void b43_lo_g_cleanup(struct b43_wldev *dev)
 /* LO Initialization */
 void b43_lo_g_init(struct b43_wldev *dev)
 {
-        struct b43_phy *phy = &dev->phy;
-
-        if (b43_has_hardware_pctl(phy)) {
+        if (b43_has_hardware_pctl(dev)) {
                 lo_read_power_vector(dev);
                 b43_gphy_dc_lt_init(dev, 1);
         }
diff --git a/drivers/net/wireless/b43/lo.h b/drivers/net/wireless/b43/lo.h
index 1da321cabc12..3b27e20eff80 100644
--- a/drivers/net/wireless/b43/lo.h
+++ b/drivers/net/wireless/b43/lo.h
@@ -1,7 +1,9 @@
 #ifndef B43_LO_H_
 #define B43_LO_H_
 
-#include "phy.h"
+/* G-PHY Local Oscillator */
+
+#include "phy_g.h"
 
 struct b43_wldev;
 
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 7205a936ec74..63bafc2f3f0a 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -44,7 +44,8 @@
 #include "b43.h"
 #include "main.h"
 #include "debugfs.h"
-#include "phy.h"
+#include "phy_common.h"
+#include "phy_g.h"
 #include "nphy.h"
 #include "dma.h"
 #include "pio.h"
@@ -1174,6 +1175,8 @@ static void b43_calculate_link_quality(struct b43_wldev *dev)
 {
         /* Top half of Link Quality calculation. */
 
+        if (dev->phy.type != B43_PHYTYPE_G)
+                return;
         if (dev->noisecalc.calculation_running)
                 return;
         dev->noisecalc.calculation_running = 1;
@@ -1184,7 +1187,7 @@ static void b43_calculate_link_quality(struct b43_wldev *dev)
 
 static void handle_irq_noise(struct b43_wldev *dev)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *phy = dev->phy.g;
         u16 tmp;
         u8 noise[4];
         u8 i, j;
@@ -1192,6 +1195,9 @@ static void handle_irq_noise(struct b43_wldev *dev)
 
         /* Bottom half of Link Quality calculation. */
 
+        if (dev->phy.type != B43_PHYTYPE_G)
+                return;
+
         /* Possible race condition: It might be possible that the user
          * changed to a different channel in the meantime since we
          * started the calculation. We ignore that fact, since it's
@@ -2688,9 +2694,7 @@ static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
 /* This is the opposite of b43_chip_init() */
 static void b43_chip_exit(struct b43_wldev *dev)
 {
-        b43_radio_turn_off(dev, 1);
         b43_gpio_cleanup(dev);
-        b43_lo_g_cleanup(dev);
         /* firmware is released later */
 }
 
@@ -2700,7 +2704,7 @@ static void b43_chip_exit(struct b43_wldev *dev)
 static int b43_chip_init(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
-        int err, tmp;
+        int err;
         u32 value32, macctl;
         u16 value16;
 
@@ -2725,19 +2729,19 @@ static int b43_chip_init(struct b43_wldev *dev)
         err = b43_upload_initvals(dev);
         if (err)
                 goto err_gpio_clean;
-        b43_radio_turn_on(dev);
 
         b43_write16(dev, 0x03E6, 0x0000);
         err = b43_phy_init(dev);
         if (err)
-                goto err_radio_off;
+                goto err_gpio_clean;
 
-        /* Select initial Interference Mitigation. */
-        tmp = phy->interfmode;
-        phy->interfmode = B43_INTERFMODE_NONE;
-        b43_radio_set_interference_mitigation(dev, tmp);
+        /* Disable Interference Mitigation. */
+        if (phy->ops->interf_mitigation)
+                phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
 
-        b43_set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
+        /* Select the antennae */
+        if (phy->ops->set_rx_antenna)
+                phy->ops->set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
         b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
 
         if (phy->type == B43_PHYTYPE_B) {
@@ -2790,8 +2794,6 @@ static int b43_chip_init(struct b43_wldev *dev)
 out:
         return err;
 
-err_radio_off:
-        b43_radio_turn_off(dev, 1);
 err_gpio_clean:
         b43_gpio_cleanup(dev);
         return err;
@@ -2799,25 +2801,13 @@ err_gpio_clean:
 
 static void b43_periodic_every60sec(struct b43_wldev *dev)
 {
-        struct b43_phy *phy = &dev->phy;
+        const struct b43_phy_operations *ops = dev->phy.ops;
 
-        if (phy->type != B43_PHYTYPE_G)
-                return;
-        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI) {
-                b43_mac_suspend(dev);
-                b43_calc_nrssi_slope(dev);
-                if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
-                        u8 old_chan = phy->channel;
-
-                        /* VCO Calibration */
-                        if (old_chan >= 8)
-                                b43_radio_selectchannel(dev, 1, 0);
-                        else
-                                b43_radio_selectchannel(dev, 13, 0);
-                        b43_radio_selectchannel(dev, old_chan, 0);
-                }
-                b43_mac_enable(dev);
-        }
+        if (ops->pwork_60sec)
+                ops->pwork_60sec(dev);
+
+        /* Force check the TX power emission now. */
+        b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME);
 }
 
 static void b43_periodic_every30sec(struct b43_wldev *dev)
@@ -2845,32 +2835,8 @@ static void b43_periodic_every15sec(struct b43_wldev *dev)
                 }
         }
 
-        if (phy->type == B43_PHYTYPE_G) {
-                //TODO: update_aci_moving_average
-                if (phy->aci_enable && phy->aci_wlan_automatic) {
-                        b43_mac_suspend(dev);
-                        if (!phy->aci_enable && 1 /*TODO: not scanning? */ ) {
-                                if (0 /*TODO: bunch of conditions */ ) {
-                                        b43_radio_set_interference_mitigation
-                                            (dev, B43_INTERFMODE_MANUALWLAN);
-                                }
-                        } else if (1 /*TODO*/) {
-                                /*
-                                   if ((aci_average > 1000) && !(b43_radio_aci_scan(dev))) {
-                                   b43_radio_set_interference_mitigation(dev,
-                                   B43_INTERFMODE_NONE);
-                                   }
-                                 */
-                        }
-                        b43_mac_enable(dev);
-                } else if (phy->interfmode == B43_INTERFMODE_NONWLAN &&
-                           phy->rev == 1) {
-                        //TODO: implement rev1 workaround
-                }
-        }
-        b43_phy_xmitpower(dev); //FIXME: unless scanning?
-        b43_lo_g_maintanance_work(dev);
-        //TODO for APHY (temperature?)
+        if (phy->ops->pwork_15sec)
+                phy->ops->pwork_15sec(dev);
 
         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
         wmb();
@@ -3401,7 +3367,7 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
         /* Switch to the requested channel.
          * The firmware takes care of races with the TX handler. */
         if (conf->channel->hw_value != phy->channel)
-                b43_radio_selectchannel(dev, conf->channel->hw_value, 0);
+                b43_switch_channel(dev, conf->channel->hw_value);
 
         /* Enable/Disable ShortSlot timing. */
         if ((!!(conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)) !=
@@ -3417,17 +3383,21 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 
         /* Adjust the desired TX power level. */
         if (conf->power_level != 0) {
-                if (conf->power_level != phy->power_level) {
-                        phy->power_level = conf->power_level;
-                        b43_phy_xmitpower(dev);
+                spin_lock_irqsave(&wl->irq_lock, flags);
+                if (conf->power_level != phy->desired_txpower) {
+                        phy->desired_txpower = conf->power_level;
+                        b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME |
+                                                   B43_TXPWR_IGNORE_TSSI);
                 }
+                spin_unlock_irqrestore(&wl->irq_lock, flags);
         }
 
         /* Antennas for RX and management frame TX. */
         antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_tx);
         b43_mgmtframe_txantenna(dev, antenna);
         antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_rx);
-        b43_set_rx_antenna(dev, antenna);
+        if (phy->ops->set_rx_antenna)
+                phy->ops->set_rx_antenna(dev, antenna);
 
         /* Update templates for AP/mesh mode. */
         if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
@@ -3436,7 +3406,7 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 
         if (!!conf->radio_enabled != phy->radio_on) {
                 if (conf->radio_enabled) {
-                        b43_radio_turn_on(dev);
+                        b43_software_rfkill(dev, RFKILL_STATE_UNBLOCKED);
                         b43info(dev->wl, "Radio turned on by software\n");
                         if (!dev->radio_hw_enable) {
                                 b43info(dev->wl, "The hardware RF-kill button "
@@ -3444,7 +3414,7 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
                                         "Press the button to turn it on.\n");
                         }
                 } else {
-                        b43_radio_turn_off(dev, 0);
+                        b43_software_rfkill(dev, RFKILL_STATE_SOFT_BLOCKED);
                         b43info(dev->wl, "Radio turned off by software\n");
                 }
         }
@@ -3818,48 +3788,10 @@ static int b43_phy_versioning(struct b43_wldev *dev)
 static void setup_struct_phy_for_init(struct b43_wldev *dev,
                                       struct b43_phy *phy)
 {
-        struct b43_txpower_lo_control *lo;
-        int i;
-
-        memset(phy->minlowsig, 0xFF, sizeof(phy->minlowsig));
-        memset(phy->minlowsigpos, 0, sizeof(phy->minlowsigpos));
-
-        phy->aci_enable = 0;
-        phy->aci_wlan_automatic = 0;
-        phy->aci_hw_rssi = 0;
-
-        phy->radio_off_context.valid = 0;
-
-        lo = phy->lo_control;
-        if (lo) {
-                memset(lo, 0, sizeof(*(phy->lo_control)));
-                lo->tx_bias = 0xFF;
-                INIT_LIST_HEAD(&lo->calib_list);
-        }
-        phy->max_lb_gain = 0;
-        phy->trsw_rx_gain = 0;
-        phy->txpwr_offset = 0;
-
-        /* NRSSI */
-        phy->nrssislope = 0;
-        for (i = 0; i < ARRAY_SIZE(phy->nrssi); i++)
-                phy->nrssi[i] = -1000;
-        for (i = 0; i < ARRAY_SIZE(phy->nrssi_lt); i++)
-                phy->nrssi_lt[i] = i;
-
-        phy->lofcal = 0xFFFF;
-        phy->initval = 0xFFFF;
-
-        phy->interfmode = B43_INTERFMODE_NONE;
-        phy->channel = 0xFF;
-
         phy->hardware_power_control = !!modparam_hwpctl;
-
+        phy->next_txpwr_check_time = jiffies;
         /* PHY TX errors counter. */
         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
-
-        /* OFDM-table address caching. */
-        phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
 }
 
 static void setup_struct_wldev_for_init(struct b43_wldev *dev)
@@ -3995,7 +3927,6 @@ static void b43_set_pretbtt(struct b43_wldev *dev)
 /* Locking: wl->mutex */
 static void b43_wireless_core_exit(struct b43_wldev *dev)
 {
-        struct b43_phy *phy = &dev->phy;
         u32 macctl;
 
         B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
@@ -4016,16 +3947,12 @@ static void b43_wireless_core_exit(struct b43_wldev *dev)
         b43_dma_free(dev);
         b43_pio_free(dev);
         b43_chip_exit(dev);
-        b43_radio_turn_off(dev, 1);
         b43_switch_analog(dev, 0);
-        if (phy->dyn_tssi_tbl)
-                kfree(phy->tssi2dbm);
-        kfree(phy->lo_control);
-        phy->lo_control = NULL;
         if (dev->wl->current_beacon) {
                 dev_kfree_skb_any(dev->wl->current_beacon);
                 dev->wl->current_beacon = NULL;
         }
+        b43_phy_exit(dev);
 
         ssb_device_disable(dev->dev, 0);
         ssb_bus_may_powerdown(dev->dev->bus);
@@ -4052,29 +3979,24 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
                 b43_wireless_core_reset(dev, tmp);
         }
 
-        if ((phy->type == B43_PHYTYPE_B) || (phy->type == B43_PHYTYPE_G)) {
-                phy->lo_control =
-                    kzalloc(sizeof(*(phy->lo_control)), GFP_KERNEL);
-                if (!phy->lo_control) {
-                        err = -ENOMEM;
-                        goto err_busdown;
-                }
-        }
         setup_struct_wldev_for_init(dev);
-
-        err = b43_phy_init_tssi2dbm_table(dev);
+        err = b43_phy_operations_setup(dev);
         if (err)
-                goto err_kfree_lo_control;
+                goto err_busdown;
 
         /* Enable IRQ routing to this device. */
         ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
 
         b43_imcfglo_timeouts_workaround(dev);
         b43_bluetooth_coext_disable(dev);
-        b43_phy_early_init(dev);
+        if (phy->ops->prepare) {
+                err = phy->ops->prepare(dev);
+                if (err)
+                        goto err_phy_exit;
+        }
         err = b43_chip_init(dev);
         if (err)
-                goto err_kfree_tssitbl;
+                goto err_phy_exit;
         b43_shm_write16(dev, B43_SHM_SHARED,
                         B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
         hf = b43_hf_read(dev);
@@ -4140,15 +4062,11 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
 out:
         return err;
 
-      err_chip_exit:
+err_chip_exit:
         b43_chip_exit(dev);
-      err_kfree_tssitbl:
-        if (phy->dyn_tssi_tbl)
-                kfree(phy->tssi2dbm);
-      err_kfree_lo_control:
-        kfree(phy->lo_control);
-        phy->lo_control = NULL;
-      err_busdown:
+err_phy_exit:
+        b43_phy_exit(dev);
+err_busdown:
         ssb_bus_may_powerdown(bus);
         B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
         return err;
@@ -4291,6 +4209,8 @@ static void b43_op_stop(struct ieee80211_hw *hw)
                 b43_wireless_core_stop(dev);
         b43_wireless_core_exit(dev);
         mutex_unlock(&wl->mutex);
+
+        cancel_work_sync(&(wl->txpower_adjust_work));
 }
 
 static int b43_op_set_retry_limit(struct ieee80211_hw *hw,
@@ -4511,7 +4431,6 @@ static int b43_wireless_core_attach(struct b43_wldev *dev)
                 wl->current_dev = dev;
         INIT_WORK(&dev->restart_work, b43_chip_reset);
 
-        b43_radio_turn_off(dev, 1);
         b43_switch_analog(dev, 0);
         ssb_device_disable(dev->dev, 0);
         ssb_bus_may_powerdown(bus);
@@ -4669,6 +4588,7 @@ static int b43_wireless_init(struct ssb_device *dev)
         INIT_LIST_HEAD(&wl->devlist);
         INIT_WORK(&wl->qos_update_work, b43_qos_update_work);
         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
+        INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
 
         ssb_set_devtypedata(dev, wl);
         b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
diff --git a/drivers/net/wireless/b43/nphy.c b/drivers/net/wireless/b43/nphy.c
index 644eed993bea..4cfeab8560f6 100644
--- a/drivers/net/wireless/b43/nphy.c
+++ b/drivers/net/wireless/b43/nphy.c
@@ -34,10 +34,16 @@ void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
 {//TODO
 }
 
-void b43_nphy_xmitpower(struct b43_wldev *dev)
+static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
 {//TODO
 }
 
+static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{//TODO
+        return B43_TXPWR_RES_DONE;
+}
+
 static void b43_chantab_radio_upload(struct b43_wldev *dev,
                                      const struct b43_nphy_channeltab_entry *e)
 {
@@ -81,9 +87,8 @@ static void b43_nphy_tx_power_fix(struct b43_wldev *dev)
         //TODO
 }
 
-/* Tune the hardware to a new channel. Don't call this directly.
- * Use b43_radio_selectchannel() */
-int b43_nphy_selectchannel(struct b43_wldev *dev, u8 channel)
+/* 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;
 
@@ -162,7 +167,7 @@ static void b43_radio_init2055_post(struct b43_wldev *dev)
         msleep(1);
         b43_radio_mask(dev, B2055_CAL_LPOCTL, 0xFF7F);
         msleep(1);
-        b43_radio_selectchannel(dev, dev->phy.channel, 0);
+        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);
@@ -484,3 +489,136 @@ int b43_phy_initn(struct b43_wldev *dev)
         b43err(dev->wl, "IEEE 802.11n devices are not supported, yet.\n");
         return 0;
 }
+
+static int b43_nphy_op_allocate(struct b43_wldev *dev)
+{
+        struct b43_phy_n *nphy;
+
+        nphy = kzalloc(sizeof(*nphy), GFP_KERNEL);
+        if (!nphy)
+                return -ENOMEM;
+        dev->phy.n = nphy;
+
+        //TODO init struct b43_phy_n
+
+        return 0;
+}
+
+static int b43_nphy_op_init(struct b43_wldev *dev)
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+        int err;
+
+        err = b43_phy_initn(dev);
+        if (err)
+                return err;
+        nphy->initialised = 1;
+
+        return 0;
+}
+
+static void b43_nphy_op_exit(struct b43_wldev *dev)
+{
+        struct b43_phy_n *nphy = dev->phy.n;
+
+        if (nphy->initialised) {
+                //TODO
+                nphy->initialised = 0;
+        }
+        //TODO
+        kfree(nphy);
+        dev->phy.n = NULL;
+}
+
+static inline void check_phyreg(struct b43_wldev *dev, u16 offset)
+{
+#if B43_DEBUG
+        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
+                /* OFDM registers are onnly available on A/G-PHYs */
+                b43err(dev->wl, "Invalid OFDM PHY access at "
+                       "0x%04X on N-PHY\n", offset);
+                dump_stack();
+        }
+        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
+                /* Ext-G registers are only available on G-PHYs */
+                b43err(dev->wl, "Invalid EXT-G PHY access at "
+                       "0x%04X on N-PHY\n", offset);
+                dump_stack();
+        }
+#endif /* B43_DEBUG */
+}
+
+static u16 b43_nphy_op_read(struct b43_wldev *dev, u16 reg)
+{
+        check_phyreg(dev, reg);
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_PHY_DATA);
+}
+
+static void b43_nphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        check_phyreg(dev, reg);
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_PHY_DATA, value);
+}
+
+static u16 b43_nphy_op_radio_read(struct b43_wldev *dev, u16 reg)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+        /* N-PHY needs 0x100 for read access */
+        reg |= 0x100;
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
+}
+
+static void b43_nphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
+}
+
+static void b43_nphy_op_software_rfkill(struct b43_wldev *dev,
+                                        enum rfkill_state state)
+{//TODO
+}
+
+static int b43_nphy_op_switch_channel(struct b43_wldev *dev,
+                                      unsigned int new_channel)
+{
+        if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
+                if ((new_channel < 1) || (new_channel > 14))
+                        return -EINVAL;
+        } else {
+                if (new_channel > 200)
+                        return -EINVAL;
+        }
+
+        return nphy_channel_switch(dev, new_channel);
+}
+
+static unsigned int b43_nphy_op_get_default_chan(struct b43_wldev *dev)
+{
+        if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
+                return 1;
+        return 36;
+}
+
+const struct b43_phy_operations b43_phyops_n = {
+        .allocate               = b43_nphy_op_allocate,
+        .init                   = b43_nphy_op_init,
+        .exit                   = b43_nphy_op_exit,
+        .phy_read               = b43_nphy_op_read,
+        .phy_write              = b43_nphy_op_write,
+        .radio_read             = b43_nphy_op_radio_read,
+        .radio_write            = b43_nphy_op_radio_write,
+        .software_rfkill        = b43_nphy_op_software_rfkill,
+        .switch_channel         = b43_nphy_op_switch_channel,
+        .get_default_chan       = b43_nphy_op_get_default_chan,
+        .recalc_txpower         = b43_nphy_op_recalc_txpower,
+        .adjust_txpower         = b43_nphy_op_adjust_txpower,
+};
diff --git a/drivers/net/wireless/b43/nphy.h b/drivers/net/wireless/b43/nphy.h
index faf46b9cbf1b..3d1f65ed2012 100644
--- a/drivers/net/wireless/b43/nphy.h
+++ b/drivers/net/wireless/b43/nphy.h
@@ -1,7 +1,7 @@
 #ifndef B43_NPHY_H_
 #define B43_NPHY_H_
 
-#include "phy.h"
+#include "phy_common.h"
 
 
 /* N-PHY registers. */
@@ -919,54 +919,14 @@
 
 struct b43_wldev;
 
+struct b43_phy_n {
+        bool initialised;
 
-#ifdef CONFIG_B43_NPHY
-/* N-PHY support enabled */
+        //TODO lots of missing stuff
+};
 
-int b43_phy_initn(struct b43_wldev *dev);
 
-void b43_nphy_radio_turn_on(struct b43_wldev *dev);
-void b43_nphy_radio_turn_off(struct b43_wldev *dev);
+struct b43_phy_operations;
+extern const struct b43_phy_operations b43_phyops_n;
 
-int b43_nphy_selectchannel(struct b43_wldev *dev, u8 channel);
-
-void b43_nphy_xmitpower(struct b43_wldev *dev);
-void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna);
-
-
-#else /* CONFIG_B43_NPHY */
-/* N-PHY support disabled */
-
-
-static inline
-int b43_phy_initn(struct b43_wldev *dev)
-{
-        return -EOPNOTSUPP;
-}
-
-static inline
-void b43_nphy_radio_turn_on(struct b43_wldev *dev)
-{
-}
-static inline
-void b43_nphy_radio_turn_off(struct b43_wldev *dev)
-{
-}
-
-static inline
-int b43_nphy_selectchannel(struct b43_wldev *dev, u8 channel)
-{
-        return -ENOSYS;
-}
-
-static inline
-void b43_nphy_xmitpower(struct b43_wldev *dev)
-{
-}
-static inline
-void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
-{
-}
-
-#endif /* CONFIG_B43_NPHY */
 #endif /* B43_NPHY_H_ */
diff --git a/drivers/net/wireless/b43/phy.c b/drivers/net/wireless/b43/phy.c
index 305d4cd6fd03..02ae450beb00 100644
--- a/drivers/net/wireless/b43/phy.c
+++ b/drivers/net/wireless/b43/phy.c
@@ -39,160 +39,6 @@
 #include "wa.h"
 
 
-static const s8 b43_tssi2dbm_b_table[] = {
-        0x4D, 0x4C, 0x4B, 0x4A,
-        0x4A, 0x49, 0x48, 0x47,
-        0x47, 0x46, 0x45, 0x45,
-        0x44, 0x43, 0x42, 0x42,
-        0x41, 0x40, 0x3F, 0x3E,
-        0x3D, 0x3C, 0x3B, 0x3A,
-        0x39, 0x38, 0x37, 0x36,
-        0x35, 0x34, 0x32, 0x31,
-        0x30, 0x2F, 0x2D, 0x2C,
-        0x2B, 0x29, 0x28, 0x26,
-        0x25, 0x23, 0x21, 0x1F,
-        0x1D, 0x1A, 0x17, 0x14,
-        0x10, 0x0C, 0x06, 0x00,
-        -7, -7, -7, -7,
-        -7, -7, -7, -7,
-        -7, -7, -7, -7,
-};
-
-static const s8 b43_tssi2dbm_g_table[] = {
-        77, 77, 77, 76,
-        76, 76, 75, 75,
-        74, 74, 73, 73,
-        73, 72, 72, 71,
-        71, 70, 70, 69,
-        68, 68, 67, 67,
-        66, 65, 65, 64,
-        63, 63, 62, 61,
-        60, 59, 58, 57,
-        56, 55, 54, 53,
-        52, 50, 49, 47,
-        45, 43, 40, 37,
-        33, 28, 22, 14,
-        5, -7, -20, -20,
-        -20, -20, -20, -20,
-        -20, -20, -20, -20,
-};
-
-const u8 b43_radio_channel_codes_bg[] = {
-        12, 17, 22, 27,
-        32, 37, 42, 47,
-        52, 57, 62, 67,
-        72, 84,
-};
-
-#define bitrev4(tmp) (bitrev8(tmp) >> 4)
-static void b43_phy_initg(struct b43_wldev *dev);
-
-static void generate_rfatt_list(struct b43_wldev *dev,
-                                struct b43_rfatt_list *list)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        /* APHY.rev < 5 || GPHY.rev < 6 */
-        static const struct b43_rfatt rfatt_0[] = {
-                {.att = 3,.with_padmix = 0,},
-                {.att = 1,.with_padmix = 0,},
-                {.att = 5,.with_padmix = 0,},
-                {.att = 7,.with_padmix = 0,},
-                {.att = 9,.with_padmix = 0,},
-                {.att = 2,.with_padmix = 0,},
-                {.att = 0,.with_padmix = 0,},
-                {.att = 4,.with_padmix = 0,},
-                {.att = 6,.with_padmix = 0,},
-                {.att = 8,.with_padmix = 0,},
-                {.att = 1,.with_padmix = 1,},
-                {.att = 2,.with_padmix = 1,},
-                {.att = 3,.with_padmix = 1,},
-                {.att = 4,.with_padmix = 1,},
-        };
-        /* Radio.rev == 8 && Radio.version == 0x2050 */
-        static const struct b43_rfatt rfatt_1[] = {
-                {.att = 2,.with_padmix = 1,},
-                {.att = 4,.with_padmix = 1,},
-                {.att = 6,.with_padmix = 1,},
-                {.att = 8,.with_padmix = 1,},
-                {.att = 10,.with_padmix = 1,},
-                {.att = 12,.with_padmix = 1,},
-                {.att = 14,.with_padmix = 1,},
-        };
-        /* Otherwise */
-        static const struct b43_rfatt rfatt_2[] = {
-                {.att = 0,.with_padmix = 1,},
-                {.att = 2,.with_padmix = 1,},
-                {.att = 4,.with_padmix = 1,},
-                {.att = 6,.with_padmix = 1,},
-                {.att = 8,.with_padmix = 1,},
-                {.att = 9,.with_padmix = 1,},
-                {.att = 9,.with_padmix = 1,},
-        };
-
-        if (!b43_has_hardware_pctl(phy)) {
-                /* Software pctl */
-                list->list = rfatt_0;
-                list->len = ARRAY_SIZE(rfatt_0);
-                list->min_val = 0;
-                list->max_val = 9;
-                return;
-        }
-        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
-                /* Hardware pctl */
-                list->list = rfatt_1;
-                list->len = ARRAY_SIZE(rfatt_1);
-                list->min_val = 0;
-                list->max_val = 14;
-                return;
-        }
-        /* Hardware pctl */
-        list->list = rfatt_2;
-        list->len = ARRAY_SIZE(rfatt_2);
-        list->min_val = 0;
-        list->max_val = 9;
-}
-
-static void generate_bbatt_list(struct b43_wldev *dev,
-                                struct b43_bbatt_list *list)
-{
-        static const struct b43_bbatt bbatt_0[] = {
-                {.att = 0,},
-                {.att = 1,},
-                {.att = 2,},
-                {.att = 3,},
-                {.att = 4,},
-                {.att = 5,},
-                {.att = 6,},
-                {.att = 7,},
-                {.att = 8,},
-        };
-
-        list->list = bbatt_0;
-        list->len = ARRAY_SIZE(bbatt_0);
-        list->min_val = 0;
-        list->max_val = 8;
-}
-
-bool b43_has_hardware_pctl(struct b43_phy *phy)
-{
-        if (!phy->hardware_power_control)
-                return 0;
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-                if (phy->rev >= 5)
-                        return 1;
-                break;
-        case B43_PHYTYPE_G:
-                if (phy->rev >= 6)
-                        return 1;
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-        return 0;
-}
-
 static void b43_shm_clear_tssi(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
@@ -212,1242 +58,6 @@ static void b43_shm_clear_tssi(struct b43_wldev *dev)
         }
 }
 
-/* Lock the PHY registers against concurrent access from the microcode.
- * This lock is nonrecursive. */
-void b43_phy_lock(struct b43_wldev *dev)
-{
-#if B43_DEBUG
-        B43_WARN_ON(dev->phy.phy_locked);
-        dev->phy.phy_locked = 1;
-#endif
-        B43_WARN_ON(dev->dev->id.revision < 3);
-
-        if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
-                b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
-}
-
-void b43_phy_unlock(struct b43_wldev *dev)
-{
-#if B43_DEBUG
-        B43_WARN_ON(!dev->phy.phy_locked);
-        dev->phy.phy_locked = 0;
-#endif
-        B43_WARN_ON(dev->dev->id.revision < 3);
-
-        if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
-                b43_power_saving_ctl_bits(dev, 0);
-}
-
-/* Different PHYs require different register routing flags.
- * This adjusts (and does sanity checks on) the routing flags.
- */
-static inline u16 adjust_phyreg_for_phytype(struct b43_phy *phy,
-                                            u16 offset, struct b43_wldev *dev)
-{
-        if (phy->type == B43_PHYTYPE_A) {
-                /* OFDM registers are base-registers for the A-PHY. */
-                if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
-                        offset &= ~B43_PHYROUTE;
-                        offset |= B43_PHYROUTE_BASE;
-                }
-        }
-
-#if B43_DEBUG
-        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
-                /* Ext-G registers are only available on G-PHYs */
-                if (phy->type != B43_PHYTYPE_G) {
-                        b43err(dev->wl, "Invalid EXT-G PHY access at "
-                               "0x%04X on PHY type %u\n", offset, phy->type);
-                        dump_stack();
-                }
-        }
-        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_N_BMODE) {
-                /* N-BMODE registers are only available on N-PHYs */
-                if (phy->type != B43_PHYTYPE_N) {
-                        b43err(dev->wl, "Invalid N-BMODE PHY access at "
-                               "0x%04X on PHY type %u\n", offset, phy->type);
-                        dump_stack();
-                }
-        }
-#endif /* B43_DEBUG */
-
-        return offset;
-}
-
-u16 b43_phy_read(struct b43_wldev * dev, u16 offset)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        offset = adjust_phyreg_for_phytype(phy, offset, dev);
-        b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
-        return b43_read16(dev, B43_MMIO_PHY_DATA);
-}
-
-void b43_phy_write(struct b43_wldev *dev, u16 offset, u16 val)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        offset = adjust_phyreg_for_phytype(phy, offset, dev);
-        b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
-        b43_write16(dev, B43_MMIO_PHY_DATA, val);
-}
-
-void b43_phy_mask(struct b43_wldev *dev, u16 offset, u16 mask)
-{
-        b43_phy_write(dev, offset,
-                      b43_phy_read(dev, offset) & mask);
-}
-
-void b43_phy_set(struct b43_wldev *dev, u16 offset, u16 set)
-{
-        b43_phy_write(dev, offset,
-                      b43_phy_read(dev, offset) | set);
-}
-
-void b43_phy_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
-{
-        b43_phy_write(dev, offset,
-                      (b43_phy_read(dev, offset) & mask) | set);
-}
-
-/* Adjust the transmission power output (G-PHY) */
-void b43_set_txpower_g(struct b43_wldev *dev,
-                       const struct b43_bbatt *bbatt,
-                       const struct b43_rfatt *rfatt, u8 tx_control)
-{
-        struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
-        u16 bb, rf;
-        u16 tx_bias, tx_magn;
-
-        bb = bbatt->att;
-        rf = rfatt->att;
-        tx_bias = lo->tx_bias;
-        tx_magn = lo->tx_magn;
-        if (unlikely(tx_bias == 0xFF))
-                tx_bias = 0;
-
-        /* Save the values for later */
-        phy->tx_control = tx_control;
-        memcpy(&phy->rfatt, rfatt, sizeof(*rfatt));
-        phy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
-        memcpy(&phy->bbatt, bbatt, sizeof(*bbatt));
-
-        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
-                b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
-                       "rfatt(%u), tx_control(0x%02X), "
-                       "tx_bias(0x%02X), tx_magn(0x%02X)\n",
-                       bb, rf, tx_control, tx_bias, tx_magn);
-        }
-
-        b43_phy_set_baseband_attenuation(dev, bb);
-        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
-        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
-                b43_radio_write16(dev, 0x43,
-                                  (rf & 0x000F) | (tx_control & 0x0070));
-        } else {
-                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
-                                              & 0xFFF0) | (rf & 0x000F));
-                b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
-                                              & ~0x0070) | (tx_control &
-                                                            0x0070));
-        }
-        if (has_tx_magnification(phy)) {
-                b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
-        } else {
-                b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
-                                              & 0xFFF0) | (tx_bias & 0x000F));
-        }
-        if (phy->type == B43_PHYTYPE_G)
-                b43_lo_g_adjust(dev);
-}
-
-static void default_baseband_attenuation(struct b43_wldev *dev,
-                                         struct b43_bbatt *bb)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
-                bb->att = 0;
-        else
-                bb->att = 2;
-}
-
-static void default_radio_attenuation(struct b43_wldev *dev,
-                                      struct b43_rfatt *rf)
-{
-        struct ssb_bus *bus = dev->dev->bus;
-        struct b43_phy *phy = &dev->phy;
-
-        rf->with_padmix = 0;
-
-        if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM &&
-            bus->boardinfo.type == SSB_BOARD_BCM4309G) {
-                if (bus->boardinfo.rev < 0x43) {
-                        rf->att = 2;
-                        return;
-                } else if (bus->boardinfo.rev < 0x51) {
-                        rf->att = 3;
-                        return;
-                }
-        }
-
-        if (phy->type == B43_PHYTYPE_A) {
-                rf->att = 0x60;
-                return;
-        }
-
-        switch (phy->radio_ver) {
-        case 0x2053:
-                switch (phy->radio_rev) {
-                case 1:
-                        rf->att = 6;
-                        return;
-                }
-                break;
-        case 0x2050:
-                switch (phy->radio_rev) {
-                case 0:
-                        rf->att = 5;
-                        return;
-                case 1:
-                        if (phy->type == B43_PHYTYPE_G) {
-                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
-                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
-                                    && bus->boardinfo.rev >= 30)
-                                        rf->att = 3;
-                                else if (bus->boardinfo.vendor ==
-                                         SSB_BOARDVENDOR_BCM
-                                         && bus->boardinfo.type ==
-                                         SSB_BOARD_BU4306)
-                                        rf->att = 3;
-                                else
-                                        rf->att = 1;
-                        } else {
-                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
-                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
-                                    && bus->boardinfo.rev >= 30)
-                                        rf->att = 7;
-                                else
-                                        rf->att = 6;
-                        }
-                        return;
-                case 2:
-                        if (phy->type == B43_PHYTYPE_G) {
-                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
-                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
-                                    && bus->boardinfo.rev >= 30)
-                                        rf->att = 3;
-                                else if (bus->boardinfo.vendor ==
-                                         SSB_BOARDVENDOR_BCM
-                                         && bus->boardinfo.type ==
-                                         SSB_BOARD_BU4306)
-                                        rf->att = 5;
-                                else if (bus->chip_id == 0x4320)
-                                        rf->att = 4;
-                                else
-                                        rf->att = 3;
-                        } else
-                                rf->att = 6;
-                        return;
-                case 3:
-                        rf->att = 5;
-                        return;
-                case 4:
-                case 5:
-                        rf->att = 1;
-                        return;
-                case 6:
-                case 7:
-                        rf->att = 5;
-                        return;
-                case 8:
-                        rf->att = 0xA;
-                        rf->with_padmix = 1;
-                        return;
-                case 9:
-                default:
-                        rf->att = 5;
-                        return;
-                }
-        }
-        rf->att = 5;
-}
-
-static u16 default_tx_control(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        if (phy->radio_ver != 0x2050)
-                return 0;
-        if (phy->radio_rev == 1)
-                return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
-        if (phy->radio_rev < 6)
-                return B43_TXCTL_PA2DB;
-        if (phy->radio_rev == 8)
-                return B43_TXCTL_TXMIX;
-        return 0;
-}
-
-/* This func is called "PHY calibrate" in the specs... */
-void b43_phy_early_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
-
-        default_baseband_attenuation(dev, &phy->bbatt);
-        default_radio_attenuation(dev, &phy->rfatt);
-        phy->tx_control = (default_tx_control(dev) << 4);
-
-        /* Commit previous writes */
-        b43_read32(dev, B43_MMIO_MACCTL);
-
-        if (phy->type == B43_PHYTYPE_B || phy->type == B43_PHYTYPE_G) {
-                generate_rfatt_list(dev, &lo->rfatt_list);
-                generate_bbatt_list(dev, &lo->bbatt_list);
-        }
-        if (phy->type == B43_PHYTYPE_G && phy->rev == 1) {
-                /* Workaround: Temporarly disable gmode through the early init
-                 * phase, as the gmode stuff is not needed for phy rev 1 */
-                phy->gmode = 0;
-                b43_wireless_core_reset(dev, 0);
-                b43_phy_initg(dev);
-                phy->gmode = 1;
-                b43_wireless_core_reset(dev, B43_TMSLOW_GMODE);
-        }
-}
-
-/* GPHY_TSSI_Power_Lookup_Table_Init */
-static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        int i;
-        u16 value;
-
-        for (i = 0; i < 32; i++)
-                b43_ofdmtab_write16(dev, 0x3C20, i, phy->tssi2dbm[i]);
-        for (i = 32; i < 64; i++)
-                b43_ofdmtab_write16(dev, 0x3C00, i - 32, phy->tssi2dbm[i]);
-        for (i = 0; i < 64; i += 2) {
-                value = (u16) phy->tssi2dbm[i];
-                value |= ((u16) phy->tssi2dbm[i + 1]) << 8;
-                b43_phy_write(dev, 0x380 + (i / 2), value);
-        }
-}
-
-/* GPHY_Gain_Lookup_Table_Init */
-static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        struct b43_txpower_lo_control *lo = phy->lo_control;
-        u16 nr_written = 0;
-        u16 tmp;
-        u8 rf, bb;
-
-        for (rf = 0; rf < lo->rfatt_list.len; rf++) {
-                for (bb = 0; bb < lo->bbatt_list.len; bb++) {
-                        if (nr_written >= 0x40)
-                                return;
-                        tmp = lo->bbatt_list.list[bb].att;
-                        tmp <<= 8;
-                        if (phy->radio_rev == 8)
-                                tmp |= 0x50;
-                        else
-                                tmp |= 0x40;
-                        tmp |= lo->rfatt_list.list[rf].att;
-                        b43_phy_write(dev, 0x3C0 + nr_written, tmp);
-                        nr_written++;
-                }
-        }
-}
-
-static void hardware_pctl_init_aphy(struct b43_wldev *dev)
-{
-        //TODO
-}
-
-static void hardware_pctl_init_gphy(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        b43_phy_write(dev, 0x0036, (b43_phy_read(dev, 0x0036) & 0xFFC0)
-                      | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
-        b43_phy_write(dev, 0x0478, (b43_phy_read(dev, 0x0478) & 0xFF00)
-                      | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
-        b43_gphy_tssi_power_lt_init(dev);
-        b43_gphy_gain_lt_init(dev);
-        b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) & 0xFFBF);
-        b43_phy_write(dev, 0x0014, 0x0000);
-
-        B43_WARN_ON(phy->rev < 6);
-        b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
-                      | 0x0800);
-        b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
-                      & 0xFEFF);
-        b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801)
-                      & 0xFFBF);
-
-        b43_gphy_dc_lt_init(dev, 1);
-}
-
-/* HardwarePowerControl init for A and G PHY */
-static void b43_hardware_pctl_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        if (!b43_has_hardware_pctl(phy)) {
-                /* No hardware power control */
-                b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_HWPCTL);
-                return;
-        }
-        /* Init the hwpctl related hardware */
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-                hardware_pctl_init_aphy(dev);
-                break;
-        case B43_PHYTYPE_G:
-                hardware_pctl_init_gphy(dev);
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-        /* Enable hardware pctl in firmware. */
-        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_HWPCTL);
-}
-
-static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        if (!b43_has_hardware_pctl(phy)) {
-                b43_phy_write(dev, 0x047A, 0xC111);
-                return;
-        }
-
-        b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036) & 0xFEFF);
-        b43_phy_write(dev, 0x002F, 0x0202);
-        b43_phy_write(dev, 0x047C, b43_phy_read(dev, 0x047C) | 0x0002);
-        b43_phy_write(dev, 0x047A, b43_phy_read(dev, 0x047A) | 0xF000);
-        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
-                b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
-                                            & 0xFF0F) | 0x0010);
-                b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
-                              | 0x8000);
-                b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
-                                            & 0xFFC0) | 0x0010);
-                b43_phy_write(dev, 0x002E, 0xC07F);
-                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
-                              | 0x0400);
-        } else {
-                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
-                              | 0x0200);
-                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
-                              | 0x0400);
-                b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
-                              & 0x7FFF);
-                b43_phy_write(dev, 0x004F, b43_phy_read(dev, 0x004F)
-                              & 0xFFFE);
-                b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
-                                            & 0xFFC0) | 0x0010);
-                b43_phy_write(dev, 0x002E, 0xC07F);
-                b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
-                                            & 0xFF0F) | 0x0010);
-        }
-}
-
-/* Intialize B/G PHY power control
- * as described in http://bcm-specs.sipsolutions.net/InitPowerControl
- */
-static void b43_phy_init_pctl(struct b43_wldev *dev)
-{
-        struct ssb_bus *bus = dev->dev->bus;
-        struct b43_phy *phy = &dev->phy;
-        struct b43_rfatt old_rfatt;
-        struct b43_bbatt old_bbatt;
-        u8 old_tx_control = 0;
-
-        if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
-            (bus->boardinfo.type == SSB_BOARD_BU4306))
-                return;
-
-        b43_phy_write(dev, 0x0028, 0x8018);
-
-        /* This does something with the Analog... */
-        b43_write16(dev, B43_MMIO_PHY0, b43_read16(dev, B43_MMIO_PHY0)
-                    & 0xFFDF);
-
-        if (phy->type == B43_PHYTYPE_G && !phy->gmode)
-                return;
-        b43_hardware_pctl_early_init(dev);
-        if (phy->cur_idle_tssi == 0) {
-                if (phy->radio_ver == 0x2050 && phy->analog == 0) {
-                        b43_radio_write16(dev, 0x0076,
-                                          (b43_radio_read16(dev, 0x0076)
-                                           & 0x00F7) | 0x0084);
-                } else {
-                        struct b43_rfatt rfatt;
-                        struct b43_bbatt bbatt;
-
-                        memcpy(&old_rfatt, &phy->rfatt, sizeof(old_rfatt));
-                        memcpy(&old_bbatt, &phy->bbatt, sizeof(old_bbatt));
-                        old_tx_control = phy->tx_control;
-
-                        bbatt.att = 11;
-                        if (phy->radio_rev == 8) {
-                                rfatt.att = 15;
-                                rfatt.with_padmix = 1;
-                        } else {
-                                rfatt.att = 9;
-                                rfatt.with_padmix = 0;
-                        }
-                        b43_set_txpower_g(dev, &bbatt, &rfatt, 0);
-                }
-                b43_dummy_transmission(dev);
-                phy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
-                if (B43_DEBUG) {
-                        /* Current-Idle-TSSI sanity check. */
-                        if (abs(phy->cur_idle_tssi - phy->tgt_idle_tssi) >= 20) {
-                                b43dbg(dev->wl,
-                                       "!WARNING! Idle-TSSI phy->cur_idle_tssi "
-                                       "measuring failed. (cur=%d, tgt=%d). Disabling TX power "
-                                       "adjustment.\n", phy->cur_idle_tssi,
-                                       phy->tgt_idle_tssi);
-                                phy->cur_idle_tssi = 0;
-                        }
-                }
-                if (phy->radio_ver == 0x2050 && phy->analog == 0) {
-                        b43_radio_write16(dev, 0x0076,
-                                          b43_radio_read16(dev, 0x0076)
-                                          & 0xFF7B);
-                } else {
-                        b43_set_txpower_g(dev, &old_bbatt,
-                                          &old_rfatt, old_tx_control);
-                }
-        }
-        b43_hardware_pctl_init(dev);
-        b43_shm_clear_tssi(dev);
-}
-
-static void b43_phy_rssiagc(struct b43_wldev *dev, u8 enable)
-{
-        int i;
-
-        if (dev->phy.rev < 3) {
-                if (enable)
-                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_LNAHPFGAIN1, i, 0xFFF8);
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_WRSSI, i, 0xFFF8);
-                        }
-                else
-                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_LNAHPFGAIN1, i, b43_tab_rssiagc1[i]);
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc1[i]);
-                        }
-        } else {
-                if (enable)
-                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++)
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_WRSSI, i, 0x0820);
-                else
-                        for (i = 0; i < B43_TAB_RSSIAGC2_SIZE; i++)
-                                b43_ofdmtab_write16(dev,
-                                        B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc2[i]);
-        }
-}
-
-static void b43_phy_ww(struct b43_wldev *dev)
-{
-        u16 b, curr_s, best_s = 0xFFFF;
-        int i;
-
-        b43_phy_write(dev, B43_PHY_CRS0,
-                b43_phy_read(dev, B43_PHY_CRS0) & ~B43_PHY_CRS0_EN);
-        b43_phy_write(dev, B43_PHY_OFDM(0x1B),
-                b43_phy_read(dev, B43_PHY_OFDM(0x1B)) | 0x1000);
-        b43_phy_write(dev, B43_PHY_OFDM(0x82),
-                (b43_phy_read(dev, B43_PHY_OFDM(0x82)) & 0xF0FF) | 0x0300);
-        b43_radio_write16(dev, 0x0009,
-                b43_radio_read16(dev, 0x0009) | 0x0080);
-        b43_radio_write16(dev, 0x0012,
-                (b43_radio_read16(dev, 0x0012) & 0xFFFC) | 0x0002);
-        b43_wa_initgains(dev);
-        b43_phy_write(dev, B43_PHY_OFDM(0xBA), 0x3ED5);
-        b = b43_phy_read(dev, B43_PHY_PWRDOWN);
-        b43_phy_write(dev, B43_PHY_PWRDOWN, (b & 0xFFF8) | 0x0005);
-        b43_radio_write16(dev, 0x0004,
-                b43_radio_read16(dev, 0x0004) | 0x0004);
-        for (i = 0x10; i <= 0x20; i++) {
-                b43_radio_write16(dev, 0x0013, i);
-                curr_s = b43_phy_read(dev, B43_PHY_OTABLEQ) & 0x00FF;
-                if (!curr_s) {
-                        best_s = 0x0000;
-                        break;
-                } else if (curr_s >= 0x0080)
-                        curr_s = 0x0100 - curr_s;
-                if (curr_s < best_s)
-                        best_s = curr_s;
-        }
-        b43_phy_write(dev, B43_PHY_PWRDOWN, b);
-        b43_radio_write16(dev, 0x0004,
-                b43_radio_read16(dev, 0x0004) & 0xFFFB);
-        b43_radio_write16(dev, 0x0013, best_s);
-        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1_R1, 0, 0xFFEC);
-        b43_phy_write(dev, B43_PHY_OFDM(0xB7), 0x1E80);
-        b43_phy_write(dev, B43_PHY_OFDM(0xB6), 0x1C00);
-        b43_phy_write(dev, B43_PHY_OFDM(0xB5), 0x0EC0);
-        b43_phy_write(dev, B43_PHY_OFDM(0xB2), 0x00C0);
-        b43_phy_write(dev, B43_PHY_OFDM(0xB9), 0x1FFF);
-        b43_phy_write(dev, B43_PHY_OFDM(0xBB),
-                (b43_phy_read(dev, B43_PHY_OFDM(0xBB)) & 0xF000) | 0x0053);
-        b43_phy_write(dev, B43_PHY_OFDM61,
-                (b43_phy_read(dev, B43_PHY_OFDM61) & 0xFE1F) | 0x0120);
-        b43_phy_write(dev, B43_PHY_OFDM(0x13),
-                (b43_phy_read(dev, B43_PHY_OFDM(0x13)) & 0x0FFF) | 0x3000);
-        b43_phy_write(dev, B43_PHY_OFDM(0x14),
-                (b43_phy_read(dev, B43_PHY_OFDM(0x14)) & 0x0FFF) | 0x3000);
-        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 6, 0x0017);
-        for (i = 0; i < 6; i++)
-                b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, i, 0x000F);
-        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0D, 0x000E);
-        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0E, 0x0011);
-        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0F, 0x0013);
-        b43_phy_write(dev, B43_PHY_OFDM(0x33), 0x5030);
-        b43_phy_write(dev, B43_PHY_CRS0,
-                b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
-}
-
-/* Initialize APHY. This is also called for the GPHY in some cases. */
-static void b43_phy_inita(struct b43_wldev *dev)
-{
-        struct ssb_bus *bus = dev->dev->bus;
-        struct b43_phy *phy = &dev->phy;
-
-        might_sleep();
-
-        if (phy->rev >= 6) {
-                if (phy->type == B43_PHYTYPE_A)
-                        b43_phy_write(dev, B43_PHY_OFDM(0x1B),
-                                b43_phy_read(dev, B43_PHY_OFDM(0x1B)) & ~0x1000);
-                if (b43_phy_read(dev, B43_PHY_ENCORE) & B43_PHY_ENCORE_EN)
-                        b43_phy_write(dev, B43_PHY_ENCORE,
-                                b43_phy_read(dev, B43_PHY_ENCORE) | 0x0010);
-                else
-                        b43_phy_write(dev, B43_PHY_ENCORE,
-                                b43_phy_read(dev, B43_PHY_ENCORE) & ~0x1010);
-        }
-
-        b43_wa_all(dev);
-
-        if (phy->type == B43_PHYTYPE_A) {
-                if (phy->gmode && (phy->rev < 3))
-                        b43_phy_write(dev, 0x0034,
-                                b43_phy_read(dev, 0x0034) | 0x0001);
-                b43_phy_rssiagc(dev, 0);
-
-                b43_phy_write(dev, B43_PHY_CRS0,
-                        b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
-
-                b43_radio_init2060(dev);
-
-                if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
-                    ((bus->boardinfo.type == SSB_BOARD_BU4306) ||
-                     (bus->boardinfo.type == SSB_BOARD_BU4309))) {
-                        ; //TODO: A PHY LO
-                }
-
-                if (phy->rev >= 3)
-                        b43_phy_ww(dev);
-
-                hardware_pctl_init_aphy(dev);
-
-                //TODO: radar detection
-        }
-
-        if ((phy->type == B43_PHYTYPE_G) &&
-            (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)) {
-                b43_phy_write(dev, B43_PHY_OFDM(0x6E),
-                                  (b43_phy_read(dev, B43_PHY_OFDM(0x6E))
-                                   & 0xE000) | 0x3CF);
-        }
-}
-
-static void b43_phy_initb5(struct b43_wldev *dev)
-{
-        struct ssb_bus *bus = dev->dev->bus;
-        struct b43_phy *phy = &dev->phy;
-        u16 offset, value;
-        u8 old_channel;
-
-        if (phy->analog == 1) {
-                b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A)
-                                  | 0x0050);
-        }
-        if ((bus->boardinfo.vendor != SSB_BOARDVENDOR_BCM) &&
-            (bus->boardinfo.type != SSB_BOARD_BU4306)) {
-                value = 0x2120;
-                for (offset = 0x00A8; offset < 0x00C7; offset++) {
-                        b43_phy_write(dev, offset, value);
-                        value += 0x202;
-                }
-        }
-        b43_phy_write(dev, 0x0035, (b43_phy_read(dev, 0x0035) & 0xF0FF)
-                      | 0x0700);
-        if (phy->radio_ver == 0x2050)
-                b43_phy_write(dev, 0x0038, 0x0667);
-
-        if (phy->gmode || phy->rev >= 2) {
-                if (phy->radio_ver == 0x2050) {
-                        b43_radio_write16(dev, 0x007A,
-                                          b43_radio_read16(dev, 0x007A)
-                                          | 0x0020);
-                        b43_radio_write16(dev, 0x0051,
-                                          b43_radio_read16(dev, 0x0051)
-                                          | 0x0004);
-                }
-                b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
-
-                b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
-                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
-
-                b43_phy_write(dev, 0x001C, 0x186A);
-
-                b43_phy_write(dev, 0x0013,
-                              (b43_phy_read(dev, 0x0013) & 0x00FF) | 0x1900);
-                b43_phy_write(dev, 0x0035,
-                              (b43_phy_read(dev, 0x0035) & 0xFFC0) | 0x0064);
-                b43_phy_write(dev, 0x005D,
-                              (b43_phy_read(dev, 0x005D) & 0xFF80) | 0x000A);
-        }
-
-        if (dev->bad_frames_preempt) {
-                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
-                              b43_phy_read(dev,
-                                           B43_PHY_RADIO_BITFIELD) | (1 << 11));
-        }
-
-        if (phy->analog == 1) {
-                b43_phy_write(dev, 0x0026, 0xCE00);
-                b43_phy_write(dev, 0x0021, 0x3763);
-                b43_phy_write(dev, 0x0022, 0x1BC3);
-                b43_phy_write(dev, 0x0023, 0x06F9);
-                b43_phy_write(dev, 0x0024, 0x037E);
-        } else
-                b43_phy_write(dev, 0x0026, 0xCC00);
-        b43_phy_write(dev, 0x0030, 0x00C6);
-        b43_write16(dev, 0x03EC, 0x3F22);
-
-        if (phy->analog == 1)
-                b43_phy_write(dev, 0x0020, 0x3E1C);
-        else
-                b43_phy_write(dev, 0x0020, 0x301C);
-
-        if (phy->analog == 0)
-                b43_write16(dev, 0x03E4, 0x3000);
-
-        old_channel = phy->channel;
-        /* Force to channel 7, even if not supported. */
-        b43_radio_selectchannel(dev, 7, 0);
-
-        if (phy->radio_ver != 0x2050) {
-                b43_radio_write16(dev, 0x0075, 0x0080);
-                b43_radio_write16(dev, 0x0079, 0x0081);
-        }
-
-        b43_radio_write16(dev, 0x0050, 0x0020);
-        b43_radio_write16(dev, 0x0050, 0x0023);
-
-        if (phy->radio_ver == 0x2050) {
-                b43_radio_write16(dev, 0x0050, 0x0020);
-                b43_radio_write16(dev, 0x005A, 0x0070);
-        }
-
-        b43_radio_write16(dev, 0x005B, 0x007B);
-        b43_radio_write16(dev, 0x005C, 0x00B0);
-
-        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0007);
-
-        b43_radio_selectchannel(dev, old_channel, 0);
-
-        b43_phy_write(dev, 0x0014, 0x0080);
-        b43_phy_write(dev, 0x0032, 0x00CA);
-        b43_phy_write(dev, 0x002A, 0x88A3);
-
-        b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
-
-        if (phy->radio_ver == 0x2050)
-                b43_radio_write16(dev, 0x005D, 0x000D);
-
-        b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
-}
-
-static void b43_phy_initb6(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 offset, val;
-        u8 old_channel;
-
-        b43_phy_write(dev, 0x003E, 0x817A);
-        b43_radio_write16(dev, 0x007A,
-                          (b43_radio_read16(dev, 0x007A) | 0x0058));
-        if (phy->radio_rev == 4 || phy->radio_rev == 5) {
-                b43_radio_write16(dev, 0x51, 0x37);
-                b43_radio_write16(dev, 0x52, 0x70);
-                b43_radio_write16(dev, 0x53, 0xB3);
-                b43_radio_write16(dev, 0x54, 0x9B);
-                b43_radio_write16(dev, 0x5A, 0x88);
-                b43_radio_write16(dev, 0x5B, 0x88);
-                b43_radio_write16(dev, 0x5D, 0x88);
-                b43_radio_write16(dev, 0x5E, 0x88);
-                b43_radio_write16(dev, 0x7D, 0x88);
-                b43_hf_write(dev, b43_hf_read(dev)
-                             | B43_HF_TSSIRPSMW);
-        }
-        B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);        /* We had code for these revs here... */
-        if (phy->radio_rev == 8) {
-                b43_radio_write16(dev, 0x51, 0);
-                b43_radio_write16(dev, 0x52, 0x40);
-                b43_radio_write16(dev, 0x53, 0xB7);
-                b43_radio_write16(dev, 0x54, 0x98);
-                b43_radio_write16(dev, 0x5A, 0x88);
-                b43_radio_write16(dev, 0x5B, 0x6B);
-                b43_radio_write16(dev, 0x5C, 0x0F);
-                if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_ALTIQ) {
-                        b43_radio_write16(dev, 0x5D, 0xFA);
-                        b43_radio_write16(dev, 0x5E, 0xD8);
-                } else {
-                        b43_radio_write16(dev, 0x5D, 0xF5);
-                        b43_radio_write16(dev, 0x5E, 0xB8);
-                }
-                b43_radio_write16(dev, 0x0073, 0x0003);
-                b43_radio_write16(dev, 0x007D, 0x00A8);
-                b43_radio_write16(dev, 0x007C, 0x0001);
-                b43_radio_write16(dev, 0x007E, 0x0008);
-        }
-        val = 0x1E1F;
-        for (offset = 0x0088; offset < 0x0098; offset++) {
-                b43_phy_write(dev, offset, val);
-                val -= 0x0202;
-        }
-        val = 0x3E3F;
-        for (offset = 0x0098; offset < 0x00A8; offset++) {
-                b43_phy_write(dev, offset, val);
-                val -= 0x0202;
-        }
-        val = 0x2120;
-        for (offset = 0x00A8; offset < 0x00C8; offset++) {
-                b43_phy_write(dev, offset, (val & 0x3F3F));
-                val += 0x0202;
-        }
-        if (phy->type == B43_PHYTYPE_G) {
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x0020);
-                b43_radio_write16(dev, 0x0051,
-                                  b43_radio_read16(dev, 0x0051) | 0x0004);
-                b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
-                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
-                b43_phy_write(dev, 0x5B, 0);
-                b43_phy_write(dev, 0x5C, 0);
-        }
-
-        old_channel = phy->channel;
-        if (old_channel >= 8)
-                b43_radio_selectchannel(dev, 1, 0);
-        else
-                b43_radio_selectchannel(dev, 13, 0);
-
-        b43_radio_write16(dev, 0x0050, 0x0020);
-        b43_radio_write16(dev, 0x0050, 0x0023);
-        udelay(40);
-        if (phy->radio_rev < 6 || phy->radio_rev == 8) {
-                b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
-                                              | 0x0002));
-                b43_radio_write16(dev, 0x50, 0x20);
-        }
-        if (phy->radio_rev <= 2) {
-                b43_radio_write16(dev, 0x7C, 0x20);
-                b43_radio_write16(dev, 0x5A, 0x70);
-                b43_radio_write16(dev, 0x5B, 0x7B);
-                b43_radio_write16(dev, 0x5C, 0xB0);
-        }
-        b43_radio_write16(dev, 0x007A,
-                          (b43_radio_read16(dev, 0x007A) & 0x00F8) | 0x0007);
-
-        b43_radio_selectchannel(dev, old_channel, 0);
-
-        b43_phy_write(dev, 0x0014, 0x0200);
-        if (phy->radio_rev >= 6)
-                b43_phy_write(dev, 0x2A, 0x88C2);
-        else
-                b43_phy_write(dev, 0x2A, 0x8AC0);
-        b43_phy_write(dev, 0x0038, 0x0668);
-        b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
-        if (phy->radio_rev <= 5) {
-                b43_phy_write(dev, 0x5D, (b43_phy_read(dev, 0x5D)
-                                          & 0xFF80) | 0x0003);
-        }
-        if (phy->radio_rev <= 2)
-                b43_radio_write16(dev, 0x005D, 0x000D);
-
-        if (phy->analog == 4) {
-                b43_write16(dev, 0x3E4, 9);
-                b43_phy_write(dev, 0x61, b43_phy_read(dev, 0x61)
-                              & 0x0FFF);
-        } else {
-                b43_phy_write(dev, 0x0002, (b43_phy_read(dev, 0x0002) & 0xFFC0)
-                              | 0x0004);
-        }
-        if (phy->type == B43_PHYTYPE_B)
-                B43_WARN_ON(1);
-        else if (phy->type == B43_PHYTYPE_G)
-                b43_write16(dev, 0x03E6, 0x0);
-}
-
-static void b43_calc_loopback_gain(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 backup_phy[16] = { 0 };
-        u16 backup_radio[3];
-        u16 backup_bband;
-        u16 i, j, loop_i_max;
-        u16 trsw_rx;
-        u16 loop1_outer_done, loop1_inner_done;
-
-        backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
-        backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
-        backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
-        backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
-                backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
-        }
-        backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
-        backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
-        backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
-        backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
-        backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
-        backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
-        backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
-        backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
-        backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
-        backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
-        backup_bband = phy->bbatt.att;
-        backup_radio[0] = b43_radio_read16(dev, 0x52);
-        backup_radio[1] = b43_radio_read16(dev, 0x43);
-        backup_radio[2] = b43_radio_read16(dev, 0x7A);
-
-        b43_phy_write(dev, B43_PHY_CRS0,
-                      b43_phy_read(dev, B43_PHY_CRS0) & 0x3FFF);
-        b43_phy_write(dev, B43_PHY_CCKBBANDCFG,
-                      b43_phy_read(dev, B43_PHY_CCKBBANDCFG) | 0x8000);
-        b43_phy_write(dev, B43_PHY_RFOVER,
-                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0002);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFD);
-        b43_phy_write(dev, B43_PHY_RFOVER,
-                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0001);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFE);
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                b43_phy_write(dev, B43_PHY_ANALOGOVER,
-                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0001);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
-                              b43_phy_read(dev,
-                                           B43_PHY_ANALOGOVERVAL) & 0xFFFE);
-                b43_phy_write(dev, B43_PHY_ANALOGOVER,
-                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0002);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
-                              b43_phy_read(dev,
-                                           B43_PHY_ANALOGOVERVAL) & 0xFFFD);
-        }
-        b43_phy_write(dev, B43_PHY_RFOVER,
-                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x000C);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                      b43_phy_read(dev, B43_PHY_RFOVERVAL) | 0x000C);
-        b43_phy_write(dev, B43_PHY_RFOVER,
-                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0030);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
-                       & 0xFFCF) | 0x10);
-
-        b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0780);
-        b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
-        b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
-
-        b43_phy_write(dev, B43_PHY_CCK(0x0A),
-                      b43_phy_read(dev, B43_PHY_CCK(0x0A)) | 0x2000);
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                b43_phy_write(dev, B43_PHY_ANALOGOVER,
-                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0004);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
-                              b43_phy_read(dev,
-                                           B43_PHY_ANALOGOVERVAL) & 0xFFFB);
-        }
-        b43_phy_write(dev, B43_PHY_CCK(0x03),
-                      (b43_phy_read(dev, B43_PHY_CCK(0x03))
-                       & 0xFF9F) | 0x40);
-
-        if (phy->radio_rev == 8) {
-                b43_radio_write16(dev, 0x43, 0x000F);
-        } else {
-                b43_radio_write16(dev, 0x52, 0);
-                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
-                                              & 0xFFF0) | 0x9);
-        }
-        b43_phy_set_baseband_attenuation(dev, 11);
-
-        if (phy->rev >= 3)
-                b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
-        else
-                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
-        b43_phy_write(dev, B43_PHY_LO_CTL, 0);
-
-        b43_phy_write(dev, B43_PHY_CCK(0x2B),
-                      (b43_phy_read(dev, B43_PHY_CCK(0x2B))
-                       & 0xFFC0) | 0x01);
-        b43_phy_write(dev, B43_PHY_CCK(0x2B),
-                      (b43_phy_read(dev, B43_PHY_CCK(0x2B))
-                       & 0xC0FF) | 0x800);
-
-        b43_phy_write(dev, B43_PHY_RFOVER,
-                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0100);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xCFFF);
-
-        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_EXTLNA) {
-                if (phy->rev >= 7) {
-                        b43_phy_write(dev, B43_PHY_RFOVER,
-                                      b43_phy_read(dev, B43_PHY_RFOVER)
-                                      | 0x0800);
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      b43_phy_read(dev, B43_PHY_RFOVERVAL)
-                                      | 0x8000);
-                }
-        }
-        b43_radio_write16(dev, 0x7A, b43_radio_read16(dev, 0x7A)
-                          & 0x00F7);
-
-        j = 0;
-        loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
-        for (i = 0; i < loop_i_max; i++) {
-                for (j = 0; j < 16; j++) {
-                        b43_radio_write16(dev, 0x43, i);
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
-                                       & 0xF0FF) | (j << 8));
-                        b43_phy_write(dev, B43_PHY_PGACTL,
-                                      (b43_phy_read(dev, B43_PHY_PGACTL)
-                                       & 0x0FFF) | 0xA000);
-                        b43_phy_write(dev, B43_PHY_PGACTL,
-                                      b43_phy_read(dev, B43_PHY_PGACTL)
-                                      | 0xF000);
-                        udelay(20);
-                        if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
-                                goto exit_loop1;
-                }
-        }
-      exit_loop1:
-        loop1_outer_done = i;
-        loop1_inner_done = j;
-        if (j >= 8) {
-                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                              b43_phy_read(dev, B43_PHY_RFOVERVAL)
-                              | 0x30);
-                trsw_rx = 0x1B;
-                for (j = j - 8; j < 16; j++) {
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
-                                       & 0xF0FF) | (j << 8));
-                        b43_phy_write(dev, B43_PHY_PGACTL,
-                                      (b43_phy_read(dev, B43_PHY_PGACTL)
-                                       & 0x0FFF) | 0xA000);
-                        b43_phy_write(dev, B43_PHY_PGACTL,
-                                      b43_phy_read(dev, B43_PHY_PGACTL)
-                                      | 0xF000);
-                        udelay(20);
-                        trsw_rx -= 3;
-                        if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
-                                goto exit_loop2;
-                }
-        } else
-                trsw_rx = 0x18;
-      exit_loop2:
-
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                b43_phy_write(dev, B43_PHY_ANALOGOVER, backup_phy[4]);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, backup_phy[5]);
-        }
-        b43_phy_write(dev, B43_PHY_CCK(0x5A), backup_phy[6]);
-        b43_phy_write(dev, B43_PHY_CCK(0x59), backup_phy[7]);
-        b43_phy_write(dev, B43_PHY_CCK(0x58), backup_phy[8]);
-        b43_phy_write(dev, B43_PHY_CCK(0x0A), backup_phy[9]);
-        b43_phy_write(dev, B43_PHY_CCK(0x03), backup_phy[10]);
-        b43_phy_write(dev, B43_PHY_LO_MASK, backup_phy[11]);
-        b43_phy_write(dev, B43_PHY_LO_CTL, backup_phy[12]);
-        b43_phy_write(dev, B43_PHY_CCK(0x2B), backup_phy[13]);
-        b43_phy_write(dev, B43_PHY_PGACTL, backup_phy[14]);
-
-        b43_phy_set_baseband_attenuation(dev, backup_bband);
-
-        b43_radio_write16(dev, 0x52, backup_radio[0]);
-        b43_radio_write16(dev, 0x43, backup_radio[1]);
-        b43_radio_write16(dev, 0x7A, backup_radio[2]);
-
-        b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2] | 0x0003);
-        udelay(10);
-        b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2]);
-        b43_phy_write(dev, B43_PHY_RFOVERVAL, backup_phy[3]);
-        b43_phy_write(dev, B43_PHY_CRS0, backup_phy[0]);
-        b43_phy_write(dev, B43_PHY_CCKBBANDCFG, backup_phy[1]);
-
-        phy->max_lb_gain =
-            ((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
-        phy->trsw_rx_gain = trsw_rx * 2;
-}
-
-static void b43_phy_initg(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 tmp;
-
-        if (phy->rev == 1)
-                b43_phy_initb5(dev);
-        else
-                b43_phy_initb6(dev);
-
-        if (phy->rev >= 2 || phy->gmode)
-                b43_phy_inita(dev);
-
-        if (phy->rev >= 2) {
-                b43_phy_write(dev, B43_PHY_ANALOGOVER, 0);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, 0);
-        }
-        if (phy->rev == 2) {
-                b43_phy_write(dev, B43_PHY_RFOVER, 0);
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
-        }
-        if (phy->rev > 5) {
-                b43_phy_write(dev, B43_PHY_RFOVER, 0x400);
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
-        }
-        if (phy->gmode || phy->rev >= 2) {
-                tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
-                tmp &= B43_PHYVER_VERSION;
-                if (tmp == 3 || tmp == 5) {
-                        b43_phy_write(dev, B43_PHY_OFDM(0xC2), 0x1816);
-                        b43_phy_write(dev, B43_PHY_OFDM(0xC3), 0x8006);
-                }
-                if (tmp == 5) {
-                        b43_phy_write(dev, B43_PHY_OFDM(0xCC),
-                                      (b43_phy_read(dev, B43_PHY_OFDM(0xCC))
-                                       & 0x00FF) | 0x1F00);
-                }
-        }
-        if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
-                b43_phy_write(dev, B43_PHY_OFDM(0x7E), 0x78);
-        if (phy->radio_rev == 8) {
-                b43_phy_write(dev, B43_PHY_EXTG(0x01),
-                              b43_phy_read(dev, B43_PHY_EXTG(0x01))
-                              | 0x80);
-                b43_phy_write(dev, B43_PHY_OFDM(0x3E),
-                              b43_phy_read(dev, B43_PHY_OFDM(0x3E))
-                              | 0x4);
-        }
-        if (has_loopback_gain(phy))
-                b43_calc_loopback_gain(dev);
-
-        if (phy->radio_rev != 8) {
-                if (phy->initval == 0xFFFF)
-                        phy->initval = b43_radio_init2050(dev);
-                else
-                        b43_radio_write16(dev, 0x0078, phy->initval);
-        }
-        b43_lo_g_init(dev);
-        if (has_tx_magnification(phy)) {
-                b43_radio_write16(dev, 0x52,
-                                  (b43_radio_read16(dev, 0x52) & 0xFF00)
-                                  | phy->lo_control->tx_bias | phy->
-                                  lo_control->tx_magn);
-        } else {
-                b43_radio_write16(dev, 0x52,
-                                  (b43_radio_read16(dev, 0x52) & 0xFFF0)
-                                  | phy->lo_control->tx_bias);
-        }
-        if (phy->rev >= 6) {
-                b43_phy_write(dev, B43_PHY_CCK(0x36),
-                              (b43_phy_read(dev, B43_PHY_CCK(0x36))
-                               & 0x0FFF) | (phy->lo_control->
-                                            tx_bias << 12));
-        }
-        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
-                b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
-        else
-                b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
-        if (phy->rev < 2)
-                b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
-        else
-                b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
-        if (phy->gmode || phy->rev >= 2) {
-                b43_lo_g_adjust(dev);
-                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
-        }
-
-        if (!(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
-                /* The specs state to update the NRSSI LT with
-                 * the value 0x7FFFFFFF here. I think that is some weird
-                 * compiler optimization in the original driver.
-                 * Essentially, what we do here is resetting all NRSSI LT
-                 * entries to -32 (see the clamp_val() in nrssi_hw_update())
-                 */
-                b43_nrssi_hw_update(dev, 0xFFFF);       //FIXME?
-                b43_calc_nrssi_threshold(dev);
-        } else if (phy->gmode || phy->rev >= 2) {
-                if (phy->nrssi[0] == -1000) {
-                        B43_WARN_ON(phy->nrssi[1] != -1000);
-                        b43_calc_nrssi_slope(dev);
-                } else
-                        b43_calc_nrssi_threshold(dev);
-        }
-        if (phy->radio_rev == 8)
-                b43_phy_write(dev, B43_PHY_EXTG(0x05), 0x3230);
-        b43_phy_init_pctl(dev);
-        /* FIXME: The spec says in the following if, the 0 should be replaced
-           'if OFDM may not be used in the current locale'
-           but OFDM is legal everywhere */
-        if ((dev->dev->bus->chip_id == 0x4306
-             && dev->dev->bus->chip_package == 2) || 0) {
-                b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
-                              & 0xBFFF);
-                b43_phy_write(dev, B43_PHY_OFDM(0xC3),
-                              b43_phy_read(dev, B43_PHY_OFDM(0xC3))
-                              & 0x7FFF);
-        }
-}
-
-/* Set the baseband attenuation value on chip. */
-void b43_phy_set_baseband_attenuation(struct b43_wldev *dev,
-                                      u16 baseband_attenuation)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        if (phy->analog == 0) {
-                b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
-                                                 & 0xFFF0) |
-                            baseband_attenuation);
-        } else if (phy->analog > 1) {
-                b43_phy_write(dev, B43_PHY_DACCTL,
-                              (b43_phy_read(dev, B43_PHY_DACCTL)
-                               & 0xFFC3) | (baseband_attenuation << 2));
-        } else {
-                b43_phy_write(dev, B43_PHY_DACCTL,
-                              (b43_phy_read(dev, B43_PHY_DACCTL)
-                               & 0xFF87) | (baseband_attenuation << 3));
-        }
-}
-
 /* http://bcm-specs.sipsolutions.net/EstimatePowerOut
  * This function converts a TSSI value to dBm in Q5.2
  */
@@ -1819,2009 +429,6 @@ int b43_phy_init_tssi2dbm_table(struct b43_wldev *dev)
         return 0;
 }
 
-int b43_phy_init(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        bool unsupported = 0;
-        int err = 0;
-
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-                if (phy->rev == 2 || phy->rev == 3)
-                        b43_phy_inita(dev);
-                else
-                        unsupported = 1;
-                break;
-        case B43_PHYTYPE_G:
-                b43_phy_initg(dev);
-                break;
-        case B43_PHYTYPE_N:
-                err = b43_phy_initn(dev);
-                break;
-        default:
-                unsupported = 1;
-        }
-        if (unsupported)
-                b43err(dev->wl, "Unknown PHYTYPE found\n");
-
-        return err;
-}
-
-void b43_set_rx_antenna(struct b43_wldev *dev, int antenna)
-{
-        struct b43_phy *phy = &dev->phy;
-        u64 hf;
-        u16 tmp;
-        int autodiv = 0;
-
-        if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
-                autodiv = 1;
-
-        hf = b43_hf_read(dev);
-        hf &= ~B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
-
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-        case B43_PHYTYPE_G:
-                tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
-                tmp &= ~B43_PHY_BBANDCFG_RXANT;
-                tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
-                    << B43_PHY_BBANDCFG_RXANT_SHIFT;
-                b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
-
-                if (autodiv) {
-                        tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
-                        if (antenna == B43_ANTENNA_AUTO0)
-                                tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
-                        else
-                                tmp |= B43_PHY_ANTDWELL_AUTODIV1;
-                        b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
-                }
-                if (phy->type == B43_PHYTYPE_G) {
-                        tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
-                        if (autodiv)
-                                tmp |= B43_PHY_ANTWRSETT_ARXDIV;
-                        else
-                                tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
-                        b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
-                        if (phy->rev >= 2) {
-                                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
-                                tmp |= B43_PHY_OFDM61_10;
-                                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
-
-                                tmp =
-                                    b43_phy_read(dev, B43_PHY_DIVSRCHGAINBACK);
-                                tmp = (tmp & 0xFF00) | 0x15;
-                                b43_phy_write(dev, B43_PHY_DIVSRCHGAINBACK,
-                                              tmp);
-
-                                if (phy->rev == 2) {
-                                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                                      8);
-                                } else {
-                                        tmp =
-                                            b43_phy_read(dev,
-                                                         B43_PHY_ADIVRELATED);
-                                        tmp = (tmp & 0xFF00) | 8;
-                                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                                      tmp);
-                                }
-                        }
-                        if (phy->rev >= 6)
-                                b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
-                } else {
-                        if (phy->rev < 3) {
-                                tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
-                                tmp = (tmp & 0xFF00) | 0x24;
-                                b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
-                        } else {
-                                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
-                                tmp |= 0x10;
-                                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
-                                if (phy->analog == 3) {
-                                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
-                                                      0x1D);
-                                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                                      8);
-                                } else {
-                                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
-                                                      0x3A);
-                                        tmp =
-                                            b43_phy_read(dev,
-                                                         B43_PHY_ADIVRELATED);
-                                        tmp = (tmp & 0xFF00) | 8;
-                                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
-                                                      tmp);
-                                }
-                        }
-                }
-                break;
-        case B43_PHYTYPE_B:
-                tmp = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
-                tmp &= ~B43_PHY_BBANDCFG_RXANT;
-                tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
-                    << B43_PHY_BBANDCFG_RXANT_SHIFT;
-                b43_phy_write(dev, B43_PHY_CCKBBANDCFG, tmp);
-                break;
-        case B43_PHYTYPE_N:
-                b43_nphy_set_rxantenna(dev, antenna);
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-
-        hf |= B43_HF_ANTDIVHELP;
-        b43_hf_write(dev, hf);
-}
-
-/* Get the freq, as it has to be written to the device. */
-static inline u16 channel2freq_bg(u8 channel)
-{
-        B43_WARN_ON(!(channel >= 1 && channel <= 14));
-
-        return b43_radio_channel_codes_bg[channel - 1];
-}
-
-/* Get the freq, as it has to be written to the device. */
-static inline u16 channel2freq_a(u8 channel)
-{
-        B43_WARN_ON(channel > 200);
-
-        return (5000 + 5 * channel);
-}
-
-void b43_radio_lock(struct b43_wldev *dev)
-{
-        u32 macctl;
-
-        macctl = b43_read32(dev, B43_MMIO_MACCTL);
-        B43_WARN_ON(macctl & B43_MACCTL_RADIOLOCK);
-        macctl |= B43_MACCTL_RADIOLOCK;
-        b43_write32(dev, B43_MMIO_MACCTL, macctl);
-        /* Commit the write and wait for the device
-         * to exit any radio register access. */
-        b43_read32(dev, B43_MMIO_MACCTL);
-        udelay(10);
-}
-
-void b43_radio_unlock(struct b43_wldev *dev)
-{
-        u32 macctl;
-
-        /* Commit any write */
-        b43_read16(dev, B43_MMIO_PHY_VER);
-        /* unlock */
-        macctl = b43_read32(dev, B43_MMIO_MACCTL);
-        B43_WARN_ON(!(macctl & B43_MACCTL_RADIOLOCK));
-        macctl &= ~B43_MACCTL_RADIOLOCK;
-        b43_write32(dev, B43_MMIO_MACCTL, macctl);
-}
-
-u16 b43_radio_read16(struct b43_wldev *dev, u16 offset)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        /* Offset 1 is a 32-bit register. */
-        B43_WARN_ON(offset == 1);
-
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-                offset |= 0x40;
-                break;
-        case B43_PHYTYPE_B:
-                if (phy->radio_ver == 0x2053) {
-                        if (offset < 0x70)
-                                offset += 0x80;
-                        else if (offset < 0x80)
-                                offset += 0x70;
-                } else if (phy->radio_ver == 0x2050) {
-                        offset |= 0x80;
-                } else
-                        B43_WARN_ON(1);
-                break;
-        case B43_PHYTYPE_G:
-                offset |= 0x80;
-                break;
-        case B43_PHYTYPE_N:
-                offset |= 0x100;
-                break;
-        case B43_PHYTYPE_LP:
-                /* No adjustment required. */
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-
-        b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
-        return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
-}
-
-void b43_radio_write16(struct b43_wldev *dev, u16 offset, u16 val)
-{
-        /* Offset 1 is a 32-bit register. */
-        B43_WARN_ON(offset == 1);
-
-        b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
-        b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, val);
-}
-
-void b43_radio_mask(struct b43_wldev *dev, u16 offset, u16 mask)
-{
-        b43_radio_write16(dev, offset,
-                          b43_radio_read16(dev, offset) & mask);
-}
-
-void b43_radio_set(struct b43_wldev *dev, u16 offset, u16 set)
-{
-        b43_radio_write16(dev, offset,
-                          b43_radio_read16(dev, offset) | set);
-}
-
-void b43_radio_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
-{
-        b43_radio_write16(dev, offset,
-                          (b43_radio_read16(dev, offset) & mask) | set);
-}
-
-static void b43_set_all_gains(struct b43_wldev *dev,
-                              s16 first, s16 second, s16 third)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 i;
-        u16 start = 0x08, end = 0x18;
-        u16 tmp;
-        u16 table;
-
-        if (phy->rev <= 1) {
-                start = 0x10;
-                end = 0x20;
-        }
-
-        table = B43_OFDMTAB_GAINX;
-        if (phy->rev <= 1)
-                table = B43_OFDMTAB_GAINX_R1;
-        for (i = 0; i < 4; i++)
-                b43_ofdmtab_write16(dev, table, i, first);
-
-        for (i = start; i < end; i++)
-                b43_ofdmtab_write16(dev, table, i, second);
-
-        if (third != -1) {
-                tmp = ((u16) third << 14) | ((u16) third << 6);
-                b43_phy_write(dev, 0x04A0,
-                              (b43_phy_read(dev, 0x04A0) & 0xBFBF) | tmp);
-                b43_phy_write(dev, 0x04A1,
-                              (b43_phy_read(dev, 0x04A1) & 0xBFBF) | tmp);
-                b43_phy_write(dev, 0x04A2,
-                              (b43_phy_read(dev, 0x04A2) & 0xBFBF) | tmp);
-        }
-        b43_dummy_transmission(dev);
-}
-
-static void b43_set_original_gains(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 i, tmp;
-        u16 table;
-        u16 start = 0x0008, end = 0x0018;
-
-        if (phy->rev <= 1) {
-                start = 0x0010;
-                end = 0x0020;
-        }
-
-        table = B43_OFDMTAB_GAINX;
-        if (phy->rev <= 1)
-                table = B43_OFDMTAB_GAINX_R1;
-        for (i = 0; i < 4; i++) {
-                tmp = (i & 0xFFFC);
-                tmp |= (i & 0x0001) << 1;
-                tmp |= (i & 0x0002) >> 1;
-
-                b43_ofdmtab_write16(dev, table, i, tmp);
-        }
-
-        for (i = start; i < end; i++)
-                b43_ofdmtab_write16(dev, table, i, i - start);
-
-        b43_phy_write(dev, 0x04A0,
-                      (b43_phy_read(dev, 0x04A0) & 0xBFBF) | 0x4040);
-        b43_phy_write(dev, 0x04A1,
-                      (b43_phy_read(dev, 0x04A1) & 0xBFBF) | 0x4040);
-        b43_phy_write(dev, 0x04A2,
-                      (b43_phy_read(dev, 0x04A2) & 0xBFBF) | 0x4000);
-        b43_dummy_transmission(dev);
-}
-
-/* Synthetic PU workaround */
-static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
-{
-        struct b43_phy *phy = &dev->phy;
-
-        might_sleep();
-
-        if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
-                /* We do not need the workaround. */
-                return;
-        }
-
-        if (channel <= 10) {
-                b43_write16(dev, B43_MMIO_CHANNEL,
-                            channel2freq_bg(channel + 4));
-        } else {
-                b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
-        }
-        msleep(1);
-        b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
-}
-
-u8 b43_radio_aci_detect(struct b43_wldev *dev, u8 channel)
-{
-        struct b43_phy *phy = &dev->phy;
-        u8 ret = 0;
-        u16 saved, rssi, temp;
-        int i, j = 0;
-
-        saved = b43_phy_read(dev, 0x0403);
-        b43_radio_selectchannel(dev, channel, 0);
-        b43_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
-        if (phy->aci_hw_rssi)
-                rssi = b43_phy_read(dev, 0x048A) & 0x3F;
-        else
-                rssi = saved & 0x3F;
-        /* clamp temp to signed 5bit */
-        if (rssi > 32)
-                rssi -= 64;
-        for (i = 0; i < 100; i++) {
-                temp = (b43_phy_read(dev, 0x047F) >> 8) & 0x3F;
-                if (temp > 32)
-                        temp -= 64;
-                if (temp < rssi)
-                        j++;
-                if (j >= 20)
-                        ret = 1;
-        }
-        b43_phy_write(dev, 0x0403, saved);
-
-        return ret;
-}
-
-u8 b43_radio_aci_scan(struct b43_wldev * dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u8 ret[13];
-        unsigned int channel = phy->channel;
-        unsigned int i, j, start, end;
-
-        if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
-                return 0;
-
-        b43_phy_lock(dev);
-        b43_radio_lock(dev);
-        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
-        b43_phy_write(dev, B43_PHY_G_CRS,
-                      b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
-        b43_set_all_gains(dev, 3, 8, 1);
-
-        start = (channel - 5 > 0) ? channel - 5 : 1;
-        end = (channel + 5 < 14) ? channel + 5 : 13;
-
-        for (i = start; i <= end; i++) {
-                if (abs(channel - i) > 2)
-                        ret[i - 1] = b43_radio_aci_detect(dev, i);
-        }
-        b43_radio_selectchannel(dev, channel, 0);
-        b43_phy_write(dev, 0x0802,
-                      (b43_phy_read(dev, 0x0802) & 0xFFFC) | 0x0003);
-        b43_phy_write(dev, 0x0403, b43_phy_read(dev, 0x0403) & 0xFFF8);
-        b43_phy_write(dev, B43_PHY_G_CRS,
-                      b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
-        b43_set_original_gains(dev);
-        for (i = 0; i < 13; i++) {
-                if (!ret[i])
-                        continue;
-                end = (i + 5 < 13) ? i + 5 : 13;
-                for (j = i; j < end; j++)
-                        ret[j] = 1;
-        }
-        b43_radio_unlock(dev);
-        b43_phy_unlock(dev);
-
-        return ret[channel - 1];
-}
-
-/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
-void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
-{
-        b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
-        mmiowb();
-        b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
-}
-
-/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
-s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
-{
-        u16 val;
-
-        b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
-        val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
-
-        return (s16) val;
-}
-
-/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
-void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
-{
-        u16 i;
-        s16 tmp;
-
-        for (i = 0; i < 64; i++) {
-                tmp = b43_nrssi_hw_read(dev, i);
-                tmp -= val;
-                tmp = clamp_val(tmp, -32, 31);
-                b43_nrssi_hw_write(dev, i, tmp);
-        }
-}
-
-/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
-void b43_nrssi_mem_update(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        s16 i, delta;
-        s32 tmp;
-
-        delta = 0x1F - phy->nrssi[0];
-        for (i = 0; i < 64; i++) {
-                tmp = (i - delta) * phy->nrssislope;
-                tmp /= 0x10000;
-                tmp += 0x3A;
-                tmp = clamp_val(tmp, 0, 0x3F);
-                phy->nrssi_lt[i] = tmp;
-        }
-}
-
-static void b43_calc_nrssi_offset(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 backup[20] = { 0 };
-        s16 v47F;
-        u16 i;
-        u16 saved = 0xFFFF;
-
-        backup[0] = b43_phy_read(dev, 0x0001);
-        backup[1] = b43_phy_read(dev, 0x0811);
-        backup[2] = b43_phy_read(dev, 0x0812);
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                backup[3] = b43_phy_read(dev, 0x0814);
-                backup[4] = b43_phy_read(dev, 0x0815);
-        }
-        backup[5] = b43_phy_read(dev, 0x005A);
-        backup[6] = b43_phy_read(dev, 0x0059);
-        backup[7] = b43_phy_read(dev, 0x0058);
-        backup[8] = b43_phy_read(dev, 0x000A);
-        backup[9] = b43_phy_read(dev, 0x0003);
-        backup[10] = b43_radio_read16(dev, 0x007A);
-        backup[11] = b43_radio_read16(dev, 0x0043);
-
-        b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) & 0x7FFF);
-        b43_phy_write(dev, 0x0001,
-                      (b43_phy_read(dev, 0x0001) & 0x3FFF) | 0x4000);
-        b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
-        b43_phy_write(dev, 0x0812,
-                      (b43_phy_read(dev, 0x0812) & 0xFFF3) | 0x0004);
-        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
-        if (phy->rev >= 6) {
-                backup[12] = b43_phy_read(dev, 0x002E);
-                backup[13] = b43_phy_read(dev, 0x002F);
-                backup[14] = b43_phy_read(dev, 0x080F);
-                backup[15] = b43_phy_read(dev, 0x0810);
-                backup[16] = b43_phy_read(dev, 0x0801);
-                backup[17] = b43_phy_read(dev, 0x0060);
-                backup[18] = b43_phy_read(dev, 0x0014);
-                backup[19] = b43_phy_read(dev, 0x0478);
-
-                b43_phy_write(dev, 0x002E, 0);
-                b43_phy_write(dev, 0x002F, 0);
-                b43_phy_write(dev, 0x080F, 0);
-                b43_phy_write(dev, 0x0810, 0);
-                b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478) | 0x0100);
-                b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801) | 0x0040);
-                b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) | 0x0040);
-                b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) | 0x0200);
-        }
-        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0070);
-        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0080);
-        udelay(30);
-
-        v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
-        if (v47F >= 0x20)
-                v47F -= 0x40;
-        if (v47F == 31) {
-                for (i = 7; i >= 4; i--) {
-                        b43_radio_write16(dev, 0x007B, i);
-                        udelay(20);
-                        v47F =
-                            (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
-                        if (v47F >= 0x20)
-                                v47F -= 0x40;
-                        if (v47F < 31 && saved == 0xFFFF)
-                                saved = i;
-                }
-                if (saved == 0xFFFF)
-                        saved = 4;
-        } else {
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) & 0x007F);
-                if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                        b43_phy_write(dev, 0x0814,
-                                      b43_phy_read(dev, 0x0814) | 0x0001);
-                        b43_phy_write(dev, 0x0815,
-                                      b43_phy_read(dev, 0x0815) & 0xFFFE);
-                }
-                b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
-                b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x000C);
-                b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x0030);
-                b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x0030);
-                b43_phy_write(dev, 0x005A, 0x0480);
-                b43_phy_write(dev, 0x0059, 0x0810);
-                b43_phy_write(dev, 0x0058, 0x000D);
-                if (phy->rev == 0) {
-                        b43_phy_write(dev, 0x0003, 0x0122);
-                } else {
-                        b43_phy_write(dev, 0x000A, b43_phy_read(dev, 0x000A)
-                                      | 0x2000);
-                }
-                if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                        b43_phy_write(dev, 0x0814,
-                                      b43_phy_read(dev, 0x0814) | 0x0004);
-                        b43_phy_write(dev, 0x0815,
-                                      b43_phy_read(dev, 0x0815) & 0xFFFB);
-                }
-                b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) & 0xFF9F)
-                              | 0x0040);
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x000F);
-                b43_set_all_gains(dev, 3, 0, 1);
-                b43_radio_write16(dev, 0x0043, (b43_radio_read16(dev, 0x0043)
-                                                & 0x00F0) | 0x000F);
-                udelay(30);
-                v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
-                if (v47F >= 0x20)
-                        v47F -= 0x40;
-                if (v47F == -32) {
-                        for (i = 0; i < 4; i++) {
-                                b43_radio_write16(dev, 0x007B, i);
-                                udelay(20);
-                                v47F =
-                                    (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
-                                           0x003F);
-                                if (v47F >= 0x20)
-                                        v47F -= 0x40;
-                                if (v47F > -31 && saved == 0xFFFF)
-                                        saved = i;
-                        }
-                        if (saved == 0xFFFF)
-                                saved = 3;
-                } else
-                        saved = 0;
-        }
-        b43_radio_write16(dev, 0x007B, saved);
-
-        if (phy->rev >= 6) {
-                b43_phy_write(dev, 0x002E, backup[12]);
-                b43_phy_write(dev, 0x002F, backup[13]);
-                b43_phy_write(dev, 0x080F, backup[14]);
-                b43_phy_write(dev, 0x0810, backup[15]);
-        }
-        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
-                b43_phy_write(dev, 0x0814, backup[3]);
-                b43_phy_write(dev, 0x0815, backup[4]);
-        }
-        b43_phy_write(dev, 0x005A, backup[5]);
-        b43_phy_write(dev, 0x0059, backup[6]);
-        b43_phy_write(dev, 0x0058, backup[7]);
-        b43_phy_write(dev, 0x000A, backup[8]);
-        b43_phy_write(dev, 0x0003, backup[9]);
-        b43_radio_write16(dev, 0x0043, backup[11]);
-        b43_radio_write16(dev, 0x007A, backup[10]);
-        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
-        b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) | 0x8000);
-        b43_set_original_gains(dev);
-        if (phy->rev >= 6) {
-                b43_phy_write(dev, 0x0801, backup[16]);
-                b43_phy_write(dev, 0x0060, backup[17]);
-                b43_phy_write(dev, 0x0014, backup[18]);
-                b43_phy_write(dev, 0x0478, backup[19]);
-        }
-        b43_phy_write(dev, 0x0001, backup[0]);
-        b43_phy_write(dev, 0x0812, backup[2]);
-        b43_phy_write(dev, 0x0811, backup[1]);
-}
-
-void b43_calc_nrssi_slope(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 backup[18] = { 0 };
-        u16 tmp;
-        s16 nrssi0, nrssi1;
-
-        switch (phy->type) {
-        case B43_PHYTYPE_B:
-                backup[0] = b43_radio_read16(dev, 0x007A);
-                backup[1] = b43_radio_read16(dev, 0x0052);
-                backup[2] = b43_radio_read16(dev, 0x0043);
-                backup[3] = b43_phy_read(dev, 0x0030);
-                backup[4] = b43_phy_read(dev, 0x0026);
-                backup[5] = b43_phy_read(dev, 0x0015);
-                backup[6] = b43_phy_read(dev, 0x002A);
-                backup[7] = b43_phy_read(dev, 0x0020);
-                backup[8] = b43_phy_read(dev, 0x005A);
-                backup[9] = b43_phy_read(dev, 0x0059);
-                backup[10] = b43_phy_read(dev, 0x0058);
-                backup[11] = b43_read16(dev, 0x03E2);
-                backup[12] = b43_read16(dev, 0x03E6);
-                backup[13] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
-
-                tmp = b43_radio_read16(dev, 0x007A);
-                tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
-                b43_radio_write16(dev, 0x007A, tmp);
-                b43_phy_write(dev, 0x0030, 0x00FF);
-                b43_write16(dev, 0x03EC, 0x7F7F);
-                b43_phy_write(dev, 0x0026, 0x0000);
-                b43_phy_write(dev, 0x0015, b43_phy_read(dev, 0x0015) | 0x0020);
-                b43_phy_write(dev, 0x002A, 0x08A3);
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x0080);
-
-                nrssi0 = (s16) b43_phy_read(dev, 0x0027);
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) & 0x007F);
-                if (phy->rev >= 2) {
-                        b43_write16(dev, 0x03E6, 0x0040);
-                } else if (phy->rev == 0) {
-                        b43_write16(dev, 0x03E6, 0x0122);
-                } else {
-                        b43_write16(dev, B43_MMIO_CHANNEL_EXT,
-                                    b43_read16(dev,
-                                               B43_MMIO_CHANNEL_EXT) & 0x2000);
-                }
-                b43_phy_write(dev, 0x0020, 0x3F3F);
-                b43_phy_write(dev, 0x0015, 0xF330);
-                b43_radio_write16(dev, 0x005A, 0x0060);
-                b43_radio_write16(dev, 0x0043,
-                                  b43_radio_read16(dev, 0x0043) & 0x00F0);
-                b43_phy_write(dev, 0x005A, 0x0480);
-                b43_phy_write(dev, 0x0059, 0x0810);
-                b43_phy_write(dev, 0x0058, 0x000D);
-                udelay(20);
-
-                nrssi1 = (s16) b43_phy_read(dev, 0x0027);
-                b43_phy_write(dev, 0x0030, backup[3]);
-                b43_radio_write16(dev, 0x007A, backup[0]);
-                b43_write16(dev, 0x03E2, backup[11]);
-                b43_phy_write(dev, 0x0026, backup[4]);
-                b43_phy_write(dev, 0x0015, backup[5]);
-                b43_phy_write(dev, 0x002A, backup[6]);
-                b43_synth_pu_workaround(dev, phy->channel);
-                if (phy->rev != 0)
-                        b43_write16(dev, 0x03F4, backup[13]);
-
-                b43_phy_write(dev, 0x0020, backup[7]);
-                b43_phy_write(dev, 0x005A, backup[8]);
-                b43_phy_write(dev, 0x0059, backup[9]);
-                b43_phy_write(dev, 0x0058, backup[10]);
-                b43_radio_write16(dev, 0x0052, backup[1]);
-                b43_radio_write16(dev, 0x0043, backup[2]);
-
-                if (nrssi0 == nrssi1)
-                        phy->nrssislope = 0x00010000;
-                else
-                        phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
-
-                if (nrssi0 <= -4) {
-                        phy->nrssi[0] = nrssi0;
-                        phy->nrssi[1] = nrssi1;
-                }
-                break;
-        case B43_PHYTYPE_G:
-                if (phy->radio_rev >= 9)
-                        return;
-                if (phy->radio_rev == 8)
-                        b43_calc_nrssi_offset(dev);
-
-                b43_phy_write(dev, B43_PHY_G_CRS,
-                              b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
-                b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
-                backup[7] = b43_read16(dev, 0x03E2);
-                b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
-                backup[0] = b43_radio_read16(dev, 0x007A);
-                backup[1] = b43_radio_read16(dev, 0x0052);
-                backup[2] = b43_radio_read16(dev, 0x0043);
-                backup[3] = b43_phy_read(dev, 0x0015);
-                backup[4] = b43_phy_read(dev, 0x005A);
-                backup[5] = b43_phy_read(dev, 0x0059);
-                backup[6] = b43_phy_read(dev, 0x0058);
-                backup[8] = b43_read16(dev, 0x03E6);
-                backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
-                if (phy->rev >= 3) {
-                        backup[10] = b43_phy_read(dev, 0x002E);
-                        backup[11] = b43_phy_read(dev, 0x002F);
-                        backup[12] = b43_phy_read(dev, 0x080F);
-                        backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
-                        backup[14] = b43_phy_read(dev, 0x0801);
-                        backup[15] = b43_phy_read(dev, 0x0060);
-                        backup[16] = b43_phy_read(dev, 0x0014);
-                        backup[17] = b43_phy_read(dev, 0x0478);
-                        b43_phy_write(dev, 0x002E, 0);
-                        b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
-                        switch (phy->rev) {
-                        case 4:
-                        case 6:
-                        case 7:
-                                b43_phy_write(dev, 0x0478,
-                                              b43_phy_read(dev, 0x0478)
-                                              | 0x0100);
-                                b43_phy_write(dev, 0x0801,
-                                              b43_phy_read(dev, 0x0801)
-                                              | 0x0040);
-                                break;
-                        case 3:
-                        case 5:
-                                b43_phy_write(dev, 0x0801,
-                                              b43_phy_read(dev, 0x0801)
-                                              & 0xFFBF);
-                                break;
-                        }
-                        b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060)
-                                      | 0x0040);
-                        b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014)
-                                      | 0x0200);
-                }
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x0070);
-                b43_set_all_gains(dev, 0, 8, 0);
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) & 0x00F7);
-                if (phy->rev >= 2) {
-                        b43_phy_write(dev, 0x0811,
-                                      (b43_phy_read(dev, 0x0811) & 0xFFCF) |
-                                      0x0030);
-                        b43_phy_write(dev, 0x0812,
-                                      (b43_phy_read(dev, 0x0812) & 0xFFCF) |
-                                      0x0010);
-                }
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x0080);
-                udelay(20);
-
-                nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
-                if (nrssi0 >= 0x0020)
-                        nrssi0 -= 0x0040;
-
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) & 0x007F);
-                if (phy->rev >= 2) {
-                        b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003)
-                                                    & 0xFF9F) | 0x0040);
-                }
-
-                b43_write16(dev, B43_MMIO_CHANNEL_EXT,
-                            b43_read16(dev, B43_MMIO_CHANNEL_EXT)
-                            | 0x2000);
-                b43_radio_write16(dev, 0x007A,
-                                  b43_radio_read16(dev, 0x007A) | 0x000F);
-                b43_phy_write(dev, 0x0015, 0xF330);
-                if (phy->rev >= 2) {
-                        b43_phy_write(dev, 0x0812,
-                                      (b43_phy_read(dev, 0x0812) & 0xFFCF) |
-                                      0x0020);
-                        b43_phy_write(dev, 0x0811,
-                                      (b43_phy_read(dev, 0x0811) & 0xFFCF) |
-                                      0x0020);
-                }
-
-                b43_set_all_gains(dev, 3, 0, 1);
-                if (phy->radio_rev == 8) {
-                        b43_radio_write16(dev, 0x0043, 0x001F);
-                } else {
-                        tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
-                        b43_radio_write16(dev, 0x0052, tmp | 0x0060);
-                        tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
-                        b43_radio_write16(dev, 0x0043, tmp | 0x0009);
-                }
-                b43_phy_write(dev, 0x005A, 0x0480);
-                b43_phy_write(dev, 0x0059, 0x0810);
-                b43_phy_write(dev, 0x0058, 0x000D);
-                udelay(20);
-                nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
-                if (nrssi1 >= 0x0020)
-                        nrssi1 -= 0x0040;
-                if (nrssi0 == nrssi1)
-                        phy->nrssislope = 0x00010000;
-                else
-                        phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
-                if (nrssi0 >= -4) {
-                        phy->nrssi[0] = nrssi1;
-                        phy->nrssi[1] = nrssi0;
-                }
-                if (phy->rev >= 3) {
-                        b43_phy_write(dev, 0x002E, backup[10]);
-                        b43_phy_write(dev, 0x002F, backup[11]);
-                        b43_phy_write(dev, 0x080F, backup[12]);
-                        b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
-                }
-                if (phy->rev >= 2) {
-                        b43_phy_write(dev, 0x0812,
-                                      b43_phy_read(dev, 0x0812) & 0xFFCF);
-                        b43_phy_write(dev, 0x0811,
-                                      b43_phy_read(dev, 0x0811) & 0xFFCF);
-                }
-
-                b43_radio_write16(dev, 0x007A, backup[0]);
-                b43_radio_write16(dev, 0x0052, backup[1]);
-                b43_radio_write16(dev, 0x0043, backup[2]);
-                b43_write16(dev, 0x03E2, backup[7]);
-                b43_write16(dev, 0x03E6, backup[8]);
-                b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
-                b43_phy_write(dev, 0x0015, backup[3]);
-                b43_phy_write(dev, 0x005A, backup[4]);
-                b43_phy_write(dev, 0x0059, backup[5]);
-                b43_phy_write(dev, 0x0058, backup[6]);
-                b43_synth_pu_workaround(dev, phy->channel);
-                b43_phy_write(dev, 0x0802,
-                              b43_phy_read(dev, 0x0802) | (0x0001 | 0x0002));
-                b43_set_original_gains(dev);
-                b43_phy_write(dev, B43_PHY_G_CRS,
-                              b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
-                if (phy->rev >= 3) {
-                        b43_phy_write(dev, 0x0801, backup[14]);
-                        b43_phy_write(dev, 0x0060, backup[15]);
-                        b43_phy_write(dev, 0x0014, backup[16]);
-                        b43_phy_write(dev, 0x0478, backup[17]);
-                }
-                b43_nrssi_mem_update(dev);
-                b43_calc_nrssi_threshold(dev);
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-}
-
-void b43_calc_nrssi_threshold(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        s32 threshold;
-        s32 a, b;
-        s16 tmp16;
-        u16 tmp_u16;
-
-        switch (phy->type) {
-        case B43_PHYTYPE_B:{
-                        if (phy->radio_ver != 0x2050)
-                                return;
-                        if (!
-                            (dev->dev->bus->sprom.
-                             boardflags_lo & B43_BFL_RSSI))
-                                return;
-
-                        if (phy->radio_rev >= 6) {
-                                threshold =
-                                    (phy->nrssi[1] - phy->nrssi[0]) * 32;
-                                threshold += 20 * (phy->nrssi[0] + 1);
-                                threshold /= 40;
-                        } else
-                                threshold = phy->nrssi[1] - 5;
-
-                        threshold = clamp_val(threshold, 0, 0x3E);
-                        b43_phy_read(dev, 0x0020);      /* dummy read */
-                        b43_phy_write(dev, 0x0020,
-                                      (((u16) threshold) << 8) | 0x001C);
-
-                        if (phy->radio_rev >= 6) {
-                                b43_phy_write(dev, 0x0087, 0x0E0D);
-                                b43_phy_write(dev, 0x0086, 0x0C0B);
-                                b43_phy_write(dev, 0x0085, 0x0A09);
-                                b43_phy_write(dev, 0x0084, 0x0808);
-                                b43_phy_write(dev, 0x0083, 0x0808);
-                                b43_phy_write(dev, 0x0082, 0x0604);
-                                b43_phy_write(dev, 0x0081, 0x0302);
-                                b43_phy_write(dev, 0x0080, 0x0100);
-                        }
-                        break;
-                }
-        case B43_PHYTYPE_G:
-                if (!phy->gmode ||
-                    !(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
-                        tmp16 = b43_nrssi_hw_read(dev, 0x20);
-                        if (tmp16 >= 0x20)
-                                tmp16 -= 0x40;
-                        if (tmp16 < 3) {
-                                b43_phy_write(dev, 0x048A,
-                                              (b43_phy_read(dev, 0x048A)
-                                               & 0xF000) | 0x09EB);
-                        } else {
-                                b43_phy_write(dev, 0x048A,
-                                              (b43_phy_read(dev, 0x048A)
-                                               & 0xF000) | 0x0AED);
-                        }
-                } else {
-                        if (phy->interfmode == B43_INTERFMODE_NONWLAN) {
-                                a = 0xE;
-                                b = 0xA;
-                        } else if (!phy->aci_wlan_automatic && phy->aci_enable) {
-                                a = 0x13;
-                                b = 0x12;
-                        } else {
-                                a = 0xE;
-                                b = 0x11;
-                        }
-
-                        a = a * (phy->nrssi[1] - phy->nrssi[0]);
-                        a += (phy->nrssi[0] << 6);
-                        if (a < 32)
-                                a += 31;
-                        else
-                                a += 32;
-                        a = a >> 6;
-                        a = clamp_val(a, -31, 31);
-
-                        b = b * (phy->nrssi[1] - phy->nrssi[0]);
-                        b += (phy->nrssi[0] << 6);
-                        if (b < 32)
-                                b += 31;
-                        else
-                                b += 32;
-                        b = b >> 6;
-                        b = clamp_val(b, -31, 31);
-
-                        tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
-                        tmp_u16 |= ((u32) b & 0x0000003F);
-                        tmp_u16 |= (((u32) a & 0x0000003F) << 6);
-                        b43_phy_write(dev, 0x048A, tmp_u16);
-                }
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-}
-
-/* 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]);
-
-        B43_WARN_ON(offset & 0xF000);
-        B43_WARN_ON(id & 0xF0);
-        *stackptr = offset;
-        *stackptr |= ((u32) id) << 12;
-        *stackptr |= ((u32) value) << 16;
-        (*stackidx)++;
-        B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
-}
-
-static u16 _stack_restore(u32 * stackptr, u8 id, u16 offset)
-{
-        size_t i;
-
-        B43_WARN_ON(offset & 0xF000);
-        B43_WARN_ON(id & 0xF0);
-        for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
-                if ((*stackptr & 0x00000FFF) != offset)
-                        continue;
-                if (((*stackptr & 0x0000F000) >> 12) != id)
-                        continue;
-                return ((*stackptr & 0xFFFF0000) >> 16);
-        }
-        B43_WARN_ON(1);
-
-        return 0;
-}
-
-#define phy_stacksave(offset)                                   \
-        do {                                                    \
-                _stack_save(stack, &stackidx, 0x1, (offset),    \
-                            b43_phy_read(dev, (offset)));       \
-        } while (0)
-#define phy_stackrestore(offset)                                \
-        do {                                                    \
-                b43_phy_write(dev, (offset),            \
-                                  _stack_restore(stack, 0x1,    \
-                                                 (offset)));    \
-        } while (0)
-#define radio_stacksave(offset)                                         \
-        do {                                                            \
-                _stack_save(stack, &stackidx, 0x2, (offset),            \
-                            b43_radio_read16(dev, (offset)));   \
-        } while (0)
-#define radio_stackrestore(offset)                                      \
-        do {                                                            \
-                b43_radio_write16(dev, (offset),                        \
-                                      _stack_restore(stack, 0x2,        \
-                                                     (offset)));        \
-        } while (0)
-#define ofdmtab_stacksave(table, offset)                        \
-        do {                                                    \
-                _stack_save(stack, &stackidx, 0x3, (offset)|(table),    \
-                            b43_ofdmtab_read16(dev, (table), (offset)));        \
-        } while (0)
-#define ofdmtab_stackrestore(table, offset)                     \
-        do {                                                    \
-                b43_ofdmtab_write16(dev, (table),       (offset),       \
-                                  _stack_restore(stack, 0x3,    \
-                                                 (offset)|(table)));    \
-        } while (0)
-
-static void
-b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 tmp, flipped;
-        size_t stackidx = 0;
-        u32 *stack = phy->interfstack;
-
-        switch (mode) {
-        case B43_INTERFMODE_NONWLAN:
-                if (phy->rev != 1) {
-                        b43_phy_write(dev, 0x042B,
-                                      b43_phy_read(dev, 0x042B) | 0x0800);
-                        b43_phy_write(dev, B43_PHY_G_CRS,
-                                      b43_phy_read(dev,
-                                                   B43_PHY_G_CRS) & ~0x4000);
-                        break;
-                }
-                radio_stacksave(0x0078);
-                tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
-                B43_WARN_ON(tmp > 15);
-                flipped = bitrev4(tmp);
-                if (flipped < 10 && flipped >= 8)
-                        flipped = 7;
-                else if (flipped >= 10)
-                        flipped -= 3;
-                flipped = (bitrev4(flipped) << 1) | 0x0020;
-                b43_radio_write16(dev, 0x0078, flipped);
-
-                b43_calc_nrssi_threshold(dev);
-
-                phy_stacksave(0x0406);
-                b43_phy_write(dev, 0x0406, 0x7E28);
-
-                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x0800);
-                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
-                              b43_phy_read(dev,
-                                           B43_PHY_RADIO_BITFIELD) | 0x1000);
-
-                phy_stacksave(0x04A0);
-                b43_phy_write(dev, 0x04A0,
-                              (b43_phy_read(dev, 0x04A0) & 0xC0C0) | 0x0008);
-                phy_stacksave(0x04A1);
-                b43_phy_write(dev, 0x04A1,
-                              (b43_phy_read(dev, 0x04A1) & 0xC0C0) | 0x0605);
-                phy_stacksave(0x04A2);
-                b43_phy_write(dev, 0x04A2,
-                              (b43_phy_read(dev, 0x04A2) & 0xC0C0) | 0x0204);
-                phy_stacksave(0x04A8);
-                b43_phy_write(dev, 0x04A8,
-                              (b43_phy_read(dev, 0x04A8) & 0xC0C0) | 0x0803);
-                phy_stacksave(0x04AB);
-                b43_phy_write(dev, 0x04AB,
-                              (b43_phy_read(dev, 0x04AB) & 0xC0C0) | 0x0605);
-
-                phy_stacksave(0x04A7);
-                b43_phy_write(dev, 0x04A7, 0x0002);
-                phy_stacksave(0x04A3);
-                b43_phy_write(dev, 0x04A3, 0x287A);
-                phy_stacksave(0x04A9);
-                b43_phy_write(dev, 0x04A9, 0x2027);
-                phy_stacksave(0x0493);
-                b43_phy_write(dev, 0x0493, 0x32F5);
-                phy_stacksave(0x04AA);
-                b43_phy_write(dev, 0x04AA, 0x2027);
-                phy_stacksave(0x04AC);
-                b43_phy_write(dev, 0x04AC, 0x32F5);
-                break;
-        case B43_INTERFMODE_MANUALWLAN:
-                if (b43_phy_read(dev, 0x0033) & 0x0800)
-                        break;
-
-                phy->aci_enable = 1;
-
-                phy_stacksave(B43_PHY_RADIO_BITFIELD);
-                phy_stacksave(B43_PHY_G_CRS);
-                if (phy->rev < 2) {
-                        phy_stacksave(0x0406);
-                } else {
-                        phy_stacksave(0x04C0);
-                        phy_stacksave(0x04C1);
-                }
-                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);
-                        ofdmtab_stacksave(0x1A00, 0x2);
-                        ofdmtab_stacksave(0x1A00, 0x3);
-                }
-                phy_stacksave(0x042B);
-                phy_stacksave(0x048C);
-
-                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
-                              b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
-                              & ~0x1000);
-                b43_phy_write(dev, B43_PHY_G_CRS,
-                              (b43_phy_read(dev, B43_PHY_G_CRS)
-                               & 0xFFFC) | 0x0002);
-
-                b43_phy_write(dev, 0x0033, 0x0800);
-                b43_phy_write(dev, 0x04A3, 0x2027);
-                b43_phy_write(dev, 0x04A9, 0x1CA8);
-                b43_phy_write(dev, 0x0493, 0x287A);
-                b43_phy_write(dev, 0x04AA, 0x1CA8);
-                b43_phy_write(dev, 0x04AC, 0x287A);
-
-                b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
-                                            & 0xFFC0) | 0x001A);
-                b43_phy_write(dev, 0x04A7, 0x000D);
-
-                if (phy->rev < 2) {
-                        b43_phy_write(dev, 0x0406, 0xFF0D);
-                } else if (phy->rev == 2) {
-                        b43_phy_write(dev, 0x04C0, 0xFFFF);
-                        b43_phy_write(dev, 0x04C1, 0x00A9);
-                } else {
-                        b43_phy_write(dev, 0x04C0, 0x00C1);
-                        b43_phy_write(dev, 0x04C1, 0x0059);
-                }
-
-                b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
-                                            & 0xC0FF) | 0x1800);
-                b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
-                                            & 0xFFC0) | 0x0015);
-                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
-                                            & 0xCFFF) | 0x1000);
-                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
-                                            & 0xF0FF) | 0x0A00);
-                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
-                                            & 0xCFFF) | 0x1000);
-                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
-                                            & 0xF0FF) | 0x0800);
-                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
-                                            & 0xFFCF) | 0x0010);
-                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
-                                            & 0xFFF0) | 0x0005);
-                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
-                                            & 0xFFCF) | 0x0010);
-                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
-                                            & 0xFFF0) | 0x0006);
-                b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
-                                            & 0xF0FF) | 0x0800);
-                b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
-                                            & 0xF0FF) | 0x0500);
-                b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
-                                            & 0xFFF0) | 0x000B);
-
-                if (phy->rev >= 3) {
-                        b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
-                                      & ~0x8000);
-                        b43_phy_write(dev, 0x0415, (b43_phy_read(dev, 0x0415)
-                                                    & 0x8000) | 0x36D8);
-                        b43_phy_write(dev, 0x0416, (b43_phy_read(dev, 0x0416)
-                                                    & 0x8000) | 0x36D8);
-                        b43_phy_write(dev, 0x0417, (b43_phy_read(dev, 0x0417)
-                                                    & 0xFE00) | 0x016D);
-                } else {
-                        b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
-                                      | 0x1000);
-                        b43_phy_write(dev, 0x048A, (b43_phy_read(dev, 0x048A)
-                                                    & 0x9FFF) | 0x2000);
-                        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
-                }
-                if (phy->rev >= 2) {
-                        b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B)
-                                      | 0x0800);
-                }
-                b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
-                                            & 0xF0FF) | 0x0200);
-                if (phy->rev == 2) {
-                        b43_phy_write(dev, 0x04AE, (b43_phy_read(dev, 0x04AE)
-                                                    & 0xFF00) | 0x007F);
-                        b43_phy_write(dev, 0x04AD, (b43_phy_read(dev, 0x04AD)
-                                                    & 0x00FF) | 0x1300);
-                } else if (phy->rev >= 6) {
-                        b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
-                        b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
-                        b43_phy_write(dev, 0x04AD, b43_phy_read(dev, 0x04AD)
-                                      & 0x00FF);
-                }
-                b43_calc_nrssi_slope(dev);
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-}
-
-static void
-b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
-{
-        struct b43_phy *phy = &dev->phy;
-        u32 *stack = phy->interfstack;
-
-        switch (mode) {
-        case B43_INTERFMODE_NONWLAN:
-                if (phy->rev != 1) {
-                        b43_phy_write(dev, 0x042B,
-                                      b43_phy_read(dev, 0x042B) & ~0x0800);
-                        b43_phy_write(dev, B43_PHY_G_CRS,
-                                      b43_phy_read(dev,
-                                                   B43_PHY_G_CRS) | 0x4000);
-                        break;
-                }
-                radio_stackrestore(0x0078);
-                b43_calc_nrssi_threshold(dev);
-                phy_stackrestore(0x0406);
-                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) & ~0x0800);
-                if (!dev->bad_frames_preempt) {
-                        b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
-                                      b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
-                                      & ~(1 << 11));
-                }
-                b43_phy_write(dev, B43_PHY_G_CRS,
-                              b43_phy_read(dev, B43_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 B43_INTERFMODE_MANUALWLAN:
-                if (!(b43_phy_read(dev, 0x0033) & 0x0800))
-                        break;
-
-                phy->aci_enable = 0;
-
-                phy_stackrestore(B43_PHY_RADIO_BITFIELD);
-                phy_stackrestore(B43_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);
-                        ofdmtab_stackrestore(0x1A00, 0x2);
-                        ofdmtab_stackrestore(0x1A00, 0x3);
-                }
-                phy_stackrestore(0x04A2);
-                phy_stackrestore(0x048A);
-                phy_stackrestore(0x042B);
-                phy_stackrestore(0x048C);
-                b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
-                b43_calc_nrssi_slope(dev);
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-}
-
-#undef phy_stacksave
-#undef phy_stackrestore
-#undef radio_stacksave
-#undef radio_stackrestore
-#undef ofdmtab_stacksave
-#undef ofdmtab_stackrestore
-
-int b43_radio_set_interference_mitigation(struct b43_wldev *dev, int mode)
-{
-        struct b43_phy *phy = &dev->phy;
-        int currentmode;
-
-        if ((phy->type != B43_PHYTYPE_G) || (phy->rev == 0) || (!phy->gmode))
-                return -ENODEV;
-
-        phy->aci_wlan_automatic = 0;
-        switch (mode) {
-        case B43_INTERFMODE_AUTOWLAN:
-                phy->aci_wlan_automatic = 1;
-                if (phy->aci_enable)
-                        mode = B43_INTERFMODE_MANUALWLAN;
-                else
-                        mode = B43_INTERFMODE_NONE;
-                break;
-        case B43_INTERFMODE_NONE:
-        case B43_INTERFMODE_NONWLAN:
-        case B43_INTERFMODE_MANUALWLAN:
-                break;
-        default:
-                return -EINVAL;
-        }
-
-        currentmode = phy->interfmode;
-        if (currentmode == mode)
-                return 0;
-        if (currentmode != B43_INTERFMODE_NONE)
-                b43_radio_interference_mitigation_disable(dev, currentmode);
-
-        if (mode == B43_INTERFMODE_NONE) {
-                phy->aci_enable = 0;
-                phy->aci_hw_rssi = 0;
-        } else
-                b43_radio_interference_mitigation_enable(dev, mode);
-        phy->interfmode = mode;
-
-        return 0;
-}
-
-static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
-{
-        u16 reg, index, ret;
-
-        static const u8 rcc_table[] = {
-                0x02, 0x03, 0x01, 0x0F,
-                0x06, 0x07, 0x05, 0x0F,
-                0x0A, 0x0B, 0x09, 0x0F,
-                0x0E, 0x0F, 0x0D, 0x0F,
-        };
-
-        reg = b43_radio_read16(dev, 0x60);
-        index = (reg & 0x001E) >> 1;
-        ret = rcc_table[index] << 1;
-        ret |= (reg & 0x0001);
-        ret |= 0x0020;
-
-        return ret;
-}
-
-#define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
-static u16 radio2050_rfover_val(struct b43_wldev *dev,
-                                u16 phy_register, unsigned int lpd)
-{
-        struct b43_phy *phy = &dev->phy;
-        struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
-
-        if (!phy->gmode)
-                return 0;
-
-        if (has_loopback_gain(phy)) {
-                int max_lb_gain = phy->max_lb_gain;
-                u16 extlna;
-                u16 i;
-
-                if (phy->radio_rev == 8)
-                        max_lb_gain += 0x3E;
-                else
-                        max_lb_gain += 0x26;
-                if (max_lb_gain >= 0x46) {
-                        extlna = 0x3000;
-                        max_lb_gain -= 0x46;
-                } else if (max_lb_gain >= 0x3A) {
-                        extlna = 0x1000;
-                        max_lb_gain -= 0x3A;
-                } else if (max_lb_gain >= 0x2E) {
-                        extlna = 0x2000;
-                        max_lb_gain -= 0x2E;
-                } else {
-                        extlna = 0;
-                        max_lb_gain -= 0x10;
-                }
-
-                for (i = 0; i < 16; i++) {
-                        max_lb_gain -= (i * 6);
-                        if (max_lb_gain < 6)
-                                break;
-                }
-
-                if ((phy->rev < 7) ||
-                    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
-                        if (phy_register == B43_PHY_RFOVER) {
-                                return 0x1B3;
-                        } else if (phy_register == B43_PHY_RFOVERVAL) {
-                                extlna |= (i << 8);
-                                switch (lpd) {
-                                case LPD(0, 1, 1):
-                                        return 0x0F92;
-                                case LPD(0, 0, 1):
-                                case LPD(1, 0, 1):
-                                        return (0x0092 | extlna);
-                                case LPD(1, 0, 0):
-                                        return (0x0093 | extlna);
-                                }
-                                B43_WARN_ON(1);
-                        }
-                        B43_WARN_ON(1);
-                } else {
-                        if (phy_register == B43_PHY_RFOVER) {
-                                return 0x9B3;
-                        } else if (phy_register == B43_PHY_RFOVERVAL) {
-                                if (extlna)
-                                        extlna |= 0x8000;
-                                extlna |= (i << 8);
-                                switch (lpd) {
-                                case LPD(0, 1, 1):
-                                        return 0x8F92;
-                                case LPD(0, 0, 1):
-                                        return (0x8092 | extlna);
-                                case LPD(1, 0, 1):
-                                        return (0x2092 | extlna);
-                                case LPD(1, 0, 0):
-                                        return (0x2093 | extlna);
-                                }
-                                B43_WARN_ON(1);
-                        }
-                        B43_WARN_ON(1);
-                }
-        } else {
-                if ((phy->rev < 7) ||
-                    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
-                        if (phy_register == B43_PHY_RFOVER) {
-                                return 0x1B3;
-                        } else if (phy_register == B43_PHY_RFOVERVAL) {
-                                switch (lpd) {
-                                case LPD(0, 1, 1):
-                                        return 0x0FB2;
-                                case LPD(0, 0, 1):
-                                        return 0x00B2;
-                                case LPD(1, 0, 1):
-                                        return 0x30B2;
-                                case LPD(1, 0, 0):
-                                        return 0x30B3;
-                                }
-                                B43_WARN_ON(1);
-                        }
-                        B43_WARN_ON(1);
-                } else {
-                        if (phy_register == B43_PHY_RFOVER) {
-                                return 0x9B3;
-                        } else if (phy_register == B43_PHY_RFOVERVAL) {
-                                switch (lpd) {
-                                case LPD(0, 1, 1):
-                                        return 0x8FB2;
-                                case LPD(0, 0, 1):
-                                        return 0x80B2;
-                                case LPD(1, 0, 1):
-                                        return 0x20B2;
-                                case LPD(1, 0, 0):
-                                        return 0x20B3;
-                                }
-                                B43_WARN_ON(1);
-                        }
-                        B43_WARN_ON(1);
-                }
-        }
-        return 0;
-}
-
-struct init2050_saved_values {
-        /* Core registers */
-        u16 reg_3EC;
-        u16 reg_3E6;
-        u16 reg_3F4;
-        /* Radio registers */
-        u16 radio_43;
-        u16 radio_51;
-        u16 radio_52;
-        /* PHY registers */
-        u16 phy_pgactl;
-        u16 phy_cck_5A;
-        u16 phy_cck_59;
-        u16 phy_cck_58;
-        u16 phy_cck_30;
-        u16 phy_rfover;
-        u16 phy_rfoverval;
-        u16 phy_analogover;
-        u16 phy_analogoverval;
-        u16 phy_crs0;
-        u16 phy_classctl;
-        u16 phy_lo_mask;
-        u16 phy_lo_ctl;
-        u16 phy_syncctl;
-};
-
-u16 b43_radio_init2050(struct b43_wldev *dev)
-{
-        struct b43_phy *phy = &dev->phy;
-        struct init2050_saved_values sav;
-        u16 rcc;
-        u16 radio78;
-        u16 ret;
-        u16 i, j;
-        u32 tmp1 = 0, tmp2 = 0;
-
-        memset(&sav, 0, sizeof(sav));   /* get rid of "may be used uninitialized..." */
-
-        sav.radio_43 = b43_radio_read16(dev, 0x43);
-        sav.radio_51 = b43_radio_read16(dev, 0x51);
-        sav.radio_52 = b43_radio_read16(dev, 0x52);
-        sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
-        sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
-        sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
-        sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
-
-        if (phy->type == B43_PHYTYPE_B) {
-                sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
-                sav.reg_3EC = b43_read16(dev, 0x3EC);
-
-                b43_phy_write(dev, B43_PHY_CCK(0x30), 0xFF);
-                b43_write16(dev, 0x3EC, 0x3F3F);
-        } else if (phy->gmode || phy->rev >= 2) {
-                sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
-                sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
-                sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
-                sav.phy_analogoverval =
-                    b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
-                sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
-                sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
-
-                b43_phy_write(dev, B43_PHY_ANALOGOVER,
-                              b43_phy_read(dev, B43_PHY_ANALOGOVER)
-                              | 0x0003);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
-                              b43_phy_read(dev, B43_PHY_ANALOGOVERVAL)
-                              & 0xFFFC);
-                b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
-                              & 0x7FFF);
-                b43_phy_write(dev, B43_PHY_CLASSCTL,
-                              b43_phy_read(dev, B43_PHY_CLASSCTL)
-                              & 0xFFFC);
-                if (has_loopback_gain(phy)) {
-                        sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
-                        sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
-
-                        if (phy->rev >= 3)
-                                b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
-                        else
-                                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
-                        b43_phy_write(dev, B43_PHY_LO_CTL, 0);
-                }
-
-                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
-                                                   LPD(0, 1, 1)));
-                b43_phy_write(dev, B43_PHY_RFOVER,
-                              radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
-        }
-        b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
-
-        sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
-        b43_phy_write(dev, B43_PHY_SYNCCTL, b43_phy_read(dev, B43_PHY_SYNCCTL)
-                      & 0xFF7F);
-        sav.reg_3E6 = b43_read16(dev, 0x3E6);
-        sav.reg_3F4 = b43_read16(dev, 0x3F4);
-
-        if (phy->analog == 0) {
-                b43_write16(dev, 0x03E6, 0x0122);
-        } else {
-                if (phy->analog >= 2) {
-                        b43_phy_write(dev, B43_PHY_CCK(0x03),
-                                      (b43_phy_read(dev, B43_PHY_CCK(0x03))
-                                       & 0xFFBF) | 0x40);
-                }
-                b43_write16(dev, B43_MMIO_CHANNEL_EXT,
-                            (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
-        }
-
-        rcc = b43_radio_core_calibration_value(dev);
-
-        if (phy->type == B43_PHYTYPE_B)
-                b43_radio_write16(dev, 0x78, 0x26);
-        if (phy->gmode || phy->rev >= 2) {
-                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
-                                                   LPD(0, 1, 1)));
-        }
-        b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
-        b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x1403);
-        if (phy->gmode || phy->rev >= 2) {
-                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
-                                                   LPD(0, 0, 1)));
-        }
-        b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
-        b43_radio_write16(dev, 0x51, b43_radio_read16(dev, 0x51)
-                          | 0x0004);
-        if (phy->radio_rev == 8) {
-                b43_radio_write16(dev, 0x43, 0x1F);
-        } else {
-                b43_radio_write16(dev, 0x52, 0);
-                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
-                                              & 0xFFF0) | 0x0009);
-        }
-        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
-
-        for (i = 0; i < 16; i++) {
-                b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0480);
-                b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
-                b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
-                if (phy->gmode || phy->rev >= 2) {
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      radio2050_rfover_val(dev,
-                                                           B43_PHY_RFOVERVAL,
-                                                           LPD(1, 0, 1)));
-                }
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
-                udelay(10);
-                if (phy->gmode || phy->rev >= 2) {
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      radio2050_rfover_val(dev,
-                                                           B43_PHY_RFOVERVAL,
-                                                           LPD(1, 0, 1)));
-                }
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
-                udelay(10);
-                if (phy->gmode || phy->rev >= 2) {
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      radio2050_rfover_val(dev,
-                                                           B43_PHY_RFOVERVAL,
-                                                           LPD(1, 0, 0)));
-                }
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
-                udelay(20);
-                tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
-                b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
-                if (phy->gmode || phy->rev >= 2) {
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      radio2050_rfover_val(dev,
-                                                           B43_PHY_RFOVERVAL,
-                                                           LPD(1, 0, 1)));
-                }
-                b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
-        }
-        udelay(10);
-
-        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
-        tmp1++;
-        tmp1 >>= 9;
-
-        for (i = 0; i < 16; i++) {
-                radio78 = (bitrev4(i) << 1) | 0x0020;
-                b43_radio_write16(dev, 0x78, radio78);
-                udelay(10);
-                for (j = 0; j < 16; j++) {
-                        b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0D80);
-                        b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
-                        b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
-                        if (phy->gmode || phy->rev >= 2) {
-                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                              radio2050_rfover_val(dev,
-                                                                   B43_PHY_RFOVERVAL,
-                                                                   LPD(1, 0,
-                                                                       1)));
-                        }
-                        b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
-                        udelay(10);
-                        if (phy->gmode || phy->rev >= 2) {
-                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                              radio2050_rfover_val(dev,
-                                                                   B43_PHY_RFOVERVAL,
-                                                                   LPD(1, 0,
-                                                                       1)));
-                        }
-                        b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
-                        udelay(10);
-                        if (phy->gmode || phy->rev >= 2) {
-                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                              radio2050_rfover_val(dev,
-                                                                   B43_PHY_RFOVERVAL,
-                                                                   LPD(1, 0,
-                                                                       0)));
-                        }
-                        b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
-                        udelay(10);
-                        tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
-                        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
-                        if (phy->gmode || phy->rev >= 2) {
-                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                              radio2050_rfover_val(dev,
-                                                                   B43_PHY_RFOVERVAL,
-                                                                   LPD(1, 0,
-                                                                       1)));
-                        }
-                        b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
-                }
-                tmp2++;
-                tmp2 >>= 8;
-                if (tmp1 < tmp2)
-                        break;
-        }
-
-        /* Restore the registers */
-        b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
-        b43_radio_write16(dev, 0x51, sav.radio_51);
-        b43_radio_write16(dev, 0x52, sav.radio_52);
-        b43_radio_write16(dev, 0x43, sav.radio_43);
-        b43_phy_write(dev, B43_PHY_CCK(0x5A), sav.phy_cck_5A);
-        b43_phy_write(dev, B43_PHY_CCK(0x59), sav.phy_cck_59);
-        b43_phy_write(dev, B43_PHY_CCK(0x58), sav.phy_cck_58);
-        b43_write16(dev, 0x3E6, sav.reg_3E6);
-        if (phy->analog != 0)
-                b43_write16(dev, 0x3F4, sav.reg_3F4);
-        b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
-        b43_synth_pu_workaround(dev, phy->channel);
-        if (phy->type == B43_PHYTYPE_B) {
-                b43_phy_write(dev, B43_PHY_CCK(0x30), sav.phy_cck_30);
-                b43_write16(dev, 0x3EC, sav.reg_3EC);
-        } else if (phy->gmode) {
-                b43_write16(dev, B43_MMIO_PHY_RADIO,
-                            b43_read16(dev, B43_MMIO_PHY_RADIO)
-                            & 0x7FFF);
-                b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
-                b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
-                b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
-                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
-                              sav.phy_analogoverval);
-                b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
-                b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
-                if (has_loopback_gain(phy)) {
-                        b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
-                        b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
-                }
-        }
-        if (i > 15)
-                ret = radio78;
-        else
-                ret = rcc;
-
-        return ret;
-}
-
-void b43_radio_init2060(struct b43_wldev *dev)
-{
-        int err;
-
-        b43_radio_write16(dev, 0x0004, 0x00C0);
-        b43_radio_write16(dev, 0x0005, 0x0008);
-        b43_radio_write16(dev, 0x0009, 0x0040);
-        b43_radio_write16(dev, 0x0005, 0x00AA);
-        b43_radio_write16(dev, 0x0032, 0x008F);
-        b43_radio_write16(dev, 0x0006, 0x008F);
-        b43_radio_write16(dev, 0x0034, 0x008F);
-        b43_radio_write16(dev, 0x002C, 0x0007);
-        b43_radio_write16(dev, 0x0082, 0x0080);
-        b43_radio_write16(dev, 0x0080, 0x0000);
-        b43_radio_write16(dev, 0x003F, 0x00DA);
-        b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
-        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0010);
-        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
-        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
-        msleep(1);              /* delay 400usec */
-
-        b43_radio_write16(dev, 0x0081,
-                          (b43_radio_read16(dev, 0x0081) & ~0x0020) | 0x0010);
-        msleep(1);              /* delay 400usec */
-
-        b43_radio_write16(dev, 0x0005,
-                          (b43_radio_read16(dev, 0x0005) & ~0x0008) | 0x0008);
-        b43_radio_write16(dev, 0x0085, b43_radio_read16(dev, 0x0085) & ~0x0010);
-        b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
-        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0040);
-        b43_radio_write16(dev, 0x0081,
-                          (b43_radio_read16(dev, 0x0081) & ~0x0040) | 0x0040);
-        b43_radio_write16(dev, 0x0005,
-                          (b43_radio_read16(dev, 0x0081) & ~0x0008) | 0x0008);
-        b43_phy_write(dev, 0x0063, 0xDDC6);
-        b43_phy_write(dev, 0x0069, 0x07BE);
-        b43_phy_write(dev, 0x006A, 0x0000);
-
-        err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_A, 0);
-        B43_WARN_ON(err);
-
-        msleep(1);
-}
-
-static inline u16 freq_r3A_value(u16 frequency)
-{
-        u16 value;
-
-        if (frequency < 5091)
-                value = 0x0040;
-        else if (frequency < 5321)
-                value = 0x0000;
-        else if (frequency < 5806)
-                value = 0x0080;
-        else
-                value = 0x0040;
-
-        return value;
-}
-
-void b43_radio_set_tx_iq(struct b43_wldev *dev)
-{
-        static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
-        static const u8 data_low[5] = { 0x00, 0x01, 0x05, 0x06, 0x0A };
-        u16 tmp = b43_radio_read16(dev, 0x001E);
-        int i, j;
-
-        for (i = 0; i < 5; i++) {
-                for (j = 0; j < 5; j++) {
-                        if (tmp == (data_high[i] << 4 | data_low[j])) {
-                                b43_phy_write(dev, 0x0069,
-                                              (i - j) << 8 | 0x00C0);
-                                return;
-                        }
-                }
-        }
-}
-
-int b43_radio_selectchannel(struct b43_wldev *dev,
-                            u8 channel, int synthetic_pu_workaround)
-{
-        struct b43_phy *phy = &dev->phy;
-        u16 r8, tmp;
-        u16 freq;
-        u16 channelcookie, savedcookie;
-        int err = 0;
-
-        if (channel == 0xFF) {
-                switch (phy->type) {
-                case B43_PHYTYPE_A:
-                        channel = B43_DEFAULT_CHANNEL_A;
-                        break;
-                case B43_PHYTYPE_B:
-                case B43_PHYTYPE_G:
-                        channel = B43_DEFAULT_CHANNEL_BG;
-                        break;
-                case B43_PHYTYPE_N:
-                        //FIXME check if we are on 2.4GHz or 5GHz and set a default channel.
-                        channel = 1;
-                        break;
-                default:
-                        B43_WARN_ON(1);
-                }
-        }
-
-        /* First we set the channel radio code to prevent the
-         * firmware from sending ghost packets.
-         */
-        channelcookie = channel;
-        if (0 /*FIXME on 5Ghz */)
-                channelcookie |= 0x100;
-        //FIXME set 40Mhz flag if required
-        savedcookie = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN);
-        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN, channelcookie);
-
-        switch (phy->type) {
-        case B43_PHYTYPE_A:
-                if (channel > 200) {
-                        err = -EINVAL;
-                        goto out;
-                }
-                freq = channel2freq_a(channel);
-
-                r8 = b43_radio_read16(dev, 0x0008);
-                b43_write16(dev, 0x03F0, freq);
-                b43_radio_write16(dev, 0x0008, r8);
-
-                //TODO: write max channel TX power? to Radio 0x2D
-                tmp = b43_radio_read16(dev, 0x002E);
-                tmp &= 0x0080;
-                //TODO: OR tmp with the Power out estimation for this channel?
-                b43_radio_write16(dev, 0x002E, tmp);
-
-                if (freq >= 4920 && freq <= 5500) {
-                        /*
-                         * r8 = (((freq * 15 * 0xE1FC780F) >> 32) / 29) & 0x0F;
-                         *    = (freq * 0.025862069
-                         */
-                        r8 = 3 * freq / 116;    /* is equal to r8 = freq * 0.025862 */
-                }
-                b43_radio_write16(dev, 0x0007, (r8 << 4) | r8);
-                b43_radio_write16(dev, 0x0020, (r8 << 4) | r8);
-                b43_radio_write16(dev, 0x0021, (r8 << 4) | r8);
-                b43_radio_write16(dev, 0x0022, (b43_radio_read16(dev, 0x0022)
-                                                & 0x000F) | (r8 << 4));
-                b43_radio_write16(dev, 0x002A, (r8 << 4));
-                b43_radio_write16(dev, 0x002B, (r8 << 4));
-                b43_radio_write16(dev, 0x0008, (b43_radio_read16(dev, 0x0008)
-                                                & 0x00F0) | (r8 << 4));
-                b43_radio_write16(dev, 0x0029, (b43_radio_read16(dev, 0x0029)
-                                                & 0xFF0F) | 0x00B0);
-                b43_radio_write16(dev, 0x0035, 0x00AA);
-                b43_radio_write16(dev, 0x0036, 0x0085);
-                b43_radio_write16(dev, 0x003A, (b43_radio_read16(dev, 0x003A)
-                                                & 0xFF20) |
-                                  freq_r3A_value(freq));
-                b43_radio_write16(dev, 0x003D,
-                                  b43_radio_read16(dev, 0x003D) & 0x00FF);
-                b43_radio_write16(dev, 0x0081, (b43_radio_read16(dev, 0x0081)
-                                                & 0xFF7F) | 0x0080);
-                b43_radio_write16(dev, 0x0035,
-                                  b43_radio_read16(dev, 0x0035) & 0xFFEF);
-                b43_radio_write16(dev, 0x0035, (b43_radio_read16(dev, 0x0035)
-                                                & 0xFFEF) | 0x0010);
-                b43_radio_set_tx_iq(dev);
-                //TODO: TSSI2dbm workaround
-                b43_phy_xmitpower(dev); //FIXME correct?
-                break;
-        case B43_PHYTYPE_G:
-                if ((channel < 1) || (channel > 14)) {
-                        err = -EINVAL;
-                        goto out;
-                }
-
-                if (synthetic_pu_workaround)
-                        b43_synth_pu_workaround(dev, channel);
-
-                b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
-
-                if (channel == 14) {
-                        if (dev->dev->bus->sprom.country_code ==
-                            SSB_SPROM1CCODE_JAPAN)
-                                b43_hf_write(dev,
-                                             b43_hf_read(dev) & ~B43_HF_ACPR);
-                        else
-                                b43_hf_write(dev,
-                                             b43_hf_read(dev) | B43_HF_ACPR);
-                        b43_write16(dev, B43_MMIO_CHANNEL_EXT,
-                                    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
-                                    | (1 << 11));
-                } else {
-                        b43_write16(dev, B43_MMIO_CHANNEL_EXT,
-                                    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
-                                    & 0xF7BF);
-                }
-                break;
-        case B43_PHYTYPE_N:
-                err = b43_nphy_selectchannel(dev, channel);
-                if (err)
-                        goto out;
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-
-        phy->channel = channel;
-        /* Wait for the radio to tune to the channel and stabilize. */
-        msleep(8);
-out:
-        if (err) {
-                b43_shm_write16(dev, B43_SHM_SHARED,
-                                B43_SHM_SH_CHAN, savedcookie);
-        }
-        return err;
-}
-
 void b43_radio_turn_on(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
@@ -3843,21 +450,7 @@ void b43_radio_turn_on(struct b43_wldev *dev)
                 break;
         case B43_PHYTYPE_B:
         case B43_PHYTYPE_G:
-                b43_phy_write(dev, 0x0015, 0x8000);
-                b43_phy_write(dev, 0x0015, 0xCC00);
-                b43_phy_write(dev, 0x0015, (phy->gmode ? 0x00C0 : 0x0000));
-                if (phy->radio_off_context.valid) {
-                        /* Restore the RFover values. */
-                        b43_phy_write(dev, B43_PHY_RFOVER,
-                                      phy->radio_off_context.rfover);
-                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
-                                      phy->radio_off_context.rfoverval);
-                        phy->radio_off_context.valid = 0;
-                }
-                channel = phy->channel;
-                err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_BG, 1);
-                err |= b43_radio_selectchannel(dev, channel, 0);
-                B43_WARN_ON(err);
+                //XXX
                 break;
         case B43_PHYTYPE_N:
                 b43_nphy_radio_turn_on(dev);
@@ -3886,17 +479,7 @@ void b43_radio_turn_off(struct b43_wldev *dev, bool force)
                 b43_phy_write(dev, 0x0011, b43_phy_read(dev, 0x0011) | 0x0008);
                 break;
         case B43_PHYTYPE_G: {
-                u16 rfover, rfoverval;
-
-                rfover = b43_phy_read(dev, B43_PHY_RFOVER);
-                rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
-                if (!force) {
-                        phy->radio_off_context.rfover = rfover;
-                        phy->radio_off_context.rfoverval = rfoverval;
-                        phy->radio_off_context.valid = 1;
-                }
-                b43_phy_write(dev, B43_PHY_RFOVER, rfover | 0x008C);
-                b43_phy_write(dev, B43_PHY_RFOVERVAL, rfoverval & 0xFF73);
+                //XXX
                 break;
         }
         default:
diff --git a/drivers/net/wireless/b43/phy.h b/drivers/net/wireless/b43/phy.h
deleted file mode 100644
index 4aab10903529..000000000000
--- a/drivers/net/wireless/b43/phy.h
+++ /dev/null
@@ -1,340 +0,0 @@
-#ifndef B43_PHY_H_
-#define B43_PHY_H_
-
-#include <linux/types.h>
-
-struct b43_wldev;
-struct b43_phy;
-
-/*** PHY Registers ***/
-
-/* Routing */
-#define B43_PHYROUTE                    0x0C00 /* PHY register routing bits mask */
-#define  B43_PHYROUTE_BASE              0x0000 /* Base registers */
-#define  B43_PHYROUTE_OFDM_GPHY         0x0400 /* OFDM register routing for G-PHYs */
-#define  B43_PHYROUTE_EXT_GPHY          0x0800 /* Extended G-PHY registers */
-#define  B43_PHYROUTE_N_BMODE           0x0C00 /* N-PHY BMODE registers */
-
-/* CCK (B-PHY) registers. */
-#define B43_PHY_CCK(reg)                ((reg) | B43_PHYROUTE_BASE)
-/* N-PHY registers. */
-#define B43_PHY_N(reg)                  ((reg) | B43_PHYROUTE_BASE)
-/* N-PHY BMODE registers. */
-#define B43_PHY_N_BMODE(reg)            ((reg) | B43_PHYROUTE_N_BMODE)
-/* OFDM (A-PHY) registers. */
-#define B43_PHY_OFDM(reg)               ((reg) | B43_PHYROUTE_OFDM_GPHY)
-/* Extended G-PHY registers. */
-#define B43_PHY_EXTG(reg)               ((reg) | B43_PHYROUTE_EXT_GPHY)
-
-/* OFDM (A) PHY Registers */
-#define B43_PHY_VERSION_OFDM            B43_PHY_OFDM(0x00)      /* Versioning register for A-PHY */
-#define B43_PHY_BBANDCFG                B43_PHY_OFDM(0x01)      /* Baseband config */
-#define  B43_PHY_BBANDCFG_RXANT         0x180   /* RX Antenna selection */
-#define  B43_PHY_BBANDCFG_RXANT_SHIFT   7
-#define B43_PHY_PWRDOWN                 B43_PHY_OFDM(0x03)      /* Powerdown */
-#define B43_PHY_CRSTHRES1_R1            B43_PHY_OFDM(0x06)      /* CRS Threshold 1 (phy.rev 1 only) */
-#define B43_PHY_LNAHPFCTL               B43_PHY_OFDM(0x1C)      /* LNA/HPF control */
-#define B43_PHY_LPFGAINCTL              B43_PHY_OFDM(0x20)      /* LPF Gain control */
-#define B43_PHY_ADIVRELATED             B43_PHY_OFDM(0x27)      /* FIXME rename */
-#define B43_PHY_CRS0                    B43_PHY_OFDM(0x29)
-#define  B43_PHY_CRS0_EN                0x4000
-#define B43_PHY_PEAK_COUNT              B43_PHY_OFDM(0x30)
-#define B43_PHY_ANTDWELL                B43_PHY_OFDM(0x2B)      /* Antenna dwell */
-#define  B43_PHY_ANTDWELL_AUTODIV1      0x0100  /* Automatic RX diversity start antenna */
-#define B43_PHY_ENCORE                  B43_PHY_OFDM(0x49)      /* "Encore" (RangeMax / BroadRange) */
-#define  B43_PHY_ENCORE_EN              0x0200  /* Encore enable */
-#define B43_PHY_LMS                     B43_PHY_OFDM(0x55)
-#define B43_PHY_OFDM61                  B43_PHY_OFDM(0x61)      /* FIXME rename */
-#define  B43_PHY_OFDM61_10              0x0010  /* FIXME rename */
-#define B43_PHY_IQBAL                   B43_PHY_OFDM(0x69)      /* I/Q balance */
-#define B43_PHY_BBTXDC_BIAS             B43_PHY_OFDM(0x6B)      /* Baseband TX DC bias */
-#define B43_PHY_OTABLECTL               B43_PHY_OFDM(0x72)      /* OFDM table control (see below) */
-#define  B43_PHY_OTABLEOFF              0x03FF  /* OFDM table offset (see below) */
-#define  B43_PHY_OTABLENR               0xFC00  /* OFDM table number (see below) */
-#define  B43_PHY_OTABLENR_SHIFT         10
-#define B43_PHY_OTABLEI                 B43_PHY_OFDM(0x73)      /* OFDM table data I */
-#define B43_PHY_OTABLEQ                 B43_PHY_OFDM(0x74)      /* OFDM table data Q */
-#define B43_PHY_HPWR_TSSICTL            B43_PHY_OFDM(0x78)      /* Hardware power TSSI control */
-#define B43_PHY_ADCCTL                  B43_PHY_OFDM(0x7A)      /* ADC control */
-#define B43_PHY_IDLE_TSSI               B43_PHY_OFDM(0x7B)
-#define B43_PHY_A_TEMP_SENSE            B43_PHY_OFDM(0x7C)      /* A PHY temperature sense */
-#define B43_PHY_NRSSITHRES              B43_PHY_OFDM(0x8A)      /* NRSSI threshold */
-#define B43_PHY_ANTWRSETT               B43_PHY_OFDM(0x8C)      /* Antenna WR settle */
-#define  B43_PHY_ANTWRSETT_ARXDIV       0x2000  /* Automatic RX diversity enabled */
-#define B43_PHY_CLIPPWRDOWNT            B43_PHY_OFDM(0x93)      /* Clip powerdown threshold */
-#define B43_PHY_OFDM9B                  B43_PHY_OFDM(0x9B)      /* FIXME rename */
-#define B43_PHY_N1P1GAIN                B43_PHY_OFDM(0xA0)
-#define B43_PHY_P1P2GAIN                B43_PHY_OFDM(0xA1)
-#define B43_PHY_N1N2GAIN                B43_PHY_OFDM(0xA2)
-#define B43_PHY_CLIPTHRES               B43_PHY_OFDM(0xA3)
-#define B43_PHY_CLIPN1P2THRES           B43_PHY_OFDM(0xA4)
-#define B43_PHY_CCKSHIFTBITS_WA         B43_PHY_OFDM(0xA5)      /* CCK shiftbits workaround, FIXME rename */
-#define B43_PHY_CCKSHIFTBITS            B43_PHY_OFDM(0xA7)      /* FIXME rename */
-#define B43_PHY_DIVSRCHIDX              B43_PHY_OFDM(0xA8)      /* Divider search gain/index */
-#define B43_PHY_CLIPP2THRES             B43_PHY_OFDM(0xA9)
-#define B43_PHY_CLIPP3THRES             B43_PHY_OFDM(0xAA)
-#define B43_PHY_DIVP1P2GAIN             B43_PHY_OFDM(0xAB)
-#define B43_PHY_DIVSRCHGAINBACK         B43_PHY_OFDM(0xAD)      /* Divider search gain back */
-#define B43_PHY_DIVSRCHGAINCHNG         B43_PHY_OFDM(0xAE)      /* Divider search gain change */
-#define B43_PHY_CRSTHRES1               B43_PHY_OFDM(0xC0)      /* CRS Threshold 1 (phy.rev >= 2 only) */
-#define B43_PHY_CRSTHRES2               B43_PHY_OFDM(0xC1)      /* CRS Threshold 2 (phy.rev >= 2 only) */
-#define B43_PHY_TSSIP_LTBASE            B43_PHY_OFDM(0x380)     /* TSSI power lookup table base */
-#define B43_PHY_DC_LTBASE               B43_PHY_OFDM(0x3A0)     /* DC lookup table base */
-#define B43_PHY_GAIN_LTBASE             B43_PHY_OFDM(0x3C0)     /* Gain lookup table base */
-
-/* CCK (B) PHY Registers */
-#define B43_PHY_VERSION_CCK             B43_PHY_CCK(0x00)       /* Versioning register for B-PHY */
-#define B43_PHY_CCKBBANDCFG             B43_PHY_CCK(0x01)       /* Contains antenna 0/1 control bit */
-#define B43_PHY_PGACTL                  B43_PHY_CCK(0x15)       /* PGA control */
-#define  B43_PHY_PGACTL_LPF             0x1000  /* Low pass filter (?) */
-#define  B43_PHY_PGACTL_LOWBANDW        0x0040  /* Low bandwidth flag */
-#define  B43_PHY_PGACTL_UNKNOWN         0xEFA0
-#define B43_PHY_FBCTL1                  B43_PHY_CCK(0x18)       /* Frequency bandwidth control 1 */
-#define B43_PHY_ITSSI                   B43_PHY_CCK(0x29)       /* Idle TSSI */
-#define B43_PHY_LO_LEAKAGE              B43_PHY_CCK(0x2D)       /* Measured LO leakage */
-#define B43_PHY_ENERGY                  B43_PHY_CCK(0x33)       /* Energy */
-#define B43_PHY_SYNCCTL                 B43_PHY_CCK(0x35)
-#define B43_PHY_FBCTL2                  B43_PHY_CCK(0x38)       /* Frequency bandwidth control 2 */
-#define B43_PHY_DACCTL                  B43_PHY_CCK(0x60)       /* DAC control */
-#define B43_PHY_RCCALOVER               B43_PHY_CCK(0x78)       /* RC calibration override */
-
-/* Extended G-PHY Registers */
-#define B43_PHY_CLASSCTL                B43_PHY_EXTG(0x02)      /* Classify control */
-#define B43_PHY_GTABCTL                 B43_PHY_EXTG(0x03)      /* G-PHY table control (see below) */
-#define  B43_PHY_GTABOFF                0x03FF  /* G-PHY table offset (see below) */
-#define  B43_PHY_GTABNR                 0xFC00  /* G-PHY table number (see below) */
-#define  B43_PHY_GTABNR_SHIFT           10
-#define B43_PHY_GTABDATA                B43_PHY_EXTG(0x04)      /* G-PHY table data */
-#define B43_PHY_LO_MASK                 B43_PHY_EXTG(0x0F)      /* Local Oscillator control mask */
-#define B43_PHY_LO_CTL                  B43_PHY_EXTG(0x10)      /* Local Oscillator control */
-#define B43_PHY_RFOVER                  B43_PHY_EXTG(0x11)      /* RF override */
-#define B43_PHY_RFOVERVAL               B43_PHY_EXTG(0x12)      /* RF override value */
-#define  B43_PHY_RFOVERVAL_EXTLNA       0x8000
-#define  B43_PHY_RFOVERVAL_LNA          0x7000
-#define  B43_PHY_RFOVERVAL_LNA_SHIFT    12
-#define  B43_PHY_RFOVERVAL_PGA          0x0F00
-#define  B43_PHY_RFOVERVAL_PGA_SHIFT    8
-#define  B43_PHY_RFOVERVAL_UNK          0x0010  /* Unknown, always set. */
-#define  B43_PHY_RFOVERVAL_TRSWRX       0x00E0
-#define  B43_PHY_RFOVERVAL_BW           0x0003  /* Bandwidth flags */
-#define   B43_PHY_RFOVERVAL_BW_LPF      0x0001  /* Low Pass Filter */
-#define   B43_PHY_RFOVERVAL_BW_LBW      0x0002  /* Low Bandwidth (when set), high when unset */
-#define B43_PHY_ANALOGOVER              B43_PHY_EXTG(0x14)      /* Analog override */
-#define B43_PHY_ANALOGOVERVAL           B43_PHY_EXTG(0x15)      /* Analog override value */
-
-/*** OFDM table numbers ***/
-#define B43_OFDMTAB(number, offset)     (((number) << B43_PHY_OTABLENR_SHIFT) | (offset))
-#define B43_OFDMTAB_AGC1                B43_OFDMTAB(0x00, 0)
-#define B43_OFDMTAB_GAIN0               B43_OFDMTAB(0x00, 0)
-#define B43_OFDMTAB_GAINX               B43_OFDMTAB(0x01, 0)    //TODO rename
-#define B43_OFDMTAB_GAIN1               B43_OFDMTAB(0x01, 4)
-#define B43_OFDMTAB_AGC3                B43_OFDMTAB(0x02, 0)
-#define B43_OFDMTAB_GAIN2               B43_OFDMTAB(0x02, 3)
-#define B43_OFDMTAB_LNAHPFGAIN1         B43_OFDMTAB(0x03, 0)
-#define B43_OFDMTAB_WRSSI               B43_OFDMTAB(0x04, 0)
-#define B43_OFDMTAB_LNAHPFGAIN2         B43_OFDMTAB(0x04, 0)
-#define B43_OFDMTAB_NOISESCALE          B43_OFDMTAB(0x05, 0)
-#define B43_OFDMTAB_AGC2                B43_OFDMTAB(0x06, 0)
-#define B43_OFDMTAB_ROTOR               B43_OFDMTAB(0x08, 0)
-#define B43_OFDMTAB_ADVRETARD           B43_OFDMTAB(0x09, 0)
-#define B43_OFDMTAB_DAC                 B43_OFDMTAB(0x0C, 0)
-#define B43_OFDMTAB_DC                  B43_OFDMTAB(0x0E, 7)
-#define B43_OFDMTAB_PWRDYN2             B43_OFDMTAB(0x0E, 12)
-#define B43_OFDMTAB_LNAGAIN             B43_OFDMTAB(0x0E, 13)
-#define B43_OFDMTAB_UNKNOWN_0F          B43_OFDMTAB(0x0F, 0)    //TODO rename
-#define B43_OFDMTAB_UNKNOWN_APHY        B43_OFDMTAB(0x0F, 7)    //TODO rename
-#define B43_OFDMTAB_LPFGAIN             B43_OFDMTAB(0x0F, 12)
-#define B43_OFDMTAB_RSSI                B43_OFDMTAB(0x10, 0)
-#define B43_OFDMTAB_UNKNOWN_11          B43_OFDMTAB(0x11, 4)    //TODO rename
-#define B43_OFDMTAB_AGC1_R1             B43_OFDMTAB(0x13, 0)
-#define B43_OFDMTAB_GAINX_R1            B43_OFDMTAB(0x14, 0)    //TODO remove!
-#define B43_OFDMTAB_MINSIGSQ            B43_OFDMTAB(0x14, 0)
-#define B43_OFDMTAB_AGC3_R1             B43_OFDMTAB(0x15, 0)
-#define B43_OFDMTAB_WRSSI_R1            B43_OFDMTAB(0x15, 4)
-#define B43_OFDMTAB_TSSI                B43_OFDMTAB(0x15, 0)
-#define B43_OFDMTAB_DACRFPABB           B43_OFDMTAB(0x16, 0)
-#define B43_OFDMTAB_DACOFF              B43_OFDMTAB(0x17, 0)
-#define B43_OFDMTAB_DCBIAS              B43_OFDMTAB(0x18, 0)
-
-u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset);
-void b43_ofdmtab_write16(struct b43_wldev *dev, u16 table,
-                         u16 offset, u16 value);
-u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset);
-void b43_ofdmtab_write32(struct b43_wldev *dev, u16 table,
-                         u16 offset, u32 value);
-
-/*** G-PHY table numbers */
-#define B43_GTAB(number, offset)        (((number) << B43_PHY_GTABNR_SHIFT) | (offset))
-#define B43_GTAB_NRSSI                  B43_GTAB(0x00, 0)
-#define B43_GTAB_TRFEMW                 B43_GTAB(0x0C, 0x120)
-#define B43_GTAB_ORIGTR                 B43_GTAB(0x2E, 0x298)
-
-u16 b43_gtab_read(struct b43_wldev *dev, u16 table, u16 offset);        //TODO implement
-void b43_gtab_write(struct b43_wldev *dev, u16 table, u16 offset, u16 value);   //TODO implement
-
-#define B43_DEFAULT_CHANNEL_A   36
-#define B43_DEFAULT_CHANNEL_BG  6
-
-enum {
-        B43_ANTENNA0,           /* Antenna 0 */
-        B43_ANTENNA1,           /* Antenna 0 */
-        B43_ANTENNA_AUTO1,      /* Automatic, starting with antenna 1 */
-        B43_ANTENNA_AUTO0,      /* Automatic, starting with antenna 0 */
-        B43_ANTENNA2,
-        B43_ANTENNA3 = 8,
-
-        B43_ANTENNA_AUTO = B43_ANTENNA_AUTO0,
-        B43_ANTENNA_DEFAULT = B43_ANTENNA_AUTO,
-};
-
-enum {
-        B43_INTERFMODE_NONE,
-        B43_INTERFMODE_NONWLAN,
-        B43_INTERFMODE_MANUALWLAN,
-        B43_INTERFMODE_AUTOWLAN,
-};
-
-/* Masks for the different PHY versioning registers. */
-#define B43_PHYVER_ANALOG               0xF000
-#define B43_PHYVER_ANALOG_SHIFT         12
-#define B43_PHYVER_TYPE                 0x0F00
-#define B43_PHYVER_TYPE_SHIFT           8
-#define B43_PHYVER_VERSION              0x00FF
-
-void b43_phy_lock(struct b43_wldev *dev);
-void b43_phy_unlock(struct b43_wldev *dev);
-
-
-/* Read a value from a PHY register */
-u16 b43_phy_read(struct b43_wldev *dev, u16 offset);
-/* Write a value to a PHY register */
-void b43_phy_write(struct b43_wldev *dev, u16 offset, u16 val);
-/* Mask a PHY register with a mask */
-void b43_phy_mask(struct b43_wldev *dev, u16 offset, u16 mask);
-/* OR a PHY register with a bitmap */
-void b43_phy_set(struct b43_wldev *dev, u16 offset, u16 set);
-/* Mask and OR a PHY register with a mask and bitmap */
-void b43_phy_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set);
-
-
-int b43_phy_init_tssi2dbm_table(struct b43_wldev *dev);
-
-void b43_phy_early_init(struct b43_wldev *dev);
-int b43_phy_init(struct b43_wldev *dev);
-
-void b43_set_rx_antenna(struct b43_wldev *dev, int antenna);
-
-void b43_phy_xmitpower(struct b43_wldev *dev);
-
-/* Returns the boolean whether the board has HardwarePowerControl */
-bool b43_has_hardware_pctl(struct b43_phy *phy);
-/* Returns the boolean whether "TX Magnification" is enabled. */
-#define has_tx_magnification(phy) \
-        (((phy)->rev >= 2) &&                   \
-         ((phy)->radio_ver == 0x2050) &&        \
-         ((phy)->radio_rev == 8))
-/* Card uses the loopback gain stuff */
-#define has_loopback_gain(phy) \
-        (((phy)->rev > 1) || ((phy)->gmode))
-
-/* Radio Attenuation (RF Attenuation) */
-struct b43_rfatt {
-        u8 att;                 /* Attenuation value */
-        bool with_padmix;       /* Flag, PAD Mixer enabled. */
-};
-struct b43_rfatt_list {
-        /* Attenuation values list */
-        const struct b43_rfatt *list;
-        u8 len;
-        /* Minimum/Maximum attenuation values */
-        u8 min_val;
-        u8 max_val;
-};
-
-/* Returns true, if the values are the same. */
-static inline bool b43_compare_rfatt(const struct b43_rfatt *a,
-                                     const struct b43_rfatt *b)
-{
-        return ((a->att == b->att) &&
-                (a->with_padmix == b->with_padmix));
-}
-
-/* Baseband Attenuation */
-struct b43_bbatt {
-        u8 att;                 /* Attenuation value */
-};
-struct b43_bbatt_list {
-        /* Attenuation values list */
-        const struct b43_bbatt *list;
-        u8 len;
-        /* Minimum/Maximum attenuation values */
-        u8 min_val;
-        u8 max_val;
-};
-
-/* Returns true, if the values are the same. */
-static inline bool b43_compare_bbatt(const struct b43_bbatt *a,
-                                     const struct b43_bbatt *b)
-{
-        return (a->att == b->att);
-}
-
-/* tx_control bits. */
-#define B43_TXCTL_PA3DB         0x40    /* PA Gain 3dB */
-#define B43_TXCTL_PA2DB         0x20    /* PA Gain 2dB */
-#define B43_TXCTL_TXMIX         0x10    /* TX Mixer Gain */
-
-/* Write BasebandAttenuation value to the device. */
-void b43_phy_set_baseband_attenuation(struct b43_wldev *dev,
-                                      u16 baseband_attenuation);
-
-extern const u8 b43_radio_channel_codes_bg[];
-
-void b43_radio_lock(struct b43_wldev *dev);
-void b43_radio_unlock(struct b43_wldev *dev);
-
-
-/* Read a value from a 16bit radio register */
-u16 b43_radio_read16(struct b43_wldev *dev, u16 offset);
-/* Write a value to a 16bit radio register */
-void b43_radio_write16(struct b43_wldev *dev, u16 offset, u16 val);
-/* Mask a 16bit radio register with a mask */
-void b43_radio_mask(struct b43_wldev *dev, u16 offset, u16 mask);
-/* OR a 16bit radio register with a bitmap */
-void b43_radio_set(struct b43_wldev *dev, u16 offset, u16 set);
-/* Mask and OR a PHY register with a mask and bitmap */
-void b43_radio_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set);
-
-
-u16 b43_radio_init2050(struct b43_wldev *dev);
-void b43_radio_init2060(struct b43_wldev *dev);
-
-void b43_radio_turn_on(struct b43_wldev *dev);
-void b43_radio_turn_off(struct b43_wldev *dev, bool force);
-
-int b43_radio_selectchannel(struct b43_wldev *dev, u8 channel,
-                            int synthetic_pu_workaround);
-
-u8 b43_radio_aci_detect(struct b43_wldev *dev, u8 channel);
-u8 b43_radio_aci_scan(struct b43_wldev *dev);
-
-int b43_radio_set_interference_mitigation(struct b43_wldev *dev, int mode);
-
-void b43_calc_nrssi_slope(struct b43_wldev *dev);
-void b43_calc_nrssi_threshold(struct b43_wldev *dev);
-s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset);
-void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val);
-void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val);
-void b43_nrssi_mem_update(struct b43_wldev *dev);
-
-void b43_radio_set_tx_iq(struct b43_wldev *dev);
-u16 b43_radio_calibrationvalue(struct b43_wldev *dev);
-
-void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
-                                     int *_bbatt, int *_rfatt);
-
-void b43_set_txpower_g(struct b43_wldev *dev,
-                       const struct b43_bbatt *bbatt,
-                       const struct b43_rfatt *rfatt, u8 tx_control);
-
-#endif /* B43_PHY_H_ */
diff --git a/drivers/net/wireless/b43/phy_a.c b/drivers/net/wireless/b43/phy_a.c
new file mode 100644
index 000000000000..4d7d59e30960
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_a.c
@@ -0,0 +1,543 @@
+/*
+
+  Broadcom B43 wireless driver
+  IEEE 802.11a PHY driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
+  Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "phy_a.h"
+#include "phy_common.h"
+#include "wa.h"
+#include "tables.h"
+#include "main.h"
+
+
+/* Get the freq, as it has to be written to the device. */
+static inline u16 channel2freq_a(u8 channel)
+{
+        B43_WARN_ON(channel > 200);
+
+        return (5000 + 5 * channel);
+}
+
+static inline u16 freq_r3A_value(u16 frequency)
+{
+        u16 value;
+
+        if (frequency < 5091)
+                value = 0x0040;
+        else if (frequency < 5321)
+                value = 0x0000;
+        else if (frequency < 5806)
+                value = 0x0080;
+        else
+                value = 0x0040;
+
+        return value;
+}
+
+void b43_radio_set_tx_iq(struct b43_wldev *dev)
+{
+        static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
+        static const u8 data_low[5] = { 0x00, 0x01, 0x05, 0x06, 0x0A };
+        u16 tmp = b43_radio_read16(dev, 0x001E);
+        int i, j;
+
+        for (i = 0; i < 5; i++) {
+                for (j = 0; j < 5; j++) {
+                        if (tmp == (data_high[i] << 4 | data_low[j])) {
+                                b43_phy_write(dev, 0x0069,
+                                              (i - j) << 8 | 0x00C0);
+                                return;
+                        }
+                }
+        }
+}
+
+static void aphy_channel_switch(struct b43_wldev *dev, unsigned int channel)
+{
+        u16 freq, r8, tmp;
+
+        freq = channel2freq_a(channel);
+
+        r8 = b43_radio_read16(dev, 0x0008);
+        b43_write16(dev, 0x03F0, freq);
+        b43_radio_write16(dev, 0x0008, r8);
+
+        //TODO: write max channel TX power? to Radio 0x2D
+        tmp = b43_radio_read16(dev, 0x002E);
+        tmp &= 0x0080;
+        //TODO: OR tmp with the Power out estimation for this channel?
+        b43_radio_write16(dev, 0x002E, tmp);
+
+        if (freq >= 4920 && freq <= 5500) {
+                /*
+                 * r8 = (((freq * 15 * 0xE1FC780F) >> 32) / 29) & 0x0F;
+                 *    = (freq * 0.025862069
+                 */
+                r8 = 3 * freq / 116;    /* is equal to r8 = freq * 0.025862 */
+        }
+        b43_radio_write16(dev, 0x0007, (r8 << 4) | r8);
+        b43_radio_write16(dev, 0x0020, (r8 << 4) | r8);
+        b43_radio_write16(dev, 0x0021, (r8 << 4) | r8);
+        b43_radio_write16(dev, 0x0022, (b43_radio_read16(dev, 0x0022)
+                                        & 0x000F) | (r8 << 4));
+        b43_radio_write16(dev, 0x002A, (r8 << 4));
+        b43_radio_write16(dev, 0x002B, (r8 << 4));
+        b43_radio_write16(dev, 0x0008, (b43_radio_read16(dev, 0x0008)
+                                        & 0x00F0) | (r8 << 4));
+        b43_radio_write16(dev, 0x0029, (b43_radio_read16(dev, 0x0029)
+                                        & 0xFF0F) | 0x00B0);
+        b43_radio_write16(dev, 0x0035, 0x00AA);
+        b43_radio_write16(dev, 0x0036, 0x0085);
+        b43_radio_write16(dev, 0x003A, (b43_radio_read16(dev, 0x003A)
+                                        & 0xFF20) |
+                          freq_r3A_value(freq));
+        b43_radio_write16(dev, 0x003D,
+                          b43_radio_read16(dev, 0x003D) & 0x00FF);
+        b43_radio_write16(dev, 0x0081, (b43_radio_read16(dev, 0x0081)
+                                        & 0xFF7F) | 0x0080);
+        b43_radio_write16(dev, 0x0035,
+                          b43_radio_read16(dev, 0x0035) & 0xFFEF);
+        b43_radio_write16(dev, 0x0035, (b43_radio_read16(dev, 0x0035)
+                                        & 0xFFEF) | 0x0010);
+        b43_radio_set_tx_iq(dev);
+        //TODO: TSSI2dbm workaround
+//FIXME b43_phy_xmitpower(dev);
+}
+
+void b43_radio_init2060(struct b43_wldev *dev)
+{
+        b43_radio_write16(dev, 0x0004, 0x00C0);
+        b43_radio_write16(dev, 0x0005, 0x0008);
+        b43_radio_write16(dev, 0x0009, 0x0040);
+        b43_radio_write16(dev, 0x0005, 0x00AA);
+        b43_radio_write16(dev, 0x0032, 0x008F);
+        b43_radio_write16(dev, 0x0006, 0x008F);
+        b43_radio_write16(dev, 0x0034, 0x008F);
+        b43_radio_write16(dev, 0x002C, 0x0007);
+        b43_radio_write16(dev, 0x0082, 0x0080);
+        b43_radio_write16(dev, 0x0080, 0x0000);
+        b43_radio_write16(dev, 0x003F, 0x00DA);
+        b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
+        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0010);
+        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
+        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
+        msleep(1);              /* delay 400usec */
+
+        b43_radio_write16(dev, 0x0081,
+                          (b43_radio_read16(dev, 0x0081) & ~0x0020) | 0x0010);
+        msleep(1);              /* delay 400usec */
+
+        b43_radio_write16(dev, 0x0005,
+                          (b43_radio_read16(dev, 0x0005) & ~0x0008) | 0x0008);
+        b43_radio_write16(dev, 0x0085, b43_radio_read16(dev, 0x0085) & ~0x0010);
+        b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
+        b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0040);
+        b43_radio_write16(dev, 0x0081,
+                          (b43_radio_read16(dev, 0x0081) & ~0x0040) | 0x0040);
+        b43_radio_write16(dev, 0x0005,
+                          (b43_radio_read16(dev, 0x0081) & ~0x0008) | 0x0008);
+        b43_phy_write(dev, 0x0063, 0xDDC6);
+        b43_phy_write(dev, 0x0069, 0x07BE);
+        b43_phy_write(dev, 0x006A, 0x0000);
+
+        aphy_channel_switch(dev, dev->phy.ops->get_default_chan(dev));
+
+        msleep(1);
+}
+
+static void b43_phy_rssiagc(struct b43_wldev *dev, u8 enable)
+{
+        int i;
+
+        if (dev->phy.rev < 3) {
+                if (enable)
+                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_LNAHPFGAIN1, i, 0xFFF8);
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_WRSSI, i, 0xFFF8);
+                        }
+                else
+                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++) {
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_LNAHPFGAIN1, i, b43_tab_rssiagc1[i]);
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc1[i]);
+                        }
+        } else {
+                if (enable)
+                        for (i = 0; i < B43_TAB_RSSIAGC1_SIZE; i++)
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_WRSSI, i, 0x0820);
+                else
+                        for (i = 0; i < B43_TAB_RSSIAGC2_SIZE; i++)
+                                b43_ofdmtab_write16(dev,
+                                        B43_OFDMTAB_WRSSI, i, b43_tab_rssiagc2[i]);
+        }
+}
+
+static void b43_phy_ww(struct b43_wldev *dev)
+{
+        u16 b, curr_s, best_s = 0xFFFF;
+        int i;
+
+        b43_phy_write(dev, B43_PHY_CRS0,
+                b43_phy_read(dev, B43_PHY_CRS0) & ~B43_PHY_CRS0_EN);
+        b43_phy_write(dev, B43_PHY_OFDM(0x1B),
+                b43_phy_read(dev, B43_PHY_OFDM(0x1B)) | 0x1000);
+        b43_phy_write(dev, B43_PHY_OFDM(0x82),
+                (b43_phy_read(dev, B43_PHY_OFDM(0x82)) & 0xF0FF) | 0x0300);
+        b43_radio_write16(dev, 0x0009,
+                b43_radio_read16(dev, 0x0009) | 0x0080);
+        b43_radio_write16(dev, 0x0012,
+                (b43_radio_read16(dev, 0x0012) & 0xFFFC) | 0x0002);
+        b43_wa_initgains(dev);
+        b43_phy_write(dev, B43_PHY_OFDM(0xBA), 0x3ED5);
+        b = b43_phy_read(dev, B43_PHY_PWRDOWN);
+        b43_phy_write(dev, B43_PHY_PWRDOWN, (b & 0xFFF8) | 0x0005);
+        b43_radio_write16(dev, 0x0004,
+                b43_radio_read16(dev, 0x0004) | 0x0004);
+        for (i = 0x10; i <= 0x20; i++) {
+                b43_radio_write16(dev, 0x0013, i);
+                curr_s = b43_phy_read(dev, B43_PHY_OTABLEQ) & 0x00FF;
+                if (!curr_s) {
+                        best_s = 0x0000;
+                        break;
+                } else if (curr_s >= 0x0080)
+                        curr_s = 0x0100 - curr_s;
+                if (curr_s < best_s)
+                        best_s = curr_s;
+        }
+        b43_phy_write(dev, B43_PHY_PWRDOWN, b);
+        b43_radio_write16(dev, 0x0004,
+                b43_radio_read16(dev, 0x0004) & 0xFFFB);
+        b43_radio_write16(dev, 0x0013, best_s);
+        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1_R1, 0, 0xFFEC);
+        b43_phy_write(dev, B43_PHY_OFDM(0xB7), 0x1E80);
+        b43_phy_write(dev, B43_PHY_OFDM(0xB6), 0x1C00);
+        b43_phy_write(dev, B43_PHY_OFDM(0xB5), 0x0EC0);
+        b43_phy_write(dev, B43_PHY_OFDM(0xB2), 0x00C0);
+        b43_phy_write(dev, B43_PHY_OFDM(0xB9), 0x1FFF);
+        b43_phy_write(dev, B43_PHY_OFDM(0xBB),
+                (b43_phy_read(dev, B43_PHY_OFDM(0xBB)) & 0xF000) | 0x0053);
+        b43_phy_write(dev, B43_PHY_OFDM61,
+                (b43_phy_read(dev, B43_PHY_OFDM61) & 0xFE1F) | 0x0120);
+        b43_phy_write(dev, B43_PHY_OFDM(0x13),
+                (b43_phy_read(dev, B43_PHY_OFDM(0x13)) & 0x0FFF) | 0x3000);
+        b43_phy_write(dev, B43_PHY_OFDM(0x14),
+                (b43_phy_read(dev, B43_PHY_OFDM(0x14)) & 0x0FFF) | 0x3000);
+        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 6, 0x0017);
+        for (i = 0; i < 6; i++)
+                b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, i, 0x000F);
+        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0D, 0x000E);
+        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0E, 0x0011);
+        b43_ofdmtab_write16(dev, B43_OFDMTAB_AGC1, 0x0F, 0x0013);
+        b43_phy_write(dev, B43_PHY_OFDM(0x33), 0x5030);
+        b43_phy_write(dev, B43_PHY_CRS0,
+                b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
+}
+
+static void hardware_pctl_init_aphy(struct b43_wldev *dev)
+{
+        //TODO
+}
+
+void b43_phy_inita(struct b43_wldev *dev)
+{
+        struct ssb_bus *bus = dev->dev->bus;
+        struct b43_phy *phy = &dev->phy;
+
+        /* This lowlevel A-PHY init is also called from G-PHY init.
+         * So we must not access phy->a, if called from G-PHY code.
+         */
+        B43_WARN_ON((phy->type != B43_PHYTYPE_A) &&
+                    (phy->type != B43_PHYTYPE_G));
+
+        might_sleep();
+
+        if (phy->rev >= 6) {
+                if (phy->type == B43_PHYTYPE_A)
+                        b43_phy_write(dev, B43_PHY_OFDM(0x1B),
+                                b43_phy_read(dev, B43_PHY_OFDM(0x1B)) & ~0x1000);
+                if (b43_phy_read(dev, B43_PHY_ENCORE) & B43_PHY_ENCORE_EN)
+                        b43_phy_write(dev, B43_PHY_ENCORE,
+                                b43_phy_read(dev, B43_PHY_ENCORE) | 0x0010);
+                else
+                        b43_phy_write(dev, B43_PHY_ENCORE,
+                                b43_phy_read(dev, B43_PHY_ENCORE) & ~0x1010);
+        }
+
+        b43_wa_all(dev);
+
+        if (phy->type == B43_PHYTYPE_A) {
+                if (phy->gmode && (phy->rev < 3))
+                        b43_phy_write(dev, 0x0034,
+                                b43_phy_read(dev, 0x0034) | 0x0001);
+                b43_phy_rssiagc(dev, 0);
+
+                b43_phy_write(dev, B43_PHY_CRS0,
+                        b43_phy_read(dev, B43_PHY_CRS0) | B43_PHY_CRS0_EN);
+
+                b43_radio_init2060(dev);
+
+                if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+                    ((bus->boardinfo.type == SSB_BOARD_BU4306) ||
+                     (bus->boardinfo.type == SSB_BOARD_BU4309))) {
+                        ; //TODO: A PHY LO
+                }
+
+                if (phy->rev >= 3)
+                        b43_phy_ww(dev);
+
+                hardware_pctl_init_aphy(dev);
+
+                //TODO: radar detection
+        }
+
+        if ((phy->type == B43_PHYTYPE_G) &&
+            (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)) {
+                b43_phy_write(dev, B43_PHY_OFDM(0x6E),
+                                  (b43_phy_read(dev, B43_PHY_OFDM(0x6E))
+                                   & 0xE000) | 0x3CF);
+        }
+}
+
+static int b43_aphy_op_allocate(struct b43_wldev *dev)
+{
+        struct b43_phy_a *aphy;
+
+        aphy = kzalloc(sizeof(*aphy), GFP_KERNEL);
+        if (!aphy)
+                return -ENOMEM;
+        dev->phy.a = aphy;
+
+        //TODO init struct b43_phy_a
+
+        return 0;
+}
+
+static int b43_aphy_op_init(struct b43_wldev *dev)
+{
+        struct b43_phy_a *aphy = dev->phy.a;
+
+        b43_phy_inita(dev);
+        aphy->initialised = 1;
+
+        return 0;
+}
+
+static void b43_aphy_op_exit(struct b43_wldev *dev)
+{
+        struct b43_phy_a *aphy = dev->phy.a;
+
+        if (aphy->initialised) {
+                //TODO
+                aphy->initialised = 0;
+        }
+        //TODO
+        kfree(aphy);
+        dev->phy.a = NULL;
+}
+
+static inline u16 adjust_phyreg(struct b43_wldev *dev, u16 offset)
+{
+        /* OFDM registers are base-registers for the A-PHY. */
+        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_OFDM_GPHY) {
+                offset &= ~B43_PHYROUTE;
+                offset |= B43_PHYROUTE_BASE;
+        }
+
+#if B43_DEBUG
+        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_EXT_GPHY) {
+                /* Ext-G registers are only available on G-PHYs */
+                b43err(dev->wl, "Invalid EXT-G PHY access at "
+                       "0x%04X on A-PHY\n", offset);
+                dump_stack();
+        }
+        if ((offset & B43_PHYROUTE) == B43_PHYROUTE_N_BMODE) {
+                /* N-BMODE registers are only available on N-PHYs */
+                b43err(dev->wl, "Invalid N-BMODE PHY access at "
+                       "0x%04X on A-PHY\n", offset);
+                dump_stack();
+        }
+#endif /* B43_DEBUG */
+
+        return offset;
+}
+
+static u16 b43_aphy_op_read(struct b43_wldev *dev, u16 reg)
+{
+        reg = adjust_phyreg(dev, reg);
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_PHY_DATA);
+}
+
+static void b43_aphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        reg = adjust_phyreg(dev, reg);
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_PHY_DATA, value);
+}
+
+static u16 b43_aphy_op_radio_read(struct b43_wldev *dev, u16 reg)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+        /* A-PHY needs 0x40 for read access */
+        reg |= 0x40;
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
+}
+
+static void b43_aphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
+}
+
+static bool b43_aphy_op_supports_hwpctl(struct b43_wldev *dev)
+{
+        return (dev->phy.rev >= 5);
+}
+
+static void b43_aphy_op_software_rfkill(struct b43_wldev *dev,
+                                        enum rfkill_state state)
+{//TODO
+}
+
+static int b43_aphy_op_switch_channel(struct b43_wldev *dev,
+                                      unsigned int new_channel)
+{
+        if (new_channel > 200)
+                return -EINVAL;
+        aphy_channel_switch(dev, new_channel);
+
+        return 0;
+}
+
+static unsigned int b43_aphy_op_get_default_chan(struct b43_wldev *dev)
+{
+        return 36; /* Default to channel 36 */
+}
+
+static void b43_aphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
+{//TODO
+        struct b43_phy *phy = &dev->phy;
+        u64 hf;
+        u16 tmp;
+        int autodiv = 0;
+
+        if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
+                autodiv = 1;
+
+        hf = b43_hf_read(dev);
+        hf &= ~B43_HF_ANTDIVHELP;
+        b43_hf_write(dev, hf);
+
+        tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
+        tmp &= ~B43_PHY_BBANDCFG_RXANT;
+        tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
+            << B43_PHY_BBANDCFG_RXANT_SHIFT;
+        b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
+
+        if (autodiv) {
+                tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
+                if (antenna == B43_ANTENNA_AUTO0)
+                        tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
+                else
+                        tmp |= B43_PHY_ANTDWELL_AUTODIV1;
+                b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
+        }
+        if (phy->rev < 3) {
+                tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
+                tmp = (tmp & 0xFF00) | 0x24;
+                b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
+        } else {
+                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
+                tmp |= 0x10;
+                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
+                if (phy->analog == 3) {
+                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
+                                      0x1D);
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
+                                      8);
+                } else {
+                        b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
+                                      0x3A);
+                        tmp =
+                            b43_phy_read(dev,
+                                         B43_PHY_ADIVRELATED);
+                        tmp = (tmp & 0xFF00) | 8;
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
+                                      tmp);
+                }
+        }
+
+        hf |= B43_HF_ANTDIVHELP;
+        b43_hf_write(dev, hf);
+}
+
+static void b43_aphy_op_adjust_txpower(struct b43_wldev *dev)
+{//TODO
+}
+
+static enum b43_txpwr_result b43_aphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{//TODO
+        return B43_TXPWR_RES_DONE;
+}
+
+static void b43_aphy_op_pwork_15sec(struct b43_wldev *dev)
+{//TODO
+}
+
+static void b43_aphy_op_pwork_60sec(struct b43_wldev *dev)
+{//TODO
+}
+
+const struct b43_phy_operations b43_phyops_a = {
+        .allocate               = b43_aphy_op_allocate,
+        .init                   = b43_aphy_op_init,
+        .exit                   = b43_aphy_op_exit,
+        .phy_read               = b43_aphy_op_read,
+        .phy_write              = b43_aphy_op_write,
+        .radio_read             = b43_aphy_op_radio_read,
+        .radio_write            = b43_aphy_op_radio_write,
+        .supports_hwpctl        = b43_aphy_op_supports_hwpctl,
+        .software_rfkill        = b43_aphy_op_software_rfkill,
+        .switch_channel         = b43_aphy_op_switch_channel,
+        .get_default_chan       = b43_aphy_op_get_default_chan,
+        .set_rx_antenna         = b43_aphy_op_set_rx_antenna,
+        .recalc_txpower         = b43_aphy_op_recalc_txpower,
+        .adjust_txpower         = b43_aphy_op_adjust_txpower,
+        .pwork_15sec            = b43_aphy_op_pwork_15sec,
+        .pwork_60sec            = b43_aphy_op_pwork_60sec,
+};
diff --git a/drivers/net/wireless/b43/phy_a.h b/drivers/net/wireless/b43/phy_a.h
new file mode 100644
index 000000000000..e8640f7312bf
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_a.h
@@ -0,0 +1,124 @@
+#ifndef LINUX_B43_PHY_A_H_
+#define LINUX_B43_PHY_A_H_
+
+#include "phy_common.h"
+
+
+/* OFDM (A) PHY Registers */
+#define B43_PHY_VERSION_OFDM            B43_PHY_OFDM(0x00)      /* Versioning register for A-PHY */
+#define B43_PHY_BBANDCFG                B43_PHY_OFDM(0x01)      /* Baseband config */
+#define  B43_PHY_BBANDCFG_RXANT         0x180   /* RX Antenna selection */
+#define  B43_PHY_BBANDCFG_RXANT_SHIFT   7
+#define B43_PHY_PWRDOWN                 B43_PHY_OFDM(0x03)      /* Powerdown */
+#define B43_PHY_CRSTHRES1_R1            B43_PHY_OFDM(0x06)      /* CRS Threshold 1 (phy.rev 1 only) */
+#define B43_PHY_LNAHPFCTL               B43_PHY_OFDM(0x1C)      /* LNA/HPF control */
+#define B43_PHY_LPFGAINCTL              B43_PHY_OFDM(0x20)      /* LPF Gain control */
+#define B43_PHY_ADIVRELATED             B43_PHY_OFDM(0x27)      /* FIXME rename */
+#define B43_PHY_CRS0                    B43_PHY_OFDM(0x29)
+#define  B43_PHY_CRS0_EN                0x4000
+#define B43_PHY_PEAK_COUNT              B43_PHY_OFDM(0x30)
+#define B43_PHY_ANTDWELL                B43_PHY_OFDM(0x2B)      /* Antenna dwell */
+#define  B43_PHY_ANTDWELL_AUTODIV1      0x0100  /* Automatic RX diversity start antenna */
+#define B43_PHY_ENCORE                  B43_PHY_OFDM(0x49)      /* "Encore" (RangeMax / BroadRange) */
+#define  B43_PHY_ENCORE_EN              0x0200  /* Encore enable */
+#define B43_PHY_LMS                     B43_PHY_OFDM(0x55)
+#define B43_PHY_OFDM61                  B43_PHY_OFDM(0x61)      /* FIXME rename */
+#define  B43_PHY_OFDM61_10              0x0010  /* FIXME rename */
+#define B43_PHY_IQBAL                   B43_PHY_OFDM(0x69)      /* I/Q balance */
+#define B43_PHY_BBTXDC_BIAS             B43_PHY_OFDM(0x6B)      /* Baseband TX DC bias */
+#define B43_PHY_OTABLECTL               B43_PHY_OFDM(0x72)      /* OFDM table control (see below) */
+#define  B43_PHY_OTABLEOFF              0x03FF  /* OFDM table offset (see below) */
+#define  B43_PHY_OTABLENR               0xFC00  /* OFDM table number (see below) */
+#define  B43_PHY_OTABLENR_SHIFT         10
+#define B43_PHY_OTABLEI                 B43_PHY_OFDM(0x73)      /* OFDM table data I */
+#define B43_PHY_OTABLEQ                 B43_PHY_OFDM(0x74)      /* OFDM table data Q */
+#define B43_PHY_HPWR_TSSICTL            B43_PHY_OFDM(0x78)      /* Hardware power TSSI control */
+#define B43_PHY_ADCCTL                  B43_PHY_OFDM(0x7A)      /* ADC control */
+#define B43_PHY_IDLE_TSSI               B43_PHY_OFDM(0x7B)
+#define B43_PHY_A_TEMP_SENSE            B43_PHY_OFDM(0x7C)      /* A PHY temperature sense */
+#define B43_PHY_NRSSITHRES              B43_PHY_OFDM(0x8A)      /* NRSSI threshold */
+#define B43_PHY_ANTWRSETT               B43_PHY_OFDM(0x8C)      /* Antenna WR settle */
+#define  B43_PHY_ANTWRSETT_ARXDIV       0x2000  /* Automatic RX diversity enabled */
+#define B43_PHY_CLIPPWRDOWNT            B43_PHY_OFDM(0x93)      /* Clip powerdown threshold */
+#define B43_PHY_OFDM9B                  B43_PHY_OFDM(0x9B)      /* FIXME rename */
+#define B43_PHY_N1P1GAIN                B43_PHY_OFDM(0xA0)
+#define B43_PHY_P1P2GAIN                B43_PHY_OFDM(0xA1)
+#define B43_PHY_N1N2GAIN                B43_PHY_OFDM(0xA2)
+#define B43_PHY_CLIPTHRES               B43_PHY_OFDM(0xA3)
+#define B43_PHY_CLIPN1P2THRES           B43_PHY_OFDM(0xA4)
+#define B43_PHY_CCKSHIFTBITS_WA         B43_PHY_OFDM(0xA5)      /* CCK shiftbits workaround, FIXME rename */
+#define B43_PHY_CCKSHIFTBITS            B43_PHY_OFDM(0xA7)      /* FIXME rename */
+#define B43_PHY_DIVSRCHIDX              B43_PHY_OFDM(0xA8)      /* Divider search gain/index */
+#define B43_PHY_CLIPP2THRES             B43_PHY_OFDM(0xA9)
+#define B43_PHY_CLIPP3THRES             B43_PHY_OFDM(0xAA)
+#define B43_PHY_DIVP1P2GAIN             B43_PHY_OFDM(0xAB)
+#define B43_PHY_DIVSRCHGAINBACK         B43_PHY_OFDM(0xAD)      /* Divider search gain back */
+#define B43_PHY_DIVSRCHGAINCHNG         B43_PHY_OFDM(0xAE)      /* Divider search gain change */
+#define B43_PHY_CRSTHRES1               B43_PHY_OFDM(0xC0)      /* CRS Threshold 1 (phy.rev >= 2 only) */
+#define B43_PHY_CRSTHRES2               B43_PHY_OFDM(0xC1)      /* CRS Threshold 2 (phy.rev >= 2 only) */
+#define B43_PHY_TSSIP_LTBASE            B43_PHY_OFDM(0x380)     /* TSSI power lookup table base */
+#define B43_PHY_DC_LTBASE               B43_PHY_OFDM(0x3A0)     /* DC lookup table base */
+#define B43_PHY_GAIN_LTBASE             B43_PHY_OFDM(0x3C0)     /* Gain lookup table base */
+
+/*** OFDM table numbers ***/
+#define B43_OFDMTAB(number, offset)     (((number) << B43_PHY_OTABLENR_SHIFT) | (offset))
+#define B43_OFDMTAB_AGC1                B43_OFDMTAB(0x00, 0)
+#define B43_OFDMTAB_GAIN0               B43_OFDMTAB(0x00, 0)
+#define B43_OFDMTAB_GAINX               B43_OFDMTAB(0x01, 0)    //TODO rename
+#define B43_OFDMTAB_GAIN1               B43_OFDMTAB(0x01, 4)
+#define B43_OFDMTAB_AGC3                B43_OFDMTAB(0x02, 0)
+#define B43_OFDMTAB_GAIN2               B43_OFDMTAB(0x02, 3)
+#define B43_OFDMTAB_LNAHPFGAIN1         B43_OFDMTAB(0x03, 0)
+#define B43_OFDMTAB_WRSSI               B43_OFDMTAB(0x04, 0)
+#define B43_OFDMTAB_LNAHPFGAIN2         B43_OFDMTAB(0x04, 0)
+#define B43_OFDMTAB_NOISESCALE          B43_OFDMTAB(0x05, 0)
+#define B43_OFDMTAB_AGC2                B43_OFDMTAB(0x06, 0)
+#define B43_OFDMTAB_ROTOR               B43_OFDMTAB(0x08, 0)
+#define B43_OFDMTAB_ADVRETARD           B43_OFDMTAB(0x09, 0)
+#define B43_OFDMTAB_DAC                 B43_OFDMTAB(0x0C, 0)
+#define B43_OFDMTAB_DC                  B43_OFDMTAB(0x0E, 7)
+#define B43_OFDMTAB_PWRDYN2             B43_OFDMTAB(0x0E, 12)
+#define B43_OFDMTAB_LNAGAIN             B43_OFDMTAB(0x0E, 13)
+#define B43_OFDMTAB_UNKNOWN_0F          B43_OFDMTAB(0x0F, 0)    //TODO rename
+#define B43_OFDMTAB_UNKNOWN_APHY        B43_OFDMTAB(0x0F, 7)    //TODO rename
+#define B43_OFDMTAB_LPFGAIN             B43_OFDMTAB(0x0F, 12)
+#define B43_OFDMTAB_RSSI                B43_OFDMTAB(0x10, 0)
+#define B43_OFDMTAB_UNKNOWN_11          B43_OFDMTAB(0x11, 4)    //TODO rename
+#define B43_OFDMTAB_AGC1_R1             B43_OFDMTAB(0x13, 0)
+#define B43_OFDMTAB_GAINX_R1            B43_OFDMTAB(0x14, 0)    //TODO remove!
+#define B43_OFDMTAB_MINSIGSQ            B43_OFDMTAB(0x14, 0)
+#define B43_OFDMTAB_AGC3_R1             B43_OFDMTAB(0x15, 0)
+#define B43_OFDMTAB_WRSSI_R1            B43_OFDMTAB(0x15, 4)
+#define B43_OFDMTAB_TSSI                B43_OFDMTAB(0x15, 0)
+#define B43_OFDMTAB_DACRFPABB           B43_OFDMTAB(0x16, 0)
+#define B43_OFDMTAB_DACOFF              B43_OFDMTAB(0x17, 0)
+#define B43_OFDMTAB_DCBIAS              B43_OFDMTAB(0x18, 0)
+
+u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset);
+void b43_ofdmtab_write16(struct b43_wldev *dev, u16 table,
+                         u16 offset, u16 value);
+u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset);
+void b43_ofdmtab_write32(struct b43_wldev *dev, u16 table,
+                         u16 offset, u32 value);
+
+
+struct b43_phy_a {
+        bool initialised;
+
+        /* A-PHY TX Power control value. */
+        u16 txpwr_offset;
+
+        //TODO lots of missing stuff
+};
+
+/**
+ * b43_phy_inita - Lowlevel A-PHY init routine.
+ * This is _only_ used by the G-PHY code.
+ */
+void b43_phy_inita(struct b43_wldev *dev);
+
+
+struct b43_phy_operations;
+extern const struct b43_phy_operations b43_phyops_a;
+
+#endif /* LINUX_B43_PHY_A_H_ */
diff --git a/drivers/net/wireless/b43/phy_common.c b/drivers/net/wireless/b43/phy_common.c
new file mode 100644
index 000000000000..5a550a7af2e9
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_common.c
@@ -0,0 +1,367 @@
+/*
+
+  Broadcom B43 wireless driver
+  Common PHY routines
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
+  Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "phy_common.h"
+#include "phy_g.h"
+#include "phy_a.h"
+#include "nphy.h"
+#include "b43.h"
+#include "main.h"
+
+
+int b43_phy_operations_setup(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &(dev->phy);
+        int err;
+
+        phy->ops = NULL;
+
+        switch (phy->type) {
+        case B43_PHYTYPE_A:
+                phy->ops = &b43_phyops_a;
+                break;
+        case B43_PHYTYPE_G:
+                phy->ops = &b43_phyops_g;
+                break;
+        case B43_PHYTYPE_N:
+#ifdef CONFIG_B43_NPHY
+                phy->ops = &b43_phyops_n;
+#endif
+                break;
+        case B43_PHYTYPE_LP:
+                /* FIXME: Not yet */
+                break;
+        }
+        if (B43_WARN_ON(!phy->ops))
+                return -ENODEV;
+
+        err = phy->ops->allocate(dev);
+        if (err)
+                phy->ops = NULL;
+
+        return err;
+}
+
+int b43_phy_init(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        const struct b43_phy_operations *ops = phy->ops;
+        int err;
+
+        phy->channel = ops->get_default_chan(dev);
+
+        ops->software_rfkill(dev, RFKILL_STATE_UNBLOCKED);
+        err = ops->init(dev);
+        if (err) {
+                b43err(dev->wl, "PHY init failed\n");
+                goto err_block_rf;
+        }
+        /* Make sure to switch hardware and firmware (SHM) to
+         * the default channel. */
+        err = b43_switch_channel(dev, ops->get_default_chan(dev));
+        if (err) {
+                b43err(dev->wl, "PHY init: Channel switch to default failed\n");
+                goto err_phy_exit;
+        }
+
+        return 0;
+
+err_phy_exit:
+        if (ops->exit)
+                ops->exit(dev);
+err_block_rf:
+        ops->software_rfkill(dev, RFKILL_STATE_SOFT_BLOCKED);
+
+        return err;
+}
+
+void b43_phy_exit(struct b43_wldev *dev)
+{
+        const struct b43_phy_operations *ops = dev->phy.ops;
+
+        ops->software_rfkill(dev, RFKILL_STATE_SOFT_BLOCKED);
+        if (ops->exit)
+                ops->exit(dev);
+}
+
+bool b43_has_hardware_pctl(struct b43_wldev *dev)
+{
+        if (!dev->phy.hardware_power_control)
+                return 0;
+        if (!dev->phy.ops->supports_hwpctl)
+                return 0;
+        return dev->phy.ops->supports_hwpctl(dev);
+}
+
+void b43_radio_lock(struct b43_wldev *dev)
+{
+        u32 macctl;
+
+        macctl = b43_read32(dev, B43_MMIO_MACCTL);
+        B43_WARN_ON(macctl & B43_MACCTL_RADIOLOCK);
+        macctl |= B43_MACCTL_RADIOLOCK;
+        b43_write32(dev, B43_MMIO_MACCTL, macctl);
+        /* Commit the write and wait for the device
+         * to exit any radio register access. */
+        b43_read32(dev, B43_MMIO_MACCTL);
+        udelay(10);
+}
+
+void b43_radio_unlock(struct b43_wldev *dev)
+{
+        u32 macctl;
+
+        /* Commit any write */
+        b43_read16(dev, B43_MMIO_PHY_VER);
+        /* unlock */
+        macctl = b43_read32(dev, B43_MMIO_MACCTL);
+        B43_WARN_ON(!(macctl & B43_MACCTL_RADIOLOCK));
+        macctl &= ~B43_MACCTL_RADIOLOCK;
+        b43_write32(dev, B43_MMIO_MACCTL, macctl);
+}
+
+void b43_phy_lock(struct b43_wldev *dev)
+{
+#if B43_DEBUG
+        B43_WARN_ON(dev->phy.phy_locked);
+        dev->phy.phy_locked = 1;
+#endif
+        B43_WARN_ON(dev->dev->id.revision < 3);
+
+        if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+                b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
+}
+
+void b43_phy_unlock(struct b43_wldev *dev)
+{
+#if B43_DEBUG
+        B43_WARN_ON(!dev->phy.phy_locked);
+        dev->phy.phy_locked = 0;
+#endif
+        B43_WARN_ON(dev->dev->id.revision < 3);
+
+        if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+                b43_power_saving_ctl_bits(dev, 0);
+}
+
+u16 b43_radio_read(struct b43_wldev *dev, u16 reg)
+{
+        return dev->phy.ops->radio_read(dev, reg);
+}
+
+void b43_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        dev->phy.ops->radio_write(dev, reg, value);
+}
+
+void b43_radio_mask(struct b43_wldev *dev, u16 offset, u16 mask)
+{
+        b43_radio_write16(dev, offset,
+                          b43_radio_read16(dev, offset) & mask);
+}
+
+void b43_radio_set(struct b43_wldev *dev, u16 offset, u16 set)
+{
+        b43_radio_write16(dev, offset,
+                          b43_radio_read16(dev, offset) | set);
+}
+
+void b43_radio_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
+{
+        b43_radio_write16(dev, offset,
+                          (b43_radio_read16(dev, offset) & mask) | set);
+}
+
+u16 b43_phy_read(struct b43_wldev *dev, u16 reg)
+{
+        return dev->phy.ops->phy_read(dev, reg);
+}
+
+void b43_phy_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        dev->phy.ops->phy_write(dev, reg, value);
+}
+
+void b43_phy_mask(struct b43_wldev *dev, u16 offset, u16 mask)
+{
+        b43_phy_write(dev, offset,
+                      b43_phy_read(dev, offset) & mask);
+}
+
+void b43_phy_set(struct b43_wldev *dev, u16 offset, u16 set)
+{
+        b43_phy_write(dev, offset,
+                      b43_phy_read(dev, offset) | set);
+}
+
+void b43_phy_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set)
+{
+        b43_phy_write(dev, offset,
+                      (b43_phy_read(dev, offset) & mask) | set);
+}
+
+int b43_switch_channel(struct b43_wldev *dev, unsigned int new_channel)
+{
+        struct b43_phy *phy = &(dev->phy);
+        u16 channelcookie, savedcookie;
+        int err;
+
+        if (new_channel == B43_DEFAULT_CHANNEL)
+                new_channel = phy->ops->get_default_chan(dev);
+
+        /* First we set the channel radio code to prevent the
+         * firmware from sending ghost packets.
+         */
+        channelcookie = new_channel;
+        if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ)
+                channelcookie |= 0x100;
+        //FIXME set 40Mhz flag if required
+        savedcookie = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN);
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN, channelcookie);
+
+        /* Now try to switch the PHY hardware channel. */
+        err = phy->ops->switch_channel(dev, new_channel);
+        if (err)
+                goto err_restore_cookie;
+
+        dev->phy.channel = new_channel;
+        /* Wait for the radio to tune to the channel and stabilize. */
+        msleep(8);
+
+        return 0;
+
+err_restore_cookie:
+        b43_shm_write16(dev, B43_SHM_SHARED,
+                        B43_SHM_SH_CHAN, savedcookie);
+
+        return err;
+}
+
+void b43_software_rfkill(struct b43_wldev *dev, enum rfkill_state state)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (state == RFKILL_STATE_HARD_BLOCKED) {
+                /* We cannot hardware-block the device */
+                state = RFKILL_STATE_SOFT_BLOCKED;
+        }
+
+        phy->ops->software_rfkill(dev, state);
+        phy->radio_on = (state == RFKILL_STATE_UNBLOCKED);
+}
+
+/**
+ * b43_phy_txpower_adjust_work - TX power workqueue.
+ *
+ * Workqueue for updating the TX power parameters in hardware.
+ */
+void b43_phy_txpower_adjust_work(struct work_struct *work)
+{
+        struct b43_wl *wl = container_of(work, struct b43_wl,
+                                         txpower_adjust_work);
+        struct b43_wldev *dev;
+
+        mutex_lock(&wl->mutex);
+        dev = wl->current_dev;
+
+        if (likely(dev && (b43_status(dev) >= B43_STAT_STARTED)))
+                dev->phy.ops->adjust_txpower(dev);
+
+        mutex_unlock(&wl->mutex);
+}
+
+/* Called with wl->irq_lock locked */
+void b43_phy_txpower_check(struct b43_wldev *dev, unsigned int flags)
+{
+        struct b43_phy *phy = &dev->phy;
+        unsigned long now = jiffies;
+        enum b43_txpwr_result result;
+
+        if (!(flags & B43_TXPWR_IGNORE_TIME)) {
+                /* Check if it's time for a TXpower check. */
+                if (time_before(now, phy->next_txpwr_check_time))
+                        return; /* Not yet */
+        }
+        /* The next check will be needed in two seconds, or later. */
+        phy->next_txpwr_check_time = round_jiffies(now + (HZ * 2));
+
+        if ((dev->dev->bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+            (dev->dev->bus->boardinfo.type == SSB_BOARD_BU4306))
+                return; /* No software txpower adjustment needed */
+
+        result = phy->ops->recalc_txpower(dev, !!(flags & B43_TXPWR_IGNORE_TSSI));
+        if (result == B43_TXPWR_RES_DONE)
+                return; /* We are done. */
+        B43_WARN_ON(result != B43_TXPWR_RES_NEED_ADJUST);
+        B43_WARN_ON(phy->ops->adjust_txpower == NULL);
+
+        /* We must adjust the transmission power in hardware.
+         * Schedule b43_phy_txpower_adjust_work(). */
+        queue_work(dev->wl->hw->workqueue, &dev->wl->txpower_adjust_work);
+}
+
+int b43_phy_shm_tssi_read(struct b43_wldev *dev, u16 shm_offset)
+{
+        const bool is_ofdm = (shm_offset != B43_SHM_SH_TSSI_CCK);
+        unsigned int a, b, c, d;
+        unsigned int average;
+        u32 tmp;
+
+        tmp = b43_shm_read32(dev, B43_SHM_SHARED, shm_offset);
+        a = tmp & 0xFF;
+        b = (tmp >> 8) & 0xFF;
+        c = (tmp >> 16) & 0xFF;
+        d = (tmp >> 24) & 0xFF;
+        if (a == 0 || a == B43_TSSI_MAX ||
+            b == 0 || b == B43_TSSI_MAX ||
+            c == 0 || c == B43_TSSI_MAX ||
+            d == 0 || d == B43_TSSI_MAX)
+                return -ENOENT;
+        /* The values are OK. Clear them. */
+        tmp = B43_TSSI_MAX | (B43_TSSI_MAX << 8) |
+              (B43_TSSI_MAX << 16) | (B43_TSSI_MAX << 24);
+        b43_shm_write32(dev, B43_SHM_SHARED, shm_offset, tmp);
+
+        if (is_ofdm) {
+                a = (a + 32) & 0x3F;
+                b = (b + 32) & 0x3F;
+                c = (c + 32) & 0x3F;
+                d = (d + 32) & 0x3F;
+        }
+
+        /* Get the average of the values with 0.5 added to each value. */
+        average = (a + b + c + d + 2) / 4;
+        if (is_ofdm) {
+                /* Adjust for CCK-boost */
+                if (b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFLO)
+                    & B43_HF_CCKBOOST)
+                        average = (average >= 13) ? (average - 13) : 0;
+        }
+
+        return average;
+}
diff --git a/drivers/net/wireless/b43/phy_common.h b/drivers/net/wireless/b43/phy_common.h
new file mode 100644
index 000000000000..f8db9f40df5d
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_common.h
@@ -0,0 +1,381 @@
+#ifndef LINUX_B43_PHY_COMMON_H_
+#define LINUX_B43_PHY_COMMON_H_
+
+#include <linux/rfkill.h>
+
+struct b43_wldev;
+
+
+/* PHY register routing bits */
+#define B43_PHYROUTE                    0x0C00 /* PHY register routing bits mask */
+#define  B43_PHYROUTE_BASE              0x0000 /* Base registers */
+#define  B43_PHYROUTE_OFDM_GPHY         0x0400 /* OFDM register routing for G-PHYs */
+#define  B43_PHYROUTE_EXT_GPHY          0x0800 /* Extended G-PHY registers */
+#define  B43_PHYROUTE_N_BMODE           0x0C00 /* N-PHY BMODE registers */
+
+/* CCK (B-PHY) registers. */
+#define B43_PHY_CCK(reg)                ((reg) | B43_PHYROUTE_BASE)
+/* N-PHY registers. */
+#define B43_PHY_N(reg)                  ((reg) | B43_PHYROUTE_BASE)
+/* N-PHY BMODE registers. */
+#define B43_PHY_N_BMODE(reg)            ((reg) | B43_PHYROUTE_N_BMODE)
+/* OFDM (A-PHY) registers. */
+#define B43_PHY_OFDM(reg)               ((reg) | B43_PHYROUTE_OFDM_GPHY)
+/* Extended G-PHY registers. */
+#define B43_PHY_EXTG(reg)               ((reg) | B43_PHYROUTE_EXT_GPHY)
+
+
+/* Masks for the PHY versioning registers. */
+#define B43_PHYVER_ANALOG               0xF000
+#define B43_PHYVER_ANALOG_SHIFT         12
+#define B43_PHYVER_TYPE                 0x0F00
+#define B43_PHYVER_TYPE_SHIFT           8
+#define B43_PHYVER_VERSION              0x00FF
+
+/**
+ * enum b43_interference_mitigation - Interference Mitigation mode
+ *
+ * @B43_INTERFMODE_NONE:        Disabled
+ * @B43_INTERFMODE_NONWLAN:     Non-WLAN Interference Mitigation
+ * @B43_INTERFMODE_MANUALWLAN:  WLAN Interference Mitigation
+ * @B43_INTERFMODE_AUTOWLAN:    Automatic WLAN Interference Mitigation
+ */
+enum b43_interference_mitigation {
+        B43_INTERFMODE_NONE,
+        B43_INTERFMODE_NONWLAN,
+        B43_INTERFMODE_MANUALWLAN,
+        B43_INTERFMODE_AUTOWLAN,
+};
+
+/* Antenna identifiers */
+enum {
+        B43_ANTENNA0,           /* Antenna 0 */
+        B43_ANTENNA1,           /* Antenna 0 */
+        B43_ANTENNA_AUTO1,      /* Automatic, starting with antenna 1 */
+        B43_ANTENNA_AUTO0,      /* Automatic, starting with antenna 0 */
+        B43_ANTENNA2,
+        B43_ANTENNA3 = 8,
+
+        B43_ANTENNA_AUTO = B43_ANTENNA_AUTO0,
+        B43_ANTENNA_DEFAULT = B43_ANTENNA_AUTO,
+};
+
+/**
+ * enum b43_txpwr_result - Return value for the recalc_txpower PHY op.
+ *
+ * @B43_TXPWR_RES_NEED_ADJUST:  Values changed. Hardware adjustment is needed.
+ * @B43_TXPWR_RES_DONE:         No more work to do. Everything is done.
+ */
+enum b43_txpwr_result {
+        B43_TXPWR_RES_NEED_ADJUST,
+        B43_TXPWR_RES_DONE,
+};
+
+/**
+ * struct b43_phy_operations - Function pointers for PHY ops.
+ *
+ * @prepare:            Prepare the PHY. This is called before @init.
+ *                      Can be NULL, if not required.
+ * @init:               Initialize the PHY.
+ *                      Must not be NULL.
+ * @exit:               Shutdown the PHY and free all data structures.
+ *                      Can be NULL, if not required.
+ *
+ * @phy_read:           Read from a PHY register.
+ *                      Must not be NULL.
+ * @phy_write:          Write to a PHY register.
+ *                      Must not be NULL.
+ * @radio_read:         Read from a Radio register.
+ *                      Must not be NULL.
+ * @radio_write:        Write to a Radio register.
+ *                      Must not be NULL.
+ *
+ * @supports_hwpctl:    Returns a boolean whether Hardware Power Control
+ *                      is supported or not.
+ *                      If NULL, hwpctl is assumed to be never supported.
+ * @software_rfkill:    Turn the radio ON or OFF.
+ *                      Possible state values are
+ *                      RFKILL_STATE_SOFT_BLOCKED or
+ *                      RFKILL_STATE_UNBLOCKED
+ *                      Must not be NULL.
+ * @switch_channel:     Switch the radio to another channel.
+ *                      Must not be NULL.
+ * @get_default_chan:   Just returns the default channel number.
+ *                      Must not be NULL.
+ * @set_rx_antenna:     Set the antenna used for RX.
+ *                      Can be NULL, if not supported.
+ * @interf_mitigation:  Switch the Interference Mitigation mode.
+ *                      Can be NULL, if not supported.
+ *
+ * @recalc_txpower:     Recalculate the transmission power parameters.
+ *                      This callback has to recalculate the TX power settings,
+ *                      but does not need to write them to the hardware, yet.
+ *                      Returns enum b43_txpwr_result to indicate whether the hardware
+ *                      needs to be adjusted.
+ *                      If B43_TXPWR_NEED_ADJUST is returned, @adjust_txpower
+ *                      will be called later.
+ *                      If the parameter "ignore_tssi" is true, the TSSI values should
+ *                      be ignored and a recalculation of the power settings should be
+ *                      done even if the TSSI values did not change.
+ *                      This callback is called with wl->irq_lock held and must not sleep.
+ *                      Must not be NULL.
+ * @adjust_txpower:     Write the previously calculated TX power settings
+ *                      (from @recalc_txpower) to the hardware.
+ *                      This function may sleep.
+ *                      Can be NULL, if (and ONLY if) @recalc_txpower _always_
+ *                      returns B43_TXPWR_RES_DONE.
+ *
+ * @pwork_15sec:        Periodic work. Called every 15 seconds.
+ *                      Can be NULL, if not required.
+ * @pwork_60sec:        Periodic work. Called every 60 seconds.
+ *                      Can be NULL, if not required.
+ */
+struct b43_phy_operations {
+        /* Initialisation */
+        int (*allocate)(struct b43_wldev *dev);
+        int (*prepare)(struct b43_wldev *dev);
+        int (*init)(struct b43_wldev *dev);
+        void (*exit)(struct b43_wldev *dev);
+
+        /* Register access */
+        u16 (*phy_read)(struct b43_wldev *dev, u16 reg);
+        void (*phy_write)(struct b43_wldev *dev, u16 reg, u16 value);
+        u16 (*radio_read)(struct b43_wldev *dev, u16 reg);
+        void (*radio_write)(struct b43_wldev *dev, u16 reg, u16 value);
+
+        /* Radio */
+        bool (*supports_hwpctl)(struct b43_wldev *dev);
+        void (*software_rfkill)(struct b43_wldev *dev, enum rfkill_state state);
+        int (*switch_channel)(struct b43_wldev *dev, unsigned int new_channel);
+        unsigned int (*get_default_chan)(struct b43_wldev *dev);
+        void (*set_rx_antenna)(struct b43_wldev *dev, int antenna);
+        int (*interf_mitigation)(struct b43_wldev *dev,
+                                 enum b43_interference_mitigation new_mode);
+
+        /* Transmission power adjustment */
+        enum b43_txpwr_result (*recalc_txpower)(struct b43_wldev *dev,
+                                                bool ignore_tssi);
+        void (*adjust_txpower)(struct b43_wldev *dev);
+
+        /* Misc */
+        void (*pwork_15sec)(struct b43_wldev *dev);
+        void (*pwork_60sec)(struct b43_wldev *dev);
+};
+
+struct b43_phy_a;
+struct b43_phy_g;
+struct b43_phy_n;
+
+struct b43_phy {
+        /* Hardware operation callbacks. */
+        const struct b43_phy_operations *ops;
+
+        /* Most hardware context information is stored in the standard-
+         * specific data structures pointed to by the pointers below.
+         * Only one of them is valid (the currently enabled PHY). */
+#ifdef CONFIG_B43_DEBUG
+        /* No union for debug build to force NULL derefs in buggy code. */
+        struct {
+#else
+        union {
+#endif
+                /* A-PHY specific information */
+                struct b43_phy_a *a;
+                /* G-PHY specific information */
+                struct b43_phy_g *g;
+                /* N-PHY specific information */
+                struct b43_phy_n *n;
+        };
+
+        /* Band support flags. */
+        bool supports_2ghz;
+        bool supports_5ghz;
+
+        /* GMODE bit enabled? */
+        bool gmode;
+
+        /* Analog Type */
+        u8 analog;
+        /* B43_PHYTYPE_ */
+        u8 type;
+        /* PHY revision number. */
+        u8 rev;
+
+        /* Radio versioning */
+        u16 radio_manuf;        /* Radio manufacturer */
+        u16 radio_ver;          /* Radio version */
+        u8 radio_rev;           /* Radio revision */
+
+        /* Software state of the radio */
+        bool radio_on;
+
+        /* Desired TX power level (in dBm).
+         * This is set by the user and adjusted in b43_phy_xmitpower(). */
+        int desired_txpower;
+
+        /* Hardware Power Control enabled? */
+        bool hardware_power_control;
+
+        /* The time (in absolute jiffies) when the next TX power output
+         * check is needed. */
+        unsigned long next_txpwr_check_time;
+
+        /* current channel */
+        unsigned int channel;
+
+        /* PHY TX errors counter. */
+        atomic_t txerr_cnt;
+
+#ifdef CONFIG_B43_DEBUG
+        /* PHY registers locked by b43_phy_lock()? */
+        bool phy_locked;
+#endif /* B43_DEBUG */
+};
+
+
+/**
+ * b43_phy_operations_setup - Initialize the PHY operations datastructure
+ * based on the current PHY type.
+ */
+int b43_phy_operations_setup(struct b43_wldev *dev);
+
+/**
+ * b43_phy_init - Initialise the PHY
+ */
+int b43_phy_init(struct b43_wldev *dev);
+
+/**
+ * b43_phy_exit - Cleanup PHY
+ */
+void b43_phy_exit(struct b43_wldev *dev);
+
+/**
+ * b43_has_hardware_pctl - Hardware Power Control supported?
+ * Returns a boolean, whether hardware power control is supported.
+ */
+bool b43_has_hardware_pctl(struct b43_wldev *dev);
+
+/**
+ * b43_phy_read - 16bit PHY register read access
+ */
+u16 b43_phy_read(struct b43_wldev *dev, u16 reg);
+
+/**
+ * b43_phy_write - 16bit PHY register write access
+ */
+void b43_phy_write(struct b43_wldev *dev, u16 reg, u16 value);
+
+/**
+ * b43_phy_mask - Mask a PHY register with a mask
+ */
+void b43_phy_mask(struct b43_wldev *dev, u16 offset, u16 mask);
+
+/**
+ * b43_phy_set - OR a PHY register with a bitmap
+ */
+void b43_phy_set(struct b43_wldev *dev, u16 offset, u16 set);
+
+/**
+ * b43_phy_maskset - Mask and OR a PHY register with a mask and bitmap
+ */
+void b43_phy_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set);
+
+/**
+ * b43_radio_read - 16bit Radio register read access
+ */
+u16 b43_radio_read(struct b43_wldev *dev, u16 reg);
+#define b43_radio_read16        b43_radio_read /* DEPRECATED */
+
+/**
+ * b43_radio_write - 16bit Radio register write access
+ */
+void b43_radio_write(struct b43_wldev *dev, u16 reg, u16 value);
+#define b43_radio_write16       b43_radio_write /* DEPRECATED */
+
+/**
+ * b43_radio_mask - Mask a 16bit radio register with a mask
+ */
+void b43_radio_mask(struct b43_wldev *dev, u16 offset, u16 mask);
+
+/**
+ * b43_radio_set - OR a 16bit radio register with a bitmap
+ */
+void b43_radio_set(struct b43_wldev *dev, u16 offset, u16 set);
+
+/**
+ * b43_radio_maskset - Mask and OR a radio register with a mask and bitmap
+ */
+void b43_radio_maskset(struct b43_wldev *dev, u16 offset, u16 mask, u16 set);
+
+/**
+ * b43_radio_lock - Lock firmware radio register access
+ */
+void b43_radio_lock(struct b43_wldev *dev);
+
+/**
+ * b43_radio_unlock - Unlock firmware radio register access
+ */
+void b43_radio_unlock(struct b43_wldev *dev);
+
+/**
+ * b43_phy_lock - Lock firmware PHY register access
+ */
+void b43_phy_lock(struct b43_wldev *dev);
+
+/**
+ * b43_phy_unlock - Unlock firmware PHY register access
+ */
+void b43_phy_unlock(struct b43_wldev *dev);
+
+/**
+ * b43_switch_channel - Switch to another channel
+ */
+int b43_switch_channel(struct b43_wldev *dev, unsigned int new_channel);
+/**
+ * B43_DEFAULT_CHANNEL - Switch to the default channel.
+ */
+#define B43_DEFAULT_CHANNEL     UINT_MAX
+
+/**
+ * b43_software_rfkill - Turn the radio ON or OFF in software.
+ */
+void b43_software_rfkill(struct b43_wldev *dev, enum rfkill_state state);
+
+/**
+ * b43_phy_txpower_check - Check TX power output.
+ *
+ * Compare the current TX power output to the desired power emission
+ * and schedule an adjustment in case it mismatches.
+ * Requires wl->irq_lock locked.
+ *
+ * @flags:      OR'ed enum b43_phy_txpower_check_flags flags.
+ *              See the docs below.
+ */
+void b43_phy_txpower_check(struct b43_wldev *dev, unsigned int flags);
+/**
+ * enum b43_phy_txpower_check_flags - Flags for b43_phy_txpower_check()
+ *
+ * @B43_TXPWR_IGNORE_TIME: Ignore the schedule time and force-redo
+ *                         the check now.
+ * @B43_TXPWR_IGNORE_TSSI: Redo the recalculation, even if the average
+ *                         TSSI did not change.
+ */
+enum b43_phy_txpower_check_flags {
+        B43_TXPWR_IGNORE_TIME           = (1 << 0),
+        B43_TXPWR_IGNORE_TSSI           = (1 << 1),
+};
+
+struct work_struct;
+void b43_phy_txpower_adjust_work(struct work_struct *work);
+
+/**
+ * b43_phy_shm_tssi_read - Read the average of the last 4 TSSI from SHM.
+ *
+ * @shm_offset:         The SHM address to read the values from.
+ *
+ * Returns the average of the 4 TSSI values, or a negative error code.
+ */
+int b43_phy_shm_tssi_read(struct b43_wldev *dev, u16 shm_offset);
+
+
+#endif /* LINUX_B43_PHY_COMMON_H_ */
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/b43/phy_g.c
new file mode 100644
index 000000000000..fce84896d34c
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_g.c
@@ -0,0 +1,3251 @@
+/*
+
+  Broadcom B43 wireless driver
+  IEEE 802.11g PHY driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
+  Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  This program is free software; you can redistribute it and/or modify
+  it under the terms of the GNU General Public License as published by
+  the Free Software Foundation; either version 2 of the License, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "phy_g.h"
+#include "phy_common.h"
+#include "lo.h"
+#include "main.h"
+
+#include <linux/bitrev.h>
+
+
+static const s8 b43_tssi2dbm_g_table[] = {
+        77, 77, 77, 76,
+        76, 76, 75, 75,
+        74, 74, 73, 73,
+        73, 72, 72, 71,
+        71, 70, 70, 69,
+        68, 68, 67, 67,
+        66, 65, 65, 64,
+        63, 63, 62, 61,
+        60, 59, 58, 57,
+        56, 55, 54, 53,
+        52, 50, 49, 47,
+        45, 43, 40, 37,
+        33, 28, 22, 14,
+        5, -7, -20, -20,
+        -20, -20, -20, -20,
+        -20, -20, -20, -20,
+};
+
+const u8 b43_radio_channel_codes_bg[] = {
+        12, 17, 22, 27,
+        32, 37, 42, 47,
+        52, 57, 62, 67,
+        72, 84,
+};
+
+
+static void b43_calc_nrssi_threshold(struct b43_wldev *dev);
+
+
+#define bitrev4(tmp) (bitrev8(tmp) >> 4)
+
+
+/* Get the freq, as it has to be written to the device. */
+static inline u16 channel2freq_bg(u8 channel)
+{
+        B43_WARN_ON(!(channel >= 1 && channel <= 14));
+
+        return b43_radio_channel_codes_bg[channel - 1];
+}
+
+static void generate_rfatt_list(struct b43_wldev *dev,
+                                struct b43_rfatt_list *list)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        /* APHY.rev < 5 || GPHY.rev < 6 */
+        static const struct b43_rfatt rfatt_0[] = {
+                {.att = 3,.with_padmix = 0,},
+                {.att = 1,.with_padmix = 0,},
+                {.att = 5,.with_padmix = 0,},
+                {.att = 7,.with_padmix = 0,},
+                {.att = 9,.with_padmix = 0,},
+                {.att = 2,.with_padmix = 0,},
+                {.att = 0,.with_padmix = 0,},
+                {.att = 4,.with_padmix = 0,},
+                {.att = 6,.with_padmix = 0,},
+                {.att = 8,.with_padmix = 0,},
+                {.att = 1,.with_padmix = 1,},
+                {.att = 2,.with_padmix = 1,},
+                {.att = 3,.with_padmix = 1,},
+                {.att = 4,.with_padmix = 1,},
+        };
+        /* Radio.rev == 8 && Radio.version == 0x2050 */
+        static const struct b43_rfatt rfatt_1[] = {
+                {.att = 2,.with_padmix = 1,},
+                {.att = 4,.with_padmix = 1,},
+                {.att = 6,.with_padmix = 1,},
+                {.att = 8,.with_padmix = 1,},
+                {.att = 10,.with_padmix = 1,},
+                {.att = 12,.with_padmix = 1,},
+                {.att = 14,.with_padmix = 1,},
+        };
+        /* Otherwise */
+        static const struct b43_rfatt rfatt_2[] = {
+                {.att = 0,.with_padmix = 1,},
+                {.att = 2,.with_padmix = 1,},
+                {.att = 4,.with_padmix = 1,},
+                {.att = 6,.with_padmix = 1,},
+                {.att = 8,.with_padmix = 1,},
+                {.att = 9,.with_padmix = 1,},
+                {.att = 9,.with_padmix = 1,},
+        };
+
+        if (!b43_has_hardware_pctl(dev)) {
+                /* Software pctl */
+                list->list = rfatt_0;
+                list->len = ARRAY_SIZE(rfatt_0);
+                list->min_val = 0;
+                list->max_val = 9;
+                return;
+        }
+        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+                /* Hardware pctl */
+                list->list = rfatt_1;
+                list->len = ARRAY_SIZE(rfatt_1);
+                list->min_val = 0;
+                list->max_val = 14;
+                return;
+        }
+        /* Hardware pctl */
+        list->list = rfatt_2;
+        list->len = ARRAY_SIZE(rfatt_2);
+        list->min_val = 0;
+        list->max_val = 9;
+}
+
+static void generate_bbatt_list(struct b43_wldev *dev,
+                                struct b43_bbatt_list *list)
+{
+        static const struct b43_bbatt bbatt_0[] = {
+                {.att = 0,},
+                {.att = 1,},
+                {.att = 2,},
+                {.att = 3,},
+                {.att = 4,},
+                {.att = 5,},
+                {.att = 6,},
+                {.att = 7,},
+                {.att = 8,},
+        };
+
+        list->list = bbatt_0;
+        list->len = ARRAY_SIZE(bbatt_0);
+        list->min_val = 0;
+        list->max_val = 8;
+}
+
+static void b43_shm_clear_tssi(struct b43_wldev *dev)
+{
+        b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F);
+        b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F);
+        b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F);
+        b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F);
+}
+
+/* Synthetic PU workaround */
+static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        might_sleep();
+
+        if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
+                /* We do not need the workaround. */
+                return;
+        }
+
+        if (channel <= 10) {
+                b43_write16(dev, B43_MMIO_CHANNEL,
+                            channel2freq_bg(channel + 4));
+        } else {
+                b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
+        }
+        msleep(1);
+        b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
+}
+
+/* Set the baseband attenuation value on chip. */
+void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev,
+                                       u16 baseband_attenuation)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (phy->analog == 0) {
+                b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
+                                                 & 0xFFF0) |
+                            baseband_attenuation);
+        } else if (phy->analog > 1) {
+                b43_phy_write(dev, B43_PHY_DACCTL,
+                              (b43_phy_read(dev, B43_PHY_DACCTL)
+                               & 0xFFC3) | (baseband_attenuation << 2));
+        } else {
+                b43_phy_write(dev, B43_PHY_DACCTL,
+                              (b43_phy_read(dev, B43_PHY_DACCTL)
+                               & 0xFF87) | (baseband_attenuation << 3));
+        }
+}
+
+/* Adjust the transmission power output (G-PHY) */
+void b43_set_txpower_g(struct b43_wldev *dev,
+                       const struct b43_bbatt *bbatt,
+                       const struct b43_rfatt *rfatt, u8 tx_control)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
+        u16 bb, rf;
+        u16 tx_bias, tx_magn;
+
+        bb = bbatt->att;
+        rf = rfatt->att;
+        tx_bias = lo->tx_bias;
+        tx_magn = lo->tx_magn;
+        if (unlikely(tx_bias == 0xFF))
+                tx_bias = 0;
+
+        /* Save the values for later */
+        gphy->tx_control = tx_control;
+        memcpy(&gphy->rfatt, rfatt, sizeof(*rfatt));
+        gphy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
+        memcpy(&gphy->bbatt, bbatt, sizeof(*bbatt));
+
+        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+                b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
+                       "rfatt(%u), tx_control(0x%02X), "
+                       "tx_bias(0x%02X), tx_magn(0x%02X)\n",
+                       bb, rf, tx_control, tx_bias, tx_magn);
+        }
+
+        b43_gphy_set_baseband_attenuation(dev, bb);
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
+        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x43,
+                                  (rf & 0x000F) | (tx_control & 0x0070));
+        } else {
+                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+                                              & 0xFFF0) | (rf & 0x000F));
+                b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
+                                              & ~0x0070) | (tx_control &
+                                                            0x0070));
+        }
+        if (has_tx_magnification(phy)) {
+                b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
+        } else {
+                b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
+                                              & 0xFFF0) | (tx_bias & 0x000F));
+        }
+        b43_lo_g_adjust(dev);
+}
+
+/* GPHY_TSSI_Power_Lookup_Table_Init */
+static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
+{
+        struct b43_phy_g *gphy = dev->phy.g;
+        int i;
+        u16 value;
+
+        for (i = 0; i < 32; i++)
+                b43_ofdmtab_write16(dev, 0x3C20, i, gphy->tssi2dbm[i]);
+        for (i = 32; i < 64; i++)
+                b43_ofdmtab_write16(dev, 0x3C00, i - 32, gphy->tssi2dbm[i]);
+        for (i = 0; i < 64; i += 2) {
+                value = (u16) gphy->tssi2dbm[i];
+                value |= ((u16) gphy->tssi2dbm[i + 1]) << 8;
+                b43_phy_write(dev, 0x380 + (i / 2), value);
+        }
+}
+
+/* GPHY_Gain_Lookup_Table_Init */
+static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
+        u16 nr_written = 0;
+        u16 tmp;
+        u8 rf, bb;
+
+        for (rf = 0; rf < lo->rfatt_list.len; rf++) {
+                for (bb = 0; bb < lo->bbatt_list.len; bb++) {
+                        if (nr_written >= 0x40)
+                                return;
+                        tmp = lo->bbatt_list.list[bb].att;
+                        tmp <<= 8;
+                        if (phy->radio_rev == 8)
+                                tmp |= 0x50;
+                        else
+                                tmp |= 0x40;
+                        tmp |= lo->rfatt_list.list[rf].att;
+                        b43_phy_write(dev, 0x3C0 + nr_written, tmp);
+                        nr_written++;
+                }
+        }
+}
+
+static void b43_set_all_gains(struct b43_wldev *dev,
+                              s16 first, s16 second, s16 third)
+{
+        struct b43_phy *phy = &dev->phy;
+        u16 i;
+        u16 start = 0x08, end = 0x18;
+        u16 tmp;
+        u16 table;
+
+        if (phy->rev <= 1) {
+                start = 0x10;
+                end = 0x20;
+        }
+
+        table = B43_OFDMTAB_GAINX;
+        if (phy->rev <= 1)
+                table = B43_OFDMTAB_GAINX_R1;
+        for (i = 0; i < 4; i++)
+                b43_ofdmtab_write16(dev, table, i, first);
+
+        for (i = start; i < end; i++)
+                b43_ofdmtab_write16(dev, table, i, second);
+
+        if (third != -1) {
+                tmp = ((u16) third << 14) | ((u16) third << 6);
+                b43_phy_write(dev, 0x04A0,
+                              (b43_phy_read(dev, 0x04A0) & 0xBFBF) | tmp);
+                b43_phy_write(dev, 0x04A1,
+                              (b43_phy_read(dev, 0x04A1) & 0xBFBF) | tmp);
+                b43_phy_write(dev, 0x04A2,
+                              (b43_phy_read(dev, 0x04A2) & 0xBFBF) | tmp);
+        }
+        b43_dummy_transmission(dev);
+}
+
+static void b43_set_original_gains(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        u16 i, tmp;
+        u16 table;
+        u16 start = 0x0008, end = 0x0018;
+
+        if (phy->rev <= 1) {
+                start = 0x0010;
+                end = 0x0020;
+        }
+
+        table = B43_OFDMTAB_GAINX;
+        if (phy->rev <= 1)
+                table = B43_OFDMTAB_GAINX_R1;
+        for (i = 0; i < 4; i++) {
+                tmp = (i & 0xFFFC);
+                tmp |= (i & 0x0001) << 1;
+                tmp |= (i & 0x0002) >> 1;
+
+                b43_ofdmtab_write16(dev, table, i, tmp);
+        }
+
+        for (i = start; i < end; i++)
+                b43_ofdmtab_write16(dev, table, i, i - start);
+
+        b43_phy_write(dev, 0x04A0,
+                      (b43_phy_read(dev, 0x04A0) & 0xBFBF) | 0x4040);
+        b43_phy_write(dev, 0x04A1,
+                      (b43_phy_read(dev, 0x04A1) & 0xBFBF) | 0x4040);
+        b43_phy_write(dev, 0x04A2,
+                      (b43_phy_read(dev, 0x04A2) & 0xBFBF) | 0x4000);
+        b43_dummy_transmission(dev);
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
+{
+        b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
+        mmiowb();
+        b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
+{
+        u16 val;
+
+        b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
+        val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
+
+        return (s16) val;
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
+{
+        u16 i;
+        s16 tmp;
+
+        for (i = 0; i < 64; i++) {
+                tmp = b43_nrssi_hw_read(dev, i);
+                tmp -= val;
+                tmp = clamp_val(tmp, -32, 31);
+                b43_nrssi_hw_write(dev, i, tmp);
+        }
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_mem_update(struct b43_wldev *dev)
+{
+        struct b43_phy_g *gphy = dev->phy.g;
+        s16 i, delta;
+        s32 tmp;
+
+        delta = 0x1F - gphy->nrssi[0];
+        for (i = 0; i < 64; i++) {
+                tmp = (i - delta) * gphy->nrssislope;
+                tmp /= 0x10000;
+                tmp += 0x3A;
+                tmp = clamp_val(tmp, 0, 0x3F);
+                gphy->nrssi_lt[i] = tmp;
+        }
+}
+
+static void b43_calc_nrssi_offset(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        u16 backup[20] = { 0 };
+        s16 v47F;
+        u16 i;
+        u16 saved = 0xFFFF;
+
+        backup[0] = b43_phy_read(dev, 0x0001);
+        backup[1] = b43_phy_read(dev, 0x0811);
+        backup[2] = b43_phy_read(dev, 0x0812);
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                backup[3] = b43_phy_read(dev, 0x0814);
+                backup[4] = b43_phy_read(dev, 0x0815);
+        }
+        backup[5] = b43_phy_read(dev, 0x005A);
+        backup[6] = b43_phy_read(dev, 0x0059);
+        backup[7] = b43_phy_read(dev, 0x0058);
+        backup[8] = b43_phy_read(dev, 0x000A);
+        backup[9] = b43_phy_read(dev, 0x0003);
+        backup[10] = b43_radio_read16(dev, 0x007A);
+        backup[11] = b43_radio_read16(dev, 0x0043);
+
+        b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) & 0x7FFF);
+        b43_phy_write(dev, 0x0001,
+                      (b43_phy_read(dev, 0x0001) & 0x3FFF) | 0x4000);
+        b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
+        b43_phy_write(dev, 0x0812,
+                      (b43_phy_read(dev, 0x0812) & 0xFFF3) | 0x0004);
+        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
+        if (phy->rev >= 6) {
+                backup[12] = b43_phy_read(dev, 0x002E);
+                backup[13] = b43_phy_read(dev, 0x002F);
+                backup[14] = b43_phy_read(dev, 0x080F);
+                backup[15] = b43_phy_read(dev, 0x0810);
+                backup[16] = b43_phy_read(dev, 0x0801);
+                backup[17] = b43_phy_read(dev, 0x0060);
+                backup[18] = b43_phy_read(dev, 0x0014);
+                backup[19] = b43_phy_read(dev, 0x0478);
+
+                b43_phy_write(dev, 0x002E, 0);
+                b43_phy_write(dev, 0x002F, 0);
+                b43_phy_write(dev, 0x080F, 0);
+                b43_phy_write(dev, 0x0810, 0);
+                b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478) | 0x0100);
+                b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801) | 0x0040);
+                b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) | 0x0040);
+                b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) | 0x0200);
+        }
+        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0070);
+        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0080);
+        udelay(30);
+
+        v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+        if (v47F >= 0x20)
+                v47F -= 0x40;
+        if (v47F == 31) {
+                for (i = 7; i >= 4; i--) {
+                        b43_radio_write16(dev, 0x007B, i);
+                        udelay(20);
+                        v47F =
+                            (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+                        if (v47F >= 0x20)
+                                v47F -= 0x40;
+                        if (v47F < 31 && saved == 0xFFFF)
+                                saved = i;
+                }
+                if (saved == 0xFFFF)
+                        saved = 4;
+        } else {
+                b43_radio_write16(dev, 0x007A,
+                                  b43_radio_read16(dev, 0x007A) & 0x007F);
+                if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                        b43_phy_write(dev, 0x0814,
+                                      b43_phy_read(dev, 0x0814) | 0x0001);
+                        b43_phy_write(dev, 0x0815,
+                                      b43_phy_read(dev, 0x0815) & 0xFFFE);
+                }
+                b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
+                b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x000C);
+                b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x0030);
+                b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x0030);
+                b43_phy_write(dev, 0x005A, 0x0480);
+                b43_phy_write(dev, 0x0059, 0x0810);
+                b43_phy_write(dev, 0x0058, 0x000D);
+                if (phy->rev == 0) {
+                        b43_phy_write(dev, 0x0003, 0x0122);
+                } else {
+                        b43_phy_write(dev, 0x000A, b43_phy_read(dev, 0x000A)
+                                      | 0x2000);
+                }
+                if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                        b43_phy_write(dev, 0x0814,
+                                      b43_phy_read(dev, 0x0814) | 0x0004);
+                        b43_phy_write(dev, 0x0815,
+                                      b43_phy_read(dev, 0x0815) & 0xFFFB);
+                }
+                b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) & 0xFF9F)
+                              | 0x0040);
+                b43_radio_write16(dev, 0x007A,
+                                  b43_radio_read16(dev, 0x007A) | 0x000F);
+                b43_set_all_gains(dev, 3, 0, 1);
+                b43_radio_write16(dev, 0x0043, (b43_radio_read16(dev, 0x0043)
+                                                & 0x00F0) | 0x000F);
+                udelay(30);
+                v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+                if (v47F >= 0x20)
+                        v47F -= 0x40;
+                if (v47F == -32) {
+                        for (i = 0; i < 4; i++) {
+                                b43_radio_write16(dev, 0x007B, i);
+                                udelay(20);
+                                v47F =
+                                    (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
+                                           0x003F);
+                                if (v47F >= 0x20)
+                                        v47F -= 0x40;
+                                if (v47F > -31 && saved == 0xFFFF)
+                                        saved = i;
+                        }
+                        if (saved == 0xFFFF)
+                                saved = 3;
+                } else
+                        saved = 0;
+        }
+        b43_radio_write16(dev, 0x007B, saved);
+
+        if (phy->rev >= 6) {
+                b43_phy_write(dev, 0x002E, backup[12]);
+                b43_phy_write(dev, 0x002F, backup[13]);
+                b43_phy_write(dev, 0x080F, backup[14]);
+                b43_phy_write(dev, 0x0810, backup[15]);
+        }
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                b43_phy_write(dev, 0x0814, backup[3]);
+                b43_phy_write(dev, 0x0815, backup[4]);
+        }
+        b43_phy_write(dev, 0x005A, backup[5]);
+        b43_phy_write(dev, 0x0059, backup[6]);
+        b43_phy_write(dev, 0x0058, backup[7]);
+        b43_phy_write(dev, 0x000A, backup[8]);
+        b43_phy_write(dev, 0x0003, backup[9]);
+        b43_radio_write16(dev, 0x0043, backup[11]);
+        b43_radio_write16(dev, 0x007A, backup[10]);
+        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
+        b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) | 0x8000);
+        b43_set_original_gains(dev);
+        if (phy->rev >= 6) {
+                b43_phy_write(dev, 0x0801, backup[16]);
+                b43_phy_write(dev, 0x0060, backup[17]);
+                b43_phy_write(dev, 0x0014, backup[18]);
+                b43_phy_write(dev, 0x0478, backup[19]);
+        }
+        b43_phy_write(dev, 0x0001, backup[0]);
+        b43_phy_write(dev, 0x0812, backup[2]);
+        b43_phy_write(dev, 0x0811, backup[1]);
+}
+
+void b43_calc_nrssi_slope(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 backup[18] = { 0 };
+        u16 tmp;
+        s16 nrssi0, nrssi1;
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+
+        if (phy->radio_rev >= 9)
+                return;
+        if (phy->radio_rev == 8)
+                b43_calc_nrssi_offset(dev);
+
+        b43_phy_write(dev, B43_PHY_G_CRS,
+                      b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
+        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
+        backup[7] = b43_read16(dev, 0x03E2);
+        b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
+        backup[0] = b43_radio_read16(dev, 0x007A);
+        backup[1] = b43_radio_read16(dev, 0x0052);
+        backup[2] = b43_radio_read16(dev, 0x0043);
+        backup[3] = b43_phy_read(dev, 0x0015);
+        backup[4] = b43_phy_read(dev, 0x005A);
+        backup[5] = b43_phy_read(dev, 0x0059);
+        backup[6] = b43_phy_read(dev, 0x0058);
+        backup[8] = b43_read16(dev, 0x03E6);
+        backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
+        if (phy->rev >= 3) {
+                backup[10] = b43_phy_read(dev, 0x002E);
+                backup[11] = b43_phy_read(dev, 0x002F);
+                backup[12] = b43_phy_read(dev, 0x080F);
+                backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
+                backup[14] = b43_phy_read(dev, 0x0801);
+                backup[15] = b43_phy_read(dev, 0x0060);
+                backup[16] = b43_phy_read(dev, 0x0014);
+                backup[17] = b43_phy_read(dev, 0x0478);
+                b43_phy_write(dev, 0x002E, 0);
+                b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
+                switch (phy->rev) {
+                case 4:
+                case 6:
+                case 7:
+                        b43_phy_write(dev, 0x0478,
+                                      b43_phy_read(dev, 0x0478)
+                                      | 0x0100);
+                        b43_phy_write(dev, 0x0801,
+                                      b43_phy_read(dev, 0x0801)
+                                      | 0x0040);
+                        break;
+                case 3:
+                case 5:
+                        b43_phy_write(dev, 0x0801,
+                                      b43_phy_read(dev, 0x0801)
+                                      & 0xFFBF);
+                        break;
+                }
+                b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060)
+                              | 0x0040);
+                b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014)
+                              | 0x0200);
+        }
+        b43_radio_write16(dev, 0x007A,
+                          b43_radio_read16(dev, 0x007A) | 0x0070);
+        b43_set_all_gains(dev, 0, 8, 0);
+        b43_radio_write16(dev, 0x007A,
+                          b43_radio_read16(dev, 0x007A) & 0x00F7);
+        if (phy->rev >= 2) {
+                b43_phy_write(dev, 0x0811,
+                              (b43_phy_read(dev, 0x0811) & 0xFFCF) |
+                              0x0030);
+                b43_phy_write(dev, 0x0812,
+                              (b43_phy_read(dev, 0x0812) & 0xFFCF) |
+                              0x0010);
+        }
+        b43_radio_write16(dev, 0x007A,
+                          b43_radio_read16(dev, 0x007A) | 0x0080);
+        udelay(20);
+
+        nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+        if (nrssi0 >= 0x0020)
+                nrssi0 -= 0x0040;
+
+        b43_radio_write16(dev, 0x007A,
+                          b43_radio_read16(dev, 0x007A) & 0x007F);
+        if (phy->rev >= 2) {
+                b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003)
+                                            & 0xFF9F) | 0x0040);
+        }
+
+        b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+                    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+                    | 0x2000);
+        b43_radio_write16(dev, 0x007A,
+                          b43_radio_read16(dev, 0x007A) | 0x000F);
+        b43_phy_write(dev, 0x0015, 0xF330);
+        if (phy->rev >= 2) {
+                b43_phy_write(dev, 0x0812,
+                              (b43_phy_read(dev, 0x0812) & 0xFFCF) |
+                              0x0020);
+                b43_phy_write(dev, 0x0811,
+                              (b43_phy_read(dev, 0x0811) & 0xFFCF) |
+                              0x0020);
+        }
+
+        b43_set_all_gains(dev, 3, 0, 1);
+        if (phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x0043, 0x001F);
+        } else {
+                tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
+                b43_radio_write16(dev, 0x0052, tmp | 0x0060);
+                tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
+                b43_radio_write16(dev, 0x0043, tmp | 0x0009);
+        }
+        b43_phy_write(dev, 0x005A, 0x0480);
+        b43_phy_write(dev, 0x0059, 0x0810);
+        b43_phy_write(dev, 0x0058, 0x000D);
+        udelay(20);
+        nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+        if (nrssi1 >= 0x0020)
+                nrssi1 -= 0x0040;
+        if (nrssi0 == nrssi1)
+                gphy->nrssislope = 0x00010000;
+        else
+                gphy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
+        if (nrssi0 >= -4) {
+                gphy->nrssi[0] = nrssi1;
+                gphy->nrssi[1] = nrssi0;
+        }
+        if (phy->rev >= 3) {
+                b43_phy_write(dev, 0x002E, backup[10]);
+                b43_phy_write(dev, 0x002F, backup[11]);
+                b43_phy_write(dev, 0x080F, backup[12]);
+                b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
+        }
+        if (phy->rev >= 2) {
+                b43_phy_write(dev, 0x0812,
+                              b43_phy_read(dev, 0x0812) & 0xFFCF);
+                b43_phy_write(dev, 0x0811,
+                              b43_phy_read(dev, 0x0811) & 0xFFCF);
+        }
+
+        b43_radio_write16(dev, 0x007A, backup[0]);
+        b43_radio_write16(dev, 0x0052, backup[1]);
+        b43_radio_write16(dev, 0x0043, backup[2]);
+        b43_write16(dev, 0x03E2, backup[7]);
+        b43_write16(dev, 0x03E6, backup[8]);
+        b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
+        b43_phy_write(dev, 0x0015, backup[3]);
+        b43_phy_write(dev, 0x005A, backup[4]);
+        b43_phy_write(dev, 0x0059, backup[5]);
+        b43_phy_write(dev, 0x0058, backup[6]);
+        b43_synth_pu_workaround(dev, phy->channel);
+        b43_phy_write(dev, 0x0802,
+                      b43_phy_read(dev, 0x0802) | (0x0001 | 0x0002));
+        b43_set_original_gains(dev);
+        b43_phy_write(dev, B43_PHY_G_CRS,
+                      b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
+        if (phy->rev >= 3) {
+                b43_phy_write(dev, 0x0801, backup[14]);
+                b43_phy_write(dev, 0x0060, backup[15]);
+                b43_phy_write(dev, 0x0014, backup[16]);
+                b43_phy_write(dev, 0x0478, backup[17]);
+        }
+        b43_nrssi_mem_update(dev);
+        b43_calc_nrssi_threshold(dev);
+}
+
+static void b43_calc_nrssi_threshold(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        s32 a, b;
+        s16 tmp16;
+        u16 tmp_u16;
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+
+        if (!phy->gmode ||
+            !(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
+                tmp16 = b43_nrssi_hw_read(dev, 0x20);
+                if (tmp16 >= 0x20)
+                        tmp16 -= 0x40;
+                if (tmp16 < 3) {
+                        b43_phy_write(dev, 0x048A,
+                                      (b43_phy_read(dev, 0x048A)
+                                       & 0xF000) | 0x09EB);
+                } else {
+                        b43_phy_write(dev, 0x048A,
+                                      (b43_phy_read(dev, 0x048A)
+                                       & 0xF000) | 0x0AED);
+                }
+        } else {
+                if (gphy->interfmode == B43_INTERFMODE_NONWLAN) {
+                        a = 0xE;
+                        b = 0xA;
+                } else if (!gphy->aci_wlan_automatic && gphy->aci_enable) {
+                        a = 0x13;
+                        b = 0x12;
+                } else {
+                        a = 0xE;
+                        b = 0x11;
+                }
+
+                a = a * (gphy->nrssi[1] - gphy->nrssi[0]);
+                a += (gphy->nrssi[0] << 6);
+                if (a < 32)
+                        a += 31;
+                else
+                        a += 32;
+                a = a >> 6;
+                a = clamp_val(a, -31, 31);
+
+                b = b * (gphy->nrssi[1] - gphy->nrssi[0]);
+                b += (gphy->nrssi[0] << 6);
+                if (b < 32)
+                        b += 31;
+                else
+                        b += 32;
+                b = b >> 6;
+                b = clamp_val(b, -31, 31);
+
+                tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
+                tmp_u16 |= ((u32) b & 0x0000003F);
+                tmp_u16 |= (((u32) a & 0x0000003F) << 6);
+                b43_phy_write(dev, 0x048A, tmp_u16);
+        }
+}
+
+/* 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]);
+
+        B43_WARN_ON(offset & 0xF000);
+        B43_WARN_ON(id & 0xF0);
+        *stackptr = offset;
+        *stackptr |= ((u32) id) << 12;
+        *stackptr |= ((u32) value) << 16;
+        (*stackidx)++;
+        B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
+}
+
+static u16 _stack_restore(u32 * stackptr, u8 id, u16 offset)
+{
+        size_t i;
+
+        B43_WARN_ON(offset & 0xF000);
+        B43_WARN_ON(id & 0xF0);
+        for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
+                if ((*stackptr & 0x00000FFF) != offset)
+                        continue;
+                if (((*stackptr & 0x0000F000) >> 12) != id)
+                        continue;
+                return ((*stackptr & 0xFFFF0000) >> 16);
+        }
+        B43_WARN_ON(1);
+
+        return 0;
+}
+
+#define phy_stacksave(offset)                                   \
+        do {                                                    \
+                _stack_save(stack, &stackidx, 0x1, (offset),    \
+                            b43_phy_read(dev, (offset)));       \
+        } while (0)
+#define phy_stackrestore(offset)                                \
+        do {                                                    \
+                b43_phy_write(dev, (offset),            \
+                                  _stack_restore(stack, 0x1,    \
+                                                 (offset)));    \
+        } while (0)
+#define radio_stacksave(offset)                                         \
+        do {                                                            \
+                _stack_save(stack, &stackidx, 0x2, (offset),            \
+                            b43_radio_read16(dev, (offset)));   \
+        } while (0)
+#define radio_stackrestore(offset)                                      \
+        do {                                                            \
+                b43_radio_write16(dev, (offset),                        \
+                                      _stack_restore(stack, 0x2,        \
+                                                     (offset)));        \
+        } while (0)
+#define ofdmtab_stacksave(table, offset)                        \
+        do {                                                    \
+                _stack_save(stack, &stackidx, 0x3, (offset)|(table),    \
+                            b43_ofdmtab_read16(dev, (table), (offset)));        \
+        } while (0)
+#define ofdmtab_stackrestore(table, offset)                     \
+        do {                                                    \
+                b43_ofdmtab_write16(dev, (table),       (offset),       \
+                                  _stack_restore(stack, 0x3,    \
+                                                 (offset)|(table)));    \
+        } while (0)
+
+static void
+b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 tmp, flipped;
+        size_t stackidx = 0;
+        u32 *stack = gphy->interfstack;
+
+        switch (mode) {
+        case B43_INTERFMODE_NONWLAN:
+                if (phy->rev != 1) {
+                        b43_phy_write(dev, 0x042B,
+                                      b43_phy_read(dev, 0x042B) | 0x0800);
+                        b43_phy_write(dev, B43_PHY_G_CRS,
+                                      b43_phy_read(dev,
+                                                   B43_PHY_G_CRS) & ~0x4000);
+                        break;
+                }
+                radio_stacksave(0x0078);
+                tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
+                B43_WARN_ON(tmp > 15);
+                flipped = bitrev4(tmp);
+                if (flipped < 10 && flipped >= 8)
+                        flipped = 7;
+                else if (flipped >= 10)
+                        flipped -= 3;
+                flipped = (bitrev4(flipped) << 1) | 0x0020;
+                b43_radio_write16(dev, 0x0078, flipped);
+
+                b43_calc_nrssi_threshold(dev);
+
+                phy_stacksave(0x0406);
+                b43_phy_write(dev, 0x0406, 0x7E28);
+
+                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x0800);
+                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+                              b43_phy_read(dev,
+                                           B43_PHY_RADIO_BITFIELD) | 0x1000);
+
+                phy_stacksave(0x04A0);
+                b43_phy_write(dev, 0x04A0,
+                              (b43_phy_read(dev, 0x04A0) & 0xC0C0) | 0x0008);
+                phy_stacksave(0x04A1);
+                b43_phy_write(dev, 0x04A1,
+                              (b43_phy_read(dev, 0x04A1) & 0xC0C0) | 0x0605);
+                phy_stacksave(0x04A2);
+                b43_phy_write(dev, 0x04A2,
+                              (b43_phy_read(dev, 0x04A2) & 0xC0C0) | 0x0204);
+                phy_stacksave(0x04A8);
+                b43_phy_write(dev, 0x04A8,
+                              (b43_phy_read(dev, 0x04A8) & 0xC0C0) | 0x0803);
+                phy_stacksave(0x04AB);
+                b43_phy_write(dev, 0x04AB,
+                              (b43_phy_read(dev, 0x04AB) & 0xC0C0) | 0x0605);
+
+                phy_stacksave(0x04A7);
+                b43_phy_write(dev, 0x04A7, 0x0002);
+                phy_stacksave(0x04A3);
+                b43_phy_write(dev, 0x04A3, 0x287A);
+                phy_stacksave(0x04A9);
+                b43_phy_write(dev, 0x04A9, 0x2027);
+                phy_stacksave(0x0493);
+                b43_phy_write(dev, 0x0493, 0x32F5);
+                phy_stacksave(0x04AA);
+                b43_phy_write(dev, 0x04AA, 0x2027);
+                phy_stacksave(0x04AC);
+                b43_phy_write(dev, 0x04AC, 0x32F5);
+                break;
+        case B43_INTERFMODE_MANUALWLAN:
+                if (b43_phy_read(dev, 0x0033) & 0x0800)
+                        break;
+
+                gphy->aci_enable = 1;
+
+                phy_stacksave(B43_PHY_RADIO_BITFIELD);
+                phy_stacksave(B43_PHY_G_CRS);
+                if (phy->rev < 2) {
+                        phy_stacksave(0x0406);
+                } else {
+                        phy_stacksave(0x04C0);
+                        phy_stacksave(0x04C1);
+                }
+                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);
+                        ofdmtab_stacksave(0x1A00, 0x2);
+                        ofdmtab_stacksave(0x1A00, 0x3);
+                }
+                phy_stacksave(0x042B);
+                phy_stacksave(0x048C);
+
+                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+                              b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
+                              & ~0x1000);
+                b43_phy_write(dev, B43_PHY_G_CRS,
+                              (b43_phy_read(dev, B43_PHY_G_CRS)
+                               & 0xFFFC) | 0x0002);
+
+                b43_phy_write(dev, 0x0033, 0x0800);
+                b43_phy_write(dev, 0x04A3, 0x2027);
+                b43_phy_write(dev, 0x04A9, 0x1CA8);
+                b43_phy_write(dev, 0x0493, 0x287A);
+                b43_phy_write(dev, 0x04AA, 0x1CA8);
+                b43_phy_write(dev, 0x04AC, 0x287A);
+
+                b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+                                            & 0xFFC0) | 0x001A);
+                b43_phy_write(dev, 0x04A7, 0x000D);
+
+                if (phy->rev < 2) {
+                        b43_phy_write(dev, 0x0406, 0xFF0D);
+                } else if (phy->rev == 2) {
+                        b43_phy_write(dev, 0x04C0, 0xFFFF);
+                        b43_phy_write(dev, 0x04C1, 0x00A9);
+                } else {
+                        b43_phy_write(dev, 0x04C0, 0x00C1);
+                        b43_phy_write(dev, 0x04C1, 0x0059);
+                }
+
+                b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
+                                            & 0xC0FF) | 0x1800);
+                b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
+                                            & 0xFFC0) | 0x0015);
+                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+                                            & 0xCFFF) | 0x1000);
+                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+                                            & 0xF0FF) | 0x0A00);
+                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+                                            & 0xCFFF) | 0x1000);
+                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+                                            & 0xF0FF) | 0x0800);
+                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+                                            & 0xFFCF) | 0x0010);
+                b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+                                            & 0xFFF0) | 0x0005);
+                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+                                            & 0xFFCF) | 0x0010);
+                b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+                                            & 0xFFF0) | 0x0006);
+                b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+                                            & 0xF0FF) | 0x0800);
+                b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+                                            & 0xF0FF) | 0x0500);
+                b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+                                            & 0xFFF0) | 0x000B);
+
+                if (phy->rev >= 3) {
+                        b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
+                                      & ~0x8000);
+                        b43_phy_write(dev, 0x0415, (b43_phy_read(dev, 0x0415)
+                                                    & 0x8000) | 0x36D8);
+                        b43_phy_write(dev, 0x0416, (b43_phy_read(dev, 0x0416)
+                                                    & 0x8000) | 0x36D8);
+                        b43_phy_write(dev, 0x0417, (b43_phy_read(dev, 0x0417)
+                                                    & 0xFE00) | 0x016D);
+                } else {
+                        b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
+                                      | 0x1000);
+                        b43_phy_write(dev, 0x048A, (b43_phy_read(dev, 0x048A)
+                                                    & 0x9FFF) | 0x2000);
+                        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
+                }
+                if (phy->rev >= 2) {
+                        b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B)
+                                      | 0x0800);
+                }
+                b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
+                                            & 0xF0FF) | 0x0200);
+                if (phy->rev == 2) {
+                        b43_phy_write(dev, 0x04AE, (b43_phy_read(dev, 0x04AE)
+                                                    & 0xFF00) | 0x007F);
+                        b43_phy_write(dev, 0x04AD, (b43_phy_read(dev, 0x04AD)
+                                                    & 0x00FF) | 0x1300);
+                } else if (phy->rev >= 6) {
+                        b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
+                        b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
+                        b43_phy_write(dev, 0x04AD, b43_phy_read(dev, 0x04AD)
+                                      & 0x00FF);
+                }
+                b43_calc_nrssi_slope(dev);
+                break;
+        default:
+                B43_WARN_ON(1);
+        }
+}
+
+static void
+b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u32 *stack = gphy->interfstack;
+
+        switch (mode) {
+        case B43_INTERFMODE_NONWLAN:
+                if (phy->rev != 1) {
+                        b43_phy_write(dev, 0x042B,
+                                      b43_phy_read(dev, 0x042B) & ~0x0800);
+                        b43_phy_write(dev, B43_PHY_G_CRS,
+                                      b43_phy_read(dev,
+                                                   B43_PHY_G_CRS) | 0x4000);
+                        break;
+                }
+                radio_stackrestore(0x0078);
+                b43_calc_nrssi_threshold(dev);
+                phy_stackrestore(0x0406);
+                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) & ~0x0800);
+                if (!dev->bad_frames_preempt) {
+                        b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+                                      b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
+                                      & ~(1 << 11));
+                }
+                b43_phy_write(dev, B43_PHY_G_CRS,
+                              b43_phy_read(dev, B43_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 B43_INTERFMODE_MANUALWLAN:
+                if (!(b43_phy_read(dev, 0x0033) & 0x0800))
+                        break;
+
+                gphy->aci_enable = 0;
+
+                phy_stackrestore(B43_PHY_RADIO_BITFIELD);
+                phy_stackrestore(B43_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);
+                        ofdmtab_stackrestore(0x1A00, 0x2);
+                        ofdmtab_stackrestore(0x1A00, 0x3);
+                }
+                phy_stackrestore(0x04A2);
+                phy_stackrestore(0x048A);
+                phy_stackrestore(0x042B);
+                phy_stackrestore(0x048C);
+                b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
+                b43_calc_nrssi_slope(dev);
+                break;
+        default:
+                B43_WARN_ON(1);
+        }
+}
+
+#undef phy_stacksave
+#undef phy_stackrestore
+#undef radio_stacksave
+#undef radio_stackrestore
+#undef ofdmtab_stacksave
+#undef ofdmtab_stackrestore
+
+static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
+{
+        u16 reg, index, ret;
+
+        static const u8 rcc_table[] = {
+                0x02, 0x03, 0x01, 0x0F,
+                0x06, 0x07, 0x05, 0x0F,
+                0x0A, 0x0B, 0x09, 0x0F,
+                0x0E, 0x0F, 0x0D, 0x0F,
+        };
+
+        reg = b43_radio_read16(dev, 0x60);
+        index = (reg & 0x001E) >> 1;
+        ret = rcc_table[index] << 1;
+        ret |= (reg & 0x0001);
+        ret |= 0x0020;
+
+        return ret;
+}
+
+#define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
+static u16 radio2050_rfover_val(struct b43_wldev *dev,
+                                u16 phy_register, unsigned int lpd)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
+
+        if (!phy->gmode)
+                return 0;
+
+        if (has_loopback_gain(phy)) {
+                int max_lb_gain = gphy->max_lb_gain;
+                u16 extlna;
+                u16 i;
+
+                if (phy->radio_rev == 8)
+                        max_lb_gain += 0x3E;
+                else
+                        max_lb_gain += 0x26;
+                if (max_lb_gain >= 0x46) {
+                        extlna = 0x3000;
+                        max_lb_gain -= 0x46;
+                } else if (max_lb_gain >= 0x3A) {
+                        extlna = 0x1000;
+                        max_lb_gain -= 0x3A;
+                } else if (max_lb_gain >= 0x2E) {
+                        extlna = 0x2000;
+                        max_lb_gain -= 0x2E;
+                } else {
+                        extlna = 0;
+                        max_lb_gain -= 0x10;
+                }
+
+                for (i = 0; i < 16; i++) {
+                        max_lb_gain -= (i * 6);
+                        if (max_lb_gain < 6)
+                                break;
+                }
+
+                if ((phy->rev < 7) ||
+                    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
+                        if (phy_register == B43_PHY_RFOVER) {
+                                return 0x1B3;
+                        } else if (phy_register == B43_PHY_RFOVERVAL) {
+                                extlna |= (i << 8);
+                                switch (lpd) {
+                                case LPD(0, 1, 1):
+                                        return 0x0F92;
+                                case LPD(0, 0, 1):
+                                case LPD(1, 0, 1):
+                                        return (0x0092 | extlna);
+                                case LPD(1, 0, 0):
+                                        return (0x0093 | extlna);
+                                }
+                                B43_WARN_ON(1);
+                        }
+                        B43_WARN_ON(1);
+                } else {
+                        if (phy_register == B43_PHY_RFOVER) {
+                                return 0x9B3;
+                        } else if (phy_register == B43_PHY_RFOVERVAL) {
+                                if (extlna)
+                                        extlna |= 0x8000;
+                                extlna |= (i << 8);
+                                switch (lpd) {
+                                case LPD(0, 1, 1):
+                                        return 0x8F92;
+                                case LPD(0, 0, 1):
+                                        return (0x8092 | extlna);
+                                case LPD(1, 0, 1):
+                                        return (0x2092 | extlna);
+                                case LPD(1, 0, 0):
+                                        return (0x2093 | extlna);
+                                }
+                                B43_WARN_ON(1);
+                        }
+                        B43_WARN_ON(1);
+                }
+        } else {
+                if ((phy->rev < 7) ||
+                    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
+                        if (phy_register == B43_PHY_RFOVER) {
+                                return 0x1B3;
+                        } else if (phy_register == B43_PHY_RFOVERVAL) {
+                                switch (lpd) {
+                                case LPD(0, 1, 1):
+                                        return 0x0FB2;
+                                case LPD(0, 0, 1):
+                                        return 0x00B2;
+                                case LPD(1, 0, 1):
+                                        return 0x30B2;
+                                case LPD(1, 0, 0):
+                                        return 0x30B3;
+                                }
+                                B43_WARN_ON(1);
+                        }
+                        B43_WARN_ON(1);
+                } else {
+                        if (phy_register == B43_PHY_RFOVER) {
+                                return 0x9B3;
+                        } else if (phy_register == B43_PHY_RFOVERVAL) {
+                                switch (lpd) {
+                                case LPD(0, 1, 1):
+                                        return 0x8FB2;
+                                case LPD(0, 0, 1):
+                                        return 0x80B2;
+                                case LPD(1, 0, 1):
+                                        return 0x20B2;
+                                case LPD(1, 0, 0):
+                                        return 0x20B3;
+                                }
+                                B43_WARN_ON(1);
+                        }
+                        B43_WARN_ON(1);
+                }
+        }
+        return 0;
+}
+
+struct init2050_saved_values {
+        /* Core registers */
+        u16 reg_3EC;
+        u16 reg_3E6;
+        u16 reg_3F4;
+        /* Radio registers */
+        u16 radio_43;
+        u16 radio_51;
+        u16 radio_52;
+        /* PHY registers */
+        u16 phy_pgactl;
+        u16 phy_cck_5A;
+        u16 phy_cck_59;
+        u16 phy_cck_58;
+        u16 phy_cck_30;
+        u16 phy_rfover;
+        u16 phy_rfoverval;
+        u16 phy_analogover;
+        u16 phy_analogoverval;
+        u16 phy_crs0;
+        u16 phy_classctl;
+        u16 phy_lo_mask;
+        u16 phy_lo_ctl;
+        u16 phy_syncctl;
+};
+
+u16 b43_radio_init2050(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct init2050_saved_values sav;
+        u16 rcc;
+        u16 radio78;
+        u16 ret;
+        u16 i, j;
+        u32 tmp1 = 0, tmp2 = 0;
+
+        memset(&sav, 0, sizeof(sav));   /* get rid of "may be used uninitialized..." */
+
+        sav.radio_43 = b43_radio_read16(dev, 0x43);
+        sav.radio_51 = b43_radio_read16(dev, 0x51);
+        sav.radio_52 = b43_radio_read16(dev, 0x52);
+        sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
+        sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
+        sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
+        sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
+
+        if (phy->type == B43_PHYTYPE_B) {
+                sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
+                sav.reg_3EC = b43_read16(dev, 0x3EC);
+
+                b43_phy_write(dev, B43_PHY_CCK(0x30), 0xFF);
+                b43_write16(dev, 0x3EC, 0x3F3F);
+        } else if (phy->gmode || phy->rev >= 2) {
+                sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
+                sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+                sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
+                sav.phy_analogoverval =
+                    b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
+                sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
+                sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
+
+                b43_phy_write(dev, B43_PHY_ANALOGOVER,
+                              b43_phy_read(dev, B43_PHY_ANALOGOVER)
+                              | 0x0003);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+                              b43_phy_read(dev, B43_PHY_ANALOGOVERVAL)
+                              & 0xFFFC);
+                b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
+                              & 0x7FFF);
+                b43_phy_write(dev, B43_PHY_CLASSCTL,
+                              b43_phy_read(dev, B43_PHY_CLASSCTL)
+                              & 0xFFFC);
+                if (has_loopback_gain(phy)) {
+                        sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
+                        sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
+
+                        if (phy->rev >= 3)
+                                b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
+                        else
+                                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
+                        b43_phy_write(dev, B43_PHY_LO_CTL, 0);
+                }
+
+                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+                                                   LPD(0, 1, 1)));
+                b43_phy_write(dev, B43_PHY_RFOVER,
+                              radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
+        }
+        b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
+
+        sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
+        b43_phy_write(dev, B43_PHY_SYNCCTL, b43_phy_read(dev, B43_PHY_SYNCCTL)
+                      & 0xFF7F);
+        sav.reg_3E6 = b43_read16(dev, 0x3E6);
+        sav.reg_3F4 = b43_read16(dev, 0x3F4);
+
+        if (phy->analog == 0) {
+                b43_write16(dev, 0x03E6, 0x0122);
+        } else {
+                if (phy->analog >= 2) {
+                        b43_phy_write(dev, B43_PHY_CCK(0x03),
+                                      (b43_phy_read(dev, B43_PHY_CCK(0x03))
+                                       & 0xFFBF) | 0x40);
+                }
+                b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+                            (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
+        }
+
+        rcc = b43_radio_core_calibration_value(dev);
+
+        if (phy->type == B43_PHYTYPE_B)
+                b43_radio_write16(dev, 0x78, 0x26);
+        if (phy->gmode || phy->rev >= 2) {
+                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+                                                   LPD(0, 1, 1)));
+        }
+        b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
+        b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x1403);
+        if (phy->gmode || phy->rev >= 2) {
+                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                              radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+                                                   LPD(0, 0, 1)));
+        }
+        b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
+        b43_radio_write16(dev, 0x51, b43_radio_read16(dev, 0x51)
+                          | 0x0004);
+        if (phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x43, 0x1F);
+        } else {
+                b43_radio_write16(dev, 0x52, 0);
+                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+                                              & 0xFFF0) | 0x0009);
+        }
+        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
+
+        for (i = 0; i < 16; i++) {
+                b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0480);
+                b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
+                b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
+                if (phy->gmode || phy->rev >= 2) {
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      radio2050_rfover_val(dev,
+                                                           B43_PHY_RFOVERVAL,
+                                                           LPD(1, 0, 1)));
+                }
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+                udelay(10);
+                if (phy->gmode || phy->rev >= 2) {
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      radio2050_rfover_val(dev,
+                                                           B43_PHY_RFOVERVAL,
+                                                           LPD(1, 0, 1)));
+                }
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
+                udelay(10);
+                if (phy->gmode || phy->rev >= 2) {
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      radio2050_rfover_val(dev,
+                                                           B43_PHY_RFOVERVAL,
+                                                           LPD(1, 0, 0)));
+                }
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
+                udelay(20);
+                tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+                b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
+                if (phy->gmode || phy->rev >= 2) {
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      radio2050_rfover_val(dev,
+                                                           B43_PHY_RFOVERVAL,
+                                                           LPD(1, 0, 1)));
+                }
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+        }
+        udelay(10);
+
+        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
+        tmp1++;
+        tmp1 >>= 9;
+
+        for (i = 0; i < 16; i++) {
+                radio78 = (bitrev4(i) << 1) | 0x0020;
+                b43_radio_write16(dev, 0x78, radio78);
+                udelay(10);
+                for (j = 0; j < 16; j++) {
+                        b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0D80);
+                        b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
+                        b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
+                        if (phy->gmode || phy->rev >= 2) {
+                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                              radio2050_rfover_val(dev,
+                                                                   B43_PHY_RFOVERVAL,
+                                                                   LPD(1, 0,
+                                                                       1)));
+                        }
+                        b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+                        udelay(10);
+                        if (phy->gmode || phy->rev >= 2) {
+                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                              radio2050_rfover_val(dev,
+                                                                   B43_PHY_RFOVERVAL,
+                                                                   LPD(1, 0,
+                                                                       1)));
+                        }
+                        b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
+                        udelay(10);
+                        if (phy->gmode || phy->rev >= 2) {
+                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                              radio2050_rfover_val(dev,
+                                                                   B43_PHY_RFOVERVAL,
+                                                                   LPD(1, 0,
+                                                                       0)));
+                        }
+                        b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
+                        udelay(10);
+                        tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+                        b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
+                        if (phy->gmode || phy->rev >= 2) {
+                                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                              radio2050_rfover_val(dev,
+                                                                   B43_PHY_RFOVERVAL,
+                                                                   LPD(1, 0,
+                                                                       1)));
+                        }
+                        b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+                }
+                tmp2++;
+                tmp2 >>= 8;
+                if (tmp1 < tmp2)
+                        break;
+        }
+
+        /* Restore the registers */
+        b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
+        b43_radio_write16(dev, 0x51, sav.radio_51);
+        b43_radio_write16(dev, 0x52, sav.radio_52);
+        b43_radio_write16(dev, 0x43, sav.radio_43);
+        b43_phy_write(dev, B43_PHY_CCK(0x5A), sav.phy_cck_5A);
+        b43_phy_write(dev, B43_PHY_CCK(0x59), sav.phy_cck_59);
+        b43_phy_write(dev, B43_PHY_CCK(0x58), sav.phy_cck_58);
+        b43_write16(dev, 0x3E6, sav.reg_3E6);
+        if (phy->analog != 0)
+                b43_write16(dev, 0x3F4, sav.reg_3F4);
+        b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
+        b43_synth_pu_workaround(dev, phy->channel);
+        if (phy->type == B43_PHYTYPE_B) {
+                b43_phy_write(dev, B43_PHY_CCK(0x30), sav.phy_cck_30);
+                b43_write16(dev, 0x3EC, sav.reg_3EC);
+        } else if (phy->gmode) {
+                b43_write16(dev, B43_MMIO_PHY_RADIO,
+                            b43_read16(dev, B43_MMIO_PHY_RADIO)
+                            & 0x7FFF);
+                b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
+                b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
+                b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+                              sav.phy_analogoverval);
+                b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
+                b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
+                if (has_loopback_gain(phy)) {
+                        b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
+                        b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
+                }
+        }
+        if (i > 15)
+                ret = radio78;
+        else
+                ret = rcc;
+
+        return ret;
+}
+
+static void b43_phy_initb5(struct b43_wldev *dev)
+{
+        struct ssb_bus *bus = dev->dev->bus;
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 offset, value;
+        u8 old_channel;
+
+        if (phy->analog == 1) {
+                b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A)
+                                  | 0x0050);
+        }
+        if ((bus->boardinfo.vendor != SSB_BOARDVENDOR_BCM) &&
+            (bus->boardinfo.type != SSB_BOARD_BU4306)) {
+                value = 0x2120;
+                for (offset = 0x00A8; offset < 0x00C7; offset++) {
+                        b43_phy_write(dev, offset, value);
+                        value += 0x202;
+                }
+        }
+        b43_phy_write(dev, 0x0035, (b43_phy_read(dev, 0x0035) & 0xF0FF)
+                      | 0x0700);
+        if (phy->radio_ver == 0x2050)
+                b43_phy_write(dev, 0x0038, 0x0667);
+
+        if (phy->gmode || phy->rev >= 2) {
+                if (phy->radio_ver == 0x2050) {
+                        b43_radio_write16(dev, 0x007A,
+                                          b43_radio_read16(dev, 0x007A)
+                                          | 0x0020);
+                        b43_radio_write16(dev, 0x0051,
+                                          b43_radio_read16(dev, 0x0051)
+                                          | 0x0004);
+                }
+                b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
+
+                b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
+                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
+
+                b43_phy_write(dev, 0x001C, 0x186A);
+
+                b43_phy_write(dev, 0x0013,
+                              (b43_phy_read(dev, 0x0013) & 0x00FF) | 0x1900);
+                b43_phy_write(dev, 0x0035,
+                              (b43_phy_read(dev, 0x0035) & 0xFFC0) | 0x0064);
+                b43_phy_write(dev, 0x005D,
+                              (b43_phy_read(dev, 0x005D) & 0xFF80) | 0x000A);
+        }
+
+        if (dev->bad_frames_preempt) {
+                b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+                              b43_phy_read(dev,
+                                           B43_PHY_RADIO_BITFIELD) | (1 << 11));
+        }
+
+        if (phy->analog == 1) {
+                b43_phy_write(dev, 0x0026, 0xCE00);
+                b43_phy_write(dev, 0x0021, 0x3763);
+                b43_phy_write(dev, 0x0022, 0x1BC3);
+                b43_phy_write(dev, 0x0023, 0x06F9);
+                b43_phy_write(dev, 0x0024, 0x037E);
+        } else
+                b43_phy_write(dev, 0x0026, 0xCC00);
+        b43_phy_write(dev, 0x0030, 0x00C6);
+        b43_write16(dev, 0x03EC, 0x3F22);
+
+        if (phy->analog == 1)
+                b43_phy_write(dev, 0x0020, 0x3E1C);
+        else
+                b43_phy_write(dev, 0x0020, 0x301C);
+
+        if (phy->analog == 0)
+                b43_write16(dev, 0x03E4, 0x3000);
+
+        old_channel = phy->channel;
+        /* Force to channel 7, even if not supported. */
+        b43_gphy_channel_switch(dev, 7, 0);
+
+        if (phy->radio_ver != 0x2050) {
+                b43_radio_write16(dev, 0x0075, 0x0080);
+                b43_radio_write16(dev, 0x0079, 0x0081);
+        }
+
+        b43_radio_write16(dev, 0x0050, 0x0020);
+        b43_radio_write16(dev, 0x0050, 0x0023);
+
+        if (phy->radio_ver == 0x2050) {
+                b43_radio_write16(dev, 0x0050, 0x0020);
+                b43_radio_write16(dev, 0x005A, 0x0070);
+        }
+
+        b43_radio_write16(dev, 0x005B, 0x007B);
+        b43_radio_write16(dev, 0x005C, 0x00B0);
+
+        b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0007);
+
+        b43_gphy_channel_switch(dev, old_channel, 0);
+
+        b43_phy_write(dev, 0x0014, 0x0080);
+        b43_phy_write(dev, 0x0032, 0x00CA);
+        b43_phy_write(dev, 0x002A, 0x88A3);
+
+        b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
+
+        if (phy->radio_ver == 0x2050)
+                b43_radio_write16(dev, 0x005D, 0x000D);
+
+        b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
+}
+
+static void b43_phy_initb6(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 offset, val;
+        u8 old_channel;
+
+        b43_phy_write(dev, 0x003E, 0x817A);
+        b43_radio_write16(dev, 0x007A,
+                          (b43_radio_read16(dev, 0x007A) | 0x0058));
+        if (phy->radio_rev == 4 || phy->radio_rev == 5) {
+                b43_radio_write16(dev, 0x51, 0x37);
+                b43_radio_write16(dev, 0x52, 0x70);
+                b43_radio_write16(dev, 0x53, 0xB3);
+                b43_radio_write16(dev, 0x54, 0x9B);
+                b43_radio_write16(dev, 0x5A, 0x88);
+                b43_radio_write16(dev, 0x5B, 0x88);
+                b43_radio_write16(dev, 0x5D, 0x88);
+                b43_radio_write16(dev, 0x5E, 0x88);
+                b43_radio_write16(dev, 0x7D, 0x88);
+                b43_hf_write(dev, b43_hf_read(dev)
+                             | B43_HF_TSSIRPSMW);
+        }
+        B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);        /* We had code for these revs here... */
+        if (phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x51, 0);
+                b43_radio_write16(dev, 0x52, 0x40);
+                b43_radio_write16(dev, 0x53, 0xB7);
+                b43_radio_write16(dev, 0x54, 0x98);
+                b43_radio_write16(dev, 0x5A, 0x88);
+                b43_radio_write16(dev, 0x5B, 0x6B);
+                b43_radio_write16(dev, 0x5C, 0x0F);
+                if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_ALTIQ) {
+                        b43_radio_write16(dev, 0x5D, 0xFA);
+                        b43_radio_write16(dev, 0x5E, 0xD8);
+                } else {
+                        b43_radio_write16(dev, 0x5D, 0xF5);
+                        b43_radio_write16(dev, 0x5E, 0xB8);
+                }
+                b43_radio_write16(dev, 0x0073, 0x0003);
+                b43_radio_write16(dev, 0x007D, 0x00A8);
+                b43_radio_write16(dev, 0x007C, 0x0001);
+                b43_radio_write16(dev, 0x007E, 0x0008);
+        }
+        val = 0x1E1F;
+        for (offset = 0x0088; offset < 0x0098; offset++) {
+                b43_phy_write(dev, offset, val);
+                val -= 0x0202;
+        }
+        val = 0x3E3F;
+        for (offset = 0x0098; offset < 0x00A8; offset++) {
+                b43_phy_write(dev, offset, val);
+                val -= 0x0202;
+        }
+        val = 0x2120;
+        for (offset = 0x00A8; offset < 0x00C8; offset++) {
+                b43_phy_write(dev, offset, (val & 0x3F3F));
+                val += 0x0202;
+        }
+        if (phy->type == B43_PHYTYPE_G) {
+                b43_radio_write16(dev, 0x007A,
+                                  b43_radio_read16(dev, 0x007A) | 0x0020);
+                b43_radio_write16(dev, 0x0051,
+                                  b43_radio_read16(dev, 0x0051) | 0x0004);
+                b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
+                b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
+                b43_phy_write(dev, 0x5B, 0);
+                b43_phy_write(dev, 0x5C, 0);
+        }
+
+        old_channel = phy->channel;
+        if (old_channel >= 8)
+                b43_gphy_channel_switch(dev, 1, 0);
+        else
+                b43_gphy_channel_switch(dev, 13, 0);
+
+        b43_radio_write16(dev, 0x0050, 0x0020);
+        b43_radio_write16(dev, 0x0050, 0x0023);
+        udelay(40);
+        if (phy->radio_rev < 6 || phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
+                                              | 0x0002));
+                b43_radio_write16(dev, 0x50, 0x20);
+        }
+        if (phy->radio_rev <= 2) {
+                b43_radio_write16(dev, 0x7C, 0x20);
+                b43_radio_write16(dev, 0x5A, 0x70);
+                b43_radio_write16(dev, 0x5B, 0x7B);
+                b43_radio_write16(dev, 0x5C, 0xB0);
+        }
+        b43_radio_write16(dev, 0x007A,
+                          (b43_radio_read16(dev, 0x007A) & 0x00F8) | 0x0007);
+
+        b43_gphy_channel_switch(dev, old_channel, 0);
+
+        b43_phy_write(dev, 0x0014, 0x0200);
+        if (phy->radio_rev >= 6)
+                b43_phy_write(dev, 0x2A, 0x88C2);
+        else
+                b43_phy_write(dev, 0x2A, 0x8AC0);
+        b43_phy_write(dev, 0x0038, 0x0668);
+        b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
+        if (phy->radio_rev <= 5) {
+                b43_phy_write(dev, 0x5D, (b43_phy_read(dev, 0x5D)
+                                          & 0xFF80) | 0x0003);
+        }
+        if (phy->radio_rev <= 2)
+                b43_radio_write16(dev, 0x005D, 0x000D);
+
+        if (phy->analog == 4) {
+                b43_write16(dev, 0x3E4, 9);
+                b43_phy_write(dev, 0x61, b43_phy_read(dev, 0x61)
+                              & 0x0FFF);
+        } else {
+                b43_phy_write(dev, 0x0002, (b43_phy_read(dev, 0x0002) & 0xFFC0)
+                              | 0x0004);
+        }
+        if (phy->type == B43_PHYTYPE_B)
+                B43_WARN_ON(1);
+        else if (phy->type == B43_PHYTYPE_G)
+                b43_write16(dev, 0x03E6, 0x0);
+}
+
+static void b43_calc_loopback_gain(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 backup_phy[16] = { 0 };
+        u16 backup_radio[3];
+        u16 backup_bband;
+        u16 i, j, loop_i_max;
+        u16 trsw_rx;
+        u16 loop1_outer_done, loop1_inner_done;
+
+        backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
+        backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
+        backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
+        backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
+                backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
+        }
+        backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
+        backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
+        backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
+        backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
+        backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
+        backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
+        backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
+        backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
+        backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
+        backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+        backup_bband = gphy->bbatt.att;
+        backup_radio[0] = b43_radio_read16(dev, 0x52);
+        backup_radio[1] = b43_radio_read16(dev, 0x43);
+        backup_radio[2] = b43_radio_read16(dev, 0x7A);
+
+        b43_phy_write(dev, B43_PHY_CRS0,
+                      b43_phy_read(dev, B43_PHY_CRS0) & 0x3FFF);
+        b43_phy_write(dev, B43_PHY_CCKBBANDCFG,
+                      b43_phy_read(dev, B43_PHY_CCKBBANDCFG) | 0x8000);
+        b43_phy_write(dev, B43_PHY_RFOVER,
+                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0002);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFD);
+        b43_phy_write(dev, B43_PHY_RFOVER,
+                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0001);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFE);
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                b43_phy_write(dev, B43_PHY_ANALOGOVER,
+                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0001);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+                              b43_phy_read(dev,
+                                           B43_PHY_ANALOGOVERVAL) & 0xFFFE);
+                b43_phy_write(dev, B43_PHY_ANALOGOVER,
+                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0002);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+                              b43_phy_read(dev,
+                                           B43_PHY_ANALOGOVERVAL) & 0xFFFD);
+        }
+        b43_phy_write(dev, B43_PHY_RFOVER,
+                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x000C);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                      b43_phy_read(dev, B43_PHY_RFOVERVAL) | 0x000C);
+        b43_phy_write(dev, B43_PHY_RFOVER,
+                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0030);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+                       & 0xFFCF) | 0x10);
+
+        b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0780);
+        b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
+        b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
+
+        b43_phy_write(dev, B43_PHY_CCK(0x0A),
+                      b43_phy_read(dev, B43_PHY_CCK(0x0A)) | 0x2000);
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                b43_phy_write(dev, B43_PHY_ANALOGOVER,
+                              b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0004);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+                              b43_phy_read(dev,
+                                           B43_PHY_ANALOGOVERVAL) & 0xFFFB);
+        }
+        b43_phy_write(dev, B43_PHY_CCK(0x03),
+                      (b43_phy_read(dev, B43_PHY_CCK(0x03))
+                       & 0xFF9F) | 0x40);
+
+        if (phy->radio_rev == 8) {
+                b43_radio_write16(dev, 0x43, 0x000F);
+        } else {
+                b43_radio_write16(dev, 0x52, 0);
+                b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+                                              & 0xFFF0) | 0x9);
+        }
+        b43_gphy_set_baseband_attenuation(dev, 11);
+
+        if (phy->rev >= 3)
+                b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
+        else
+                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
+        b43_phy_write(dev, B43_PHY_LO_CTL, 0);
+
+        b43_phy_write(dev, B43_PHY_CCK(0x2B),
+                      (b43_phy_read(dev, B43_PHY_CCK(0x2B))
+                       & 0xFFC0) | 0x01);
+        b43_phy_write(dev, B43_PHY_CCK(0x2B),
+                      (b43_phy_read(dev, B43_PHY_CCK(0x2B))
+                       & 0xC0FF) | 0x800);
+
+        b43_phy_write(dev, B43_PHY_RFOVER,
+                      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0100);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xCFFF);
+
+        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_EXTLNA) {
+                if (phy->rev >= 7) {
+                        b43_phy_write(dev, B43_PHY_RFOVER,
+                                      b43_phy_read(dev, B43_PHY_RFOVER)
+                                      | 0x0800);
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      b43_phy_read(dev, B43_PHY_RFOVERVAL)
+                                      | 0x8000);
+                }
+        }
+        b43_radio_write16(dev, 0x7A, b43_radio_read16(dev, 0x7A)
+                          & 0x00F7);
+
+        j = 0;
+        loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
+        for (i = 0; i < loop_i_max; i++) {
+                for (j = 0; j < 16; j++) {
+                        b43_radio_write16(dev, 0x43, i);
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+                                       & 0xF0FF) | (j << 8));
+                        b43_phy_write(dev, B43_PHY_PGACTL,
+                                      (b43_phy_read(dev, B43_PHY_PGACTL)
+                                       & 0x0FFF) | 0xA000);
+                        b43_phy_write(dev, B43_PHY_PGACTL,
+                                      b43_phy_read(dev, B43_PHY_PGACTL)
+                                      | 0xF000);
+                        udelay(20);
+                        if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
+                                goto exit_loop1;
+                }
+        }
+      exit_loop1:
+        loop1_outer_done = i;
+        loop1_inner_done = j;
+        if (j >= 8) {
+                b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                              b43_phy_read(dev, B43_PHY_RFOVERVAL)
+                              | 0x30);
+                trsw_rx = 0x1B;
+                for (j = j - 8; j < 16; j++) {
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+                                       & 0xF0FF) | (j << 8));
+                        b43_phy_write(dev, B43_PHY_PGACTL,
+                                      (b43_phy_read(dev, B43_PHY_PGACTL)
+                                       & 0x0FFF) | 0xA000);
+                        b43_phy_write(dev, B43_PHY_PGACTL,
+                                      b43_phy_read(dev, B43_PHY_PGACTL)
+                                      | 0xF000);
+                        udelay(20);
+                        trsw_rx -= 3;
+                        if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
+                                goto exit_loop2;
+                }
+        } else
+                trsw_rx = 0x18;
+      exit_loop2:
+
+        if (phy->rev != 1) {    /* Not in specs, but needed to prevent PPC machine check */
+                b43_phy_write(dev, B43_PHY_ANALOGOVER, backup_phy[4]);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, backup_phy[5]);
+        }
+        b43_phy_write(dev, B43_PHY_CCK(0x5A), backup_phy[6]);
+        b43_phy_write(dev, B43_PHY_CCK(0x59), backup_phy[7]);
+        b43_phy_write(dev, B43_PHY_CCK(0x58), backup_phy[8]);
+        b43_phy_write(dev, B43_PHY_CCK(0x0A), backup_phy[9]);
+        b43_phy_write(dev, B43_PHY_CCK(0x03), backup_phy[10]);
+        b43_phy_write(dev, B43_PHY_LO_MASK, backup_phy[11]);
+        b43_phy_write(dev, B43_PHY_LO_CTL, backup_phy[12]);
+        b43_phy_write(dev, B43_PHY_CCK(0x2B), backup_phy[13]);
+        b43_phy_write(dev, B43_PHY_PGACTL, backup_phy[14]);
+
+        b43_gphy_set_baseband_attenuation(dev, backup_bband);
+
+        b43_radio_write16(dev, 0x52, backup_radio[0]);
+        b43_radio_write16(dev, 0x43, backup_radio[1]);
+        b43_radio_write16(dev, 0x7A, backup_radio[2]);
+
+        b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2] | 0x0003);
+        udelay(10);
+        b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2]);
+        b43_phy_write(dev, B43_PHY_RFOVERVAL, backup_phy[3]);
+        b43_phy_write(dev, B43_PHY_CRS0, backup_phy[0]);
+        b43_phy_write(dev, B43_PHY_CCKBBANDCFG, backup_phy[1]);
+
+        gphy->max_lb_gain =
+            ((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
+        gphy->trsw_rx_gain = trsw_rx * 2;
+}
+
+static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (!b43_has_hardware_pctl(dev)) {
+                b43_phy_write(dev, 0x047A, 0xC111);
+                return;
+        }
+
+        b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036) & 0xFEFF);
+        b43_phy_write(dev, 0x002F, 0x0202);
+        b43_phy_write(dev, 0x047C, b43_phy_read(dev, 0x047C) | 0x0002);
+        b43_phy_write(dev, 0x047A, b43_phy_read(dev, 0x047A) | 0xF000);
+        if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+                b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
+                                            & 0xFF0F) | 0x0010);
+                b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
+                              | 0x8000);
+                b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
+                                            & 0xFFC0) | 0x0010);
+                b43_phy_write(dev, 0x002E, 0xC07F);
+                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+                              | 0x0400);
+        } else {
+                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+                              | 0x0200);
+                b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+                              | 0x0400);
+                b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
+                              & 0x7FFF);
+                b43_phy_write(dev, 0x004F, b43_phy_read(dev, 0x004F)
+                              & 0xFFFE);
+                b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
+                                            & 0xFFC0) | 0x0010);
+                b43_phy_write(dev, 0x002E, 0xC07F);
+                b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
+                                            & 0xFF0F) | 0x0010);
+        }
+}
+
+/* Hardware power control for G-PHY */
+static void b43_hardware_pctl_init_gphy(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+
+        if (!b43_has_hardware_pctl(dev)) {
+                /* No hardware power control */
+                b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_HWPCTL);
+                return;
+        }
+
+        b43_phy_write(dev, 0x0036, (b43_phy_read(dev, 0x0036) & 0xFFC0)
+                      | (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
+        b43_phy_write(dev, 0x0478, (b43_phy_read(dev, 0x0478) & 0xFF00)
+                      | (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
+        b43_gphy_tssi_power_lt_init(dev);
+        b43_gphy_gain_lt_init(dev);
+        b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) & 0xFFBF);
+        b43_phy_write(dev, 0x0014, 0x0000);
+
+        B43_WARN_ON(phy->rev < 6);
+        b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
+                      | 0x0800);
+        b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
+                      & 0xFEFF);
+        b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801)
+                      & 0xFFBF);
+
+        b43_gphy_dc_lt_init(dev, 1);
+
+        /* Enable hardware pctl in firmware. */
+        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_HWPCTL);
+}
+
+/* Intialize B/G PHY power control */
+static void b43_phy_init_pctl(struct b43_wldev *dev)
+{
+        struct ssb_bus *bus = dev->dev->bus;
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_rfatt old_rfatt;
+        struct b43_bbatt old_bbatt;
+        u8 old_tx_control = 0;
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+
+        if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+            (bus->boardinfo.type == SSB_BOARD_BU4306))
+                return;
+
+        b43_phy_write(dev, 0x0028, 0x8018);
+
+        /* This does something with the Analog... */
+        b43_write16(dev, B43_MMIO_PHY0, b43_read16(dev, B43_MMIO_PHY0)
+                    & 0xFFDF);
+
+        if (!phy->gmode)
+                return;
+        b43_hardware_pctl_early_init(dev);
+        if (gphy->cur_idle_tssi == 0) {
+                if (phy->radio_ver == 0x2050 && phy->analog == 0) {
+                        b43_radio_write16(dev, 0x0076,
+                                          (b43_radio_read16(dev, 0x0076)
+                                           & 0x00F7) | 0x0084);
+                } else {
+                        struct b43_rfatt rfatt;
+                        struct b43_bbatt bbatt;
+
+                        memcpy(&old_rfatt, &gphy->rfatt, sizeof(old_rfatt));
+                        memcpy(&old_bbatt, &gphy->bbatt, sizeof(old_bbatt));
+                        old_tx_control = gphy->tx_control;
+
+                        bbatt.att = 11;
+                        if (phy->radio_rev == 8) {
+                                rfatt.att = 15;
+                                rfatt.with_padmix = 1;
+                        } else {
+                                rfatt.att = 9;
+                                rfatt.with_padmix = 0;
+                        }
+                        b43_set_txpower_g(dev, &bbatt, &rfatt, 0);
+                }
+                b43_dummy_transmission(dev);
+                gphy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
+                if (B43_DEBUG) {
+                        /* Current-Idle-TSSI sanity check. */
+                        if (abs(gphy->cur_idle_tssi - gphy->tgt_idle_tssi) >= 20) {
+                                b43dbg(dev->wl,
+                                       "!WARNING! Idle-TSSI phy->cur_idle_tssi "
+                                       "measuring failed. (cur=%d, tgt=%d). Disabling TX power "
+                                       "adjustment.\n", gphy->cur_idle_tssi,
+                                       gphy->tgt_idle_tssi);
+                                gphy->cur_idle_tssi = 0;
+                        }
+                }
+                if (phy->radio_ver == 0x2050 && phy->analog == 0) {
+                        b43_radio_write16(dev, 0x0076,
+                                          b43_radio_read16(dev, 0x0076)
+                                          & 0xFF7B);
+                } else {
+                        b43_set_txpower_g(dev, &old_bbatt,
+                                          &old_rfatt, old_tx_control);
+                }
+        }
+        b43_hardware_pctl_init_gphy(dev);
+        b43_shm_clear_tssi(dev);
+}
+
+static void b43_phy_initg(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u16 tmp;
+
+        if (phy->rev == 1)
+                b43_phy_initb5(dev);
+        else
+                b43_phy_initb6(dev);
+
+        if (phy->rev >= 2 || phy->gmode)
+                b43_phy_inita(dev);
+
+        if (phy->rev >= 2) {
+                b43_phy_write(dev, B43_PHY_ANALOGOVER, 0);
+                b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, 0);
+        }
+        if (phy->rev == 2) {
+                b43_phy_write(dev, B43_PHY_RFOVER, 0);
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
+        }
+        if (phy->rev > 5) {
+                b43_phy_write(dev, B43_PHY_RFOVER, 0x400);
+                b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
+        }
+        if (phy->gmode || phy->rev >= 2) {
+                tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
+                tmp &= B43_PHYVER_VERSION;
+                if (tmp == 3 || tmp == 5) {
+                        b43_phy_write(dev, B43_PHY_OFDM(0xC2), 0x1816);
+                        b43_phy_write(dev, B43_PHY_OFDM(0xC3), 0x8006);
+                }
+                if (tmp == 5) {
+                        b43_phy_write(dev, B43_PHY_OFDM(0xCC),
+                                      (b43_phy_read(dev, B43_PHY_OFDM(0xCC))
+                                       & 0x00FF) | 0x1F00);
+                }
+        }
+        if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
+                b43_phy_write(dev, B43_PHY_OFDM(0x7E), 0x78);
+        if (phy->radio_rev == 8) {
+                b43_phy_write(dev, B43_PHY_EXTG(0x01),
+                              b43_phy_read(dev, B43_PHY_EXTG(0x01))
+                              | 0x80);
+                b43_phy_write(dev, B43_PHY_OFDM(0x3E),
+                              b43_phy_read(dev, B43_PHY_OFDM(0x3E))
+                              | 0x4);
+        }
+        if (has_loopback_gain(phy))
+                b43_calc_loopback_gain(dev);
+
+        if (phy->radio_rev != 8) {
+                if (gphy->initval == 0xFFFF)
+                        gphy->initval = b43_radio_init2050(dev);
+                else
+                        b43_radio_write16(dev, 0x0078, gphy->initval);
+        }
+        b43_lo_g_init(dev);
+        if (has_tx_magnification(phy)) {
+                b43_radio_write16(dev, 0x52,
+                                  (b43_radio_read16(dev, 0x52) & 0xFF00)
+                                  | gphy->lo_control->tx_bias | gphy->
+                                  lo_control->tx_magn);
+        } else {
+                b43_radio_write16(dev, 0x52,
+                                  (b43_radio_read16(dev, 0x52) & 0xFFF0)
+                                  | gphy->lo_control->tx_bias);
+        }
+        if (phy->rev >= 6) {
+                b43_phy_write(dev, B43_PHY_CCK(0x36),
+                              (b43_phy_read(dev, B43_PHY_CCK(0x36))
+                               & 0x0FFF) | (gphy->lo_control->
+                                            tx_bias << 12));
+        }
+        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
+                b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
+        else
+                b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
+        if (phy->rev < 2)
+                b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
+        else
+                b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
+        if (phy->gmode || phy->rev >= 2) {
+                b43_lo_g_adjust(dev);
+                b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
+        }
+
+        if (!(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
+                /* The specs state to update the NRSSI LT with
+                 * the value 0x7FFFFFFF here. I think that is some weird
+                 * compiler optimization in the original driver.
+                 * Essentially, what we do here is resetting all NRSSI LT
+                 * entries to -32 (see the clamp_val() in nrssi_hw_update())
+                 */
+                b43_nrssi_hw_update(dev, 0xFFFF);       //FIXME?
+                b43_calc_nrssi_threshold(dev);
+        } else if (phy->gmode || phy->rev >= 2) {
+                if (gphy->nrssi[0] == -1000) {
+                        B43_WARN_ON(gphy->nrssi[1] != -1000);
+                        b43_calc_nrssi_slope(dev);
+                } else
+                        b43_calc_nrssi_threshold(dev);
+        }
+        if (phy->radio_rev == 8)
+                b43_phy_write(dev, B43_PHY_EXTG(0x05), 0x3230);
+        b43_phy_init_pctl(dev);
+        /* FIXME: The spec says in the following if, the 0 should be replaced
+           'if OFDM may not be used in the current locale'
+           but OFDM is legal everywhere */
+        if ((dev->dev->bus->chip_id == 0x4306
+             && dev->dev->bus->chip_package == 2) || 0) {
+                b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
+                              & 0xBFFF);
+                b43_phy_write(dev, B43_PHY_OFDM(0xC3),
+                              b43_phy_read(dev, B43_PHY_OFDM(0xC3))
+                              & 0x7FFF);
+        }
+}
+
+void b43_gphy_channel_switch(struct b43_wldev *dev,
+                             unsigned int channel,
+                             bool synthetic_pu_workaround)
+{
+        if (synthetic_pu_workaround)
+                b43_synth_pu_workaround(dev, channel);
+
+        b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
+
+        if (channel == 14) {
+                if (dev->dev->bus->sprom.country_code ==
+                    SSB_SPROM1CCODE_JAPAN)
+                        b43_hf_write(dev,
+                                     b43_hf_read(dev) & ~B43_HF_ACPR);
+                else
+                        b43_hf_write(dev,
+                                     b43_hf_read(dev) | B43_HF_ACPR);
+                b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+                            b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+                            | (1 << 11));
+        } else {
+                b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+                            b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+                            & 0xF7BF);
+        }
+}
+
+static void default_baseband_attenuation(struct b43_wldev *dev,
+                                         struct b43_bbatt *bb)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
+                bb->att = 0;
+        else
+                bb->att = 2;
+}
+
+static void default_radio_attenuation(struct b43_wldev *dev,
+                                      struct b43_rfatt *rf)
+{
+        struct ssb_bus *bus = dev->dev->bus;
+        struct b43_phy *phy = &dev->phy;
+
+        rf->with_padmix = 0;
+
+        if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM &&
+            bus->boardinfo.type == SSB_BOARD_BCM4309G) {
+                if (bus->boardinfo.rev < 0x43) {
+                        rf->att = 2;
+                        return;
+                } else if (bus->boardinfo.rev < 0x51) {
+                        rf->att = 3;
+                        return;
+                }
+        }
+
+        if (phy->type == B43_PHYTYPE_A) {
+                rf->att = 0x60;
+                return;
+        }
+
+        switch (phy->radio_ver) {
+        case 0x2053:
+                switch (phy->radio_rev) {
+                case 1:
+                        rf->att = 6;
+                        return;
+                }
+                break;
+        case 0x2050:
+                switch (phy->radio_rev) {
+                case 0:
+                        rf->att = 5;
+                        return;
+                case 1:
+                        if (phy->type == B43_PHYTYPE_G) {
+                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+                                    && bus->boardinfo.rev >= 30)
+                                        rf->att = 3;
+                                else if (bus->boardinfo.vendor ==
+                                         SSB_BOARDVENDOR_BCM
+                                         && bus->boardinfo.type ==
+                                         SSB_BOARD_BU4306)
+                                        rf->att = 3;
+                                else
+                                        rf->att = 1;
+                        } else {
+                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+                                    && bus->boardinfo.rev >= 30)
+                                        rf->att = 7;
+                                else
+                                        rf->att = 6;
+                        }
+                        return;
+                case 2:
+                        if (phy->type == B43_PHYTYPE_G) {
+                                if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+                                    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+                                    && bus->boardinfo.rev >= 30)
+                                        rf->att = 3;
+                                else if (bus->boardinfo.vendor ==
+                                         SSB_BOARDVENDOR_BCM
+                                         && bus->boardinfo.type ==
+                                         SSB_BOARD_BU4306)
+                                        rf->att = 5;
+                                else if (bus->chip_id == 0x4320)
+                                        rf->att = 4;
+                                else
+                                        rf->att = 3;
+                        } else
+                                rf->att = 6;
+                        return;
+                case 3:
+                        rf->att = 5;
+                        return;
+                case 4:
+                case 5:
+                        rf->att = 1;
+                        return;
+                case 6:
+                case 7:
+                        rf->att = 5;
+                        return;
+                case 8:
+                        rf->att = 0xA;
+                        rf->with_padmix = 1;
+                        return;
+                case 9:
+                default:
+                        rf->att = 5;
+                        return;
+                }
+        }
+        rf->att = 5;
+}
+
+static u16 default_tx_control(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (phy->radio_ver != 0x2050)
+                return 0;
+        if (phy->radio_rev == 1)
+                return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
+        if (phy->radio_rev < 6)
+                return B43_TXCTL_PA2DB;
+        if (phy->radio_rev == 8)
+                return B43_TXCTL_TXMIX;
+        return 0;
+}
+
+static u8 b43_gphy_aci_detect(struct b43_wldev *dev, u8 channel)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        u8 ret = 0;
+        u16 saved, rssi, temp;
+        int i, j = 0;
+
+        saved = b43_phy_read(dev, 0x0403);
+        b43_switch_channel(dev, channel);
+        b43_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
+        if (gphy->aci_hw_rssi)
+                rssi = b43_phy_read(dev, 0x048A) & 0x3F;
+        else
+                rssi = saved & 0x3F;
+        /* clamp temp to signed 5bit */
+        if (rssi > 32)
+                rssi -= 64;
+        for (i = 0; i < 100; i++) {
+                temp = (b43_phy_read(dev, 0x047F) >> 8) & 0x3F;
+                if (temp > 32)
+                        temp -= 64;
+                if (temp < rssi)
+                        j++;
+                if (j >= 20)
+                        ret = 1;
+        }
+        b43_phy_write(dev, 0x0403, saved);
+
+        return ret;
+}
+
+static u8 b43_gphy_aci_scan(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        u8 ret[13];
+        unsigned int channel = phy->channel;
+        unsigned int i, j, start, end;
+
+        if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
+                return 0;
+
+        b43_phy_lock(dev);
+        b43_radio_lock(dev);
+        b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
+        b43_phy_write(dev, B43_PHY_G_CRS,
+                      b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
+        b43_set_all_gains(dev, 3, 8, 1);
+
+        start = (channel - 5 > 0) ? channel - 5 : 1;
+        end = (channel + 5 < 14) ? channel + 5 : 13;
+
+        for (i = start; i <= end; i++) {
+                if (abs(channel - i) > 2)
+                        ret[i - 1] = b43_gphy_aci_detect(dev, i);
+        }
+        b43_switch_channel(dev, channel);
+        b43_phy_write(dev, 0x0802,
+                      (b43_phy_read(dev, 0x0802) & 0xFFFC) | 0x0003);
+        b43_phy_write(dev, 0x0403, b43_phy_read(dev, 0x0403) & 0xFFF8);
+        b43_phy_write(dev, B43_PHY_G_CRS,
+                      b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
+        b43_set_original_gains(dev);
+        for (i = 0; i < 13; i++) {
+                if (!ret[i])
+                        continue;
+                end = (i + 5 < 13) ? i + 5 : 13;
+                for (j = i; j < end; j++)
+                        ret[j] = 1;
+        }
+        b43_radio_unlock(dev);
+        b43_phy_unlock(dev);
+
+        return ret[channel - 1];
+}
+
+static s32 b43_tssi2dbm_ad(s32 num, s32 den)
+{
+        if (num < 0)
+                return num / den;
+        else
+                return (num + den / 2) / den;
+}
+
+static s8 b43_tssi2dbm_entry(s8 entry[], u8 index,
+                             s16 pab0, s16 pab1, s16 pab2)
+{
+        s32 m1, m2, f = 256, q, delta;
+        s8 i = 0;
+
+        m1 = b43_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
+        m2 = max(b43_tssi2dbm_ad(32768 + index * pab2, 256), 1);
+        do {
+                if (i > 15)
+                        return -EINVAL;
+                q = b43_tssi2dbm_ad(f * 4096 -
+                                    b43_tssi2dbm_ad(m2 * f, 16) * f, 2048);
+                delta = abs(q - f);
+                f = q;
+                i++;
+        } while (delta >= 2);
+        entry[index] = clamp_val(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
+        return 0;
+}
+
+u8 * b43_generate_dyn_tssi2dbm_tab(struct b43_wldev *dev,
+                                   s16 pab0, s16 pab1, s16 pab2)
+{
+        unsigned int i;
+        u8 *tab;
+        int err;
+
+        tab = kmalloc(64, GFP_KERNEL);
+        if (!tab) {
+                b43err(dev->wl, "Could not allocate memory "
+                       "for tssi2dbm table\n");
+                return NULL;
+        }
+        for (i = 0; i < 64; i++) {
+                err = b43_tssi2dbm_entry(tab, i, pab0, pab1, pab2);
+                if (err) {
+                        b43err(dev->wl, "Could not generate "
+                               "tssi2dBm table\n");
+                        kfree(tab);
+                        return NULL;
+                }
+        }
+
+        return tab;
+}
+
+/* Initialise the TSSI->dBm lookup table */
+static int b43_gphy_init_tssi2dbm_table(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        s16 pab0, pab1, pab2;
+
+        pab0 = (s16) (dev->dev->bus->sprom.pa0b0);
+        pab1 = (s16) (dev->dev->bus->sprom.pa0b1);
+        pab2 = (s16) (dev->dev->bus->sprom.pa0b2);
+
+        B43_WARN_ON((dev->dev->bus->chip_id == 0x4301) &&
+                    (phy->radio_ver != 0x2050)); /* Not supported anymore */
+
+        gphy->dyn_tssi_tbl = 0;
+
+        if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
+            pab0 != -1 && pab1 != -1 && pab2 != -1) {
+                /* The pabX values are set in SPROM. Use them. */
+                if ((s8) dev->dev->bus->sprom.itssi_bg != 0 &&
+                    (s8) dev->dev->bus->sprom.itssi_bg != -1) {
+                        gphy->tgt_idle_tssi =
+                                (s8) (dev->dev->bus->sprom.itssi_bg);
+                } else
+                        gphy->tgt_idle_tssi = 62;
+                gphy->tssi2dbm = b43_generate_dyn_tssi2dbm_tab(dev, pab0,
+                                                               pab1, pab2);
+                if (!gphy->tssi2dbm)
+                        return -ENOMEM;
+                gphy->dyn_tssi_tbl = 1;
+        } else {
+                /* pabX values not set in SPROM. */
+                gphy->tgt_idle_tssi = 52;
+                gphy->tssi2dbm = b43_tssi2dbm_g_table;
+        }
+
+        return 0;
+}
+
+static int b43_gphy_op_allocate(struct b43_wldev *dev)
+{
+        struct b43_phy_g *gphy;
+        struct b43_txpower_lo_control *lo;
+        int err, i;
+
+        gphy = kzalloc(sizeof(*gphy), GFP_KERNEL);
+        if (!gphy) {
+                err = -ENOMEM;
+                goto error;
+        }
+        dev->phy.g = gphy;
+
+        memset(gphy->minlowsig, 0xFF, sizeof(gphy->minlowsig));
+
+        /* NRSSI */
+        for (i = 0; i < ARRAY_SIZE(gphy->nrssi); i++)
+                gphy->nrssi[i] = -1000;
+        for (i = 0; i < ARRAY_SIZE(gphy->nrssi_lt); i++)
+                gphy->nrssi_lt[i] = i;
+
+        gphy->lofcal = 0xFFFF;
+        gphy->initval = 0xFFFF;
+
+        gphy->interfmode = B43_INTERFMODE_NONE;
+
+        /* OFDM-table address caching. */
+        gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
+
+        gphy->average_tssi = 0xFF;
+
+        lo = kzalloc(sizeof(*lo), GFP_KERNEL);
+        if (!lo) {
+                err = -ENOMEM;
+                goto err_free_gphy;
+        }
+        gphy->lo_control = lo;
+
+        lo->tx_bias = 0xFF;
+        INIT_LIST_HEAD(&lo->calib_list);
+
+        err = b43_gphy_init_tssi2dbm_table(dev);
+        if (err)
+                goto err_free_lo;
+
+        return 0;
+
+err_free_lo:
+        kfree(lo);
+err_free_gphy:
+        kfree(gphy);
+error:
+        return err;
+}
+
+static int b43_gphy_op_prepare(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        struct b43_txpower_lo_control *lo = gphy->lo_control;
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+
+        default_baseband_attenuation(dev, &gphy->bbatt);
+        default_radio_attenuation(dev, &gphy->rfatt);
+        gphy->tx_control = (default_tx_control(dev) << 4);
+        generate_rfatt_list(dev, &lo->rfatt_list);
+        generate_bbatt_list(dev, &lo->bbatt_list);
+
+        /* Commit previous writes */
+        b43_read32(dev, B43_MMIO_MACCTL);
+
+        if (phy->rev == 1) {
+                /* Workaround: Temporarly disable gmode through the early init
+                 * phase, as the gmode stuff is not needed for phy rev 1 */
+                phy->gmode = 0;
+                b43_wireless_core_reset(dev, 0);
+                b43_phy_initg(dev);
+                phy->gmode = 1;
+                b43_wireless_core_reset(dev, B43_TMSLOW_GMODE);
+        }
+
+        return 0;
+}
+
+static int b43_gphy_op_init(struct b43_wldev *dev)
+{
+        struct b43_phy_g *gphy = dev->phy.g;
+
+        b43_phy_initg(dev);
+        gphy->initialised = 1;
+
+        return 0;
+}
+
+static void b43_gphy_op_exit(struct b43_wldev *dev)
+{
+        struct b43_phy_g *gphy = dev->phy.g;
+
+        if (gphy->initialised) {
+                //TODO
+                gphy->initialised = 0;
+        }
+        b43_lo_g_cleanup(dev);
+        kfree(gphy->lo_control);
+        if (gphy->dyn_tssi_tbl)
+                kfree(gphy->tssi2dbm);
+        kfree(gphy);
+        dev->phy.g = NULL;
+}
+
+static u16 b43_gphy_op_read(struct b43_wldev *dev, u16 reg)
+{
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_PHY_DATA);
+}
+
+static void b43_gphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_PHY_DATA, value);
+}
+
+static u16 b43_gphy_op_radio_read(struct b43_wldev *dev, u16 reg)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+        /* G-PHY needs 0x80 for read access. */
+        reg |= 0x80;
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
+}
+
+static void b43_gphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
+{
+        /* Register 1 is a 32-bit register. */
+        B43_WARN_ON(reg == 1);
+
+        b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+        b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
+}
+
+static bool b43_gphy_op_supports_hwpctl(struct b43_wldev *dev)
+{
+        return (dev->phy.rev >= 6);
+}
+
+static void b43_gphy_op_software_rfkill(struct b43_wldev *dev,
+                                        enum rfkill_state state)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        unsigned int channel;
+
+        might_sleep();
+
+        if (state == RFKILL_STATE_UNBLOCKED) {
+                /* Turn radio ON */
+                if (phy->radio_on)
+                        return;
+
+                b43_phy_write(dev, 0x0015, 0x8000);
+                b43_phy_write(dev, 0x0015, 0xCC00);
+                b43_phy_write(dev, 0x0015, (phy->gmode ? 0x00C0 : 0x0000));
+                if (gphy->radio_off_context.valid) {
+                        /* Restore the RFover values. */
+                        b43_phy_write(dev, B43_PHY_RFOVER,
+                                      gphy->radio_off_context.rfover);
+                        b43_phy_write(dev, B43_PHY_RFOVERVAL,
+                                      gphy->radio_off_context.rfoverval);
+                        gphy->radio_off_context.valid = 0;
+                }
+                channel = phy->channel;
+                b43_gphy_channel_switch(dev, 6, 1);
+                b43_gphy_channel_switch(dev, channel, 0);
+        } else {
+                /* Turn radio OFF */
+                u16 rfover, rfoverval;
+
+                rfover = b43_phy_read(dev, B43_PHY_RFOVER);
+                rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+                gphy->radio_off_context.rfover = rfover;
+                gphy->radio_off_context.rfoverval = rfoverval;
+                gphy->radio_off_context.valid = 1;
+                b43_phy_write(dev, B43_PHY_RFOVER, rfover | 0x008C);
+                b43_phy_write(dev, B43_PHY_RFOVERVAL, rfoverval & 0xFF73);
+        }
+}
+
+static int b43_gphy_op_switch_channel(struct b43_wldev *dev,
+                                      unsigned int new_channel)
+{
+        if ((new_channel < 1) || (new_channel > 14))
+                return -EINVAL;
+        b43_gphy_channel_switch(dev, new_channel, 0);
+
+        return 0;
+}
+
+static unsigned int b43_gphy_op_get_default_chan(struct b43_wldev *dev)
+{
+        return 1; /* Default to channel 1 */
+}
+
+static void b43_gphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
+{
+        struct b43_phy *phy = &dev->phy;
+        u64 hf;
+        u16 tmp;
+        int autodiv = 0;
+
+        if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
+                autodiv = 1;
+
+        hf = b43_hf_read(dev);
+        hf &= ~B43_HF_ANTDIVHELP;
+        b43_hf_write(dev, hf);
+
+        tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
+        tmp &= ~B43_PHY_BBANDCFG_RXANT;
+        tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
+                        << B43_PHY_BBANDCFG_RXANT_SHIFT;
+        b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
+
+        if (autodiv) {
+                tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
+                if (antenna == B43_ANTENNA_AUTO0)
+                        tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
+                else
+                        tmp |= B43_PHY_ANTDWELL_AUTODIV1;
+                b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
+        }
+        tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
+        if (autodiv)
+                tmp |= B43_PHY_ANTWRSETT_ARXDIV;
+        else
+                tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
+        b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
+        if (phy->rev >= 2) {
+                tmp = b43_phy_read(dev, B43_PHY_OFDM61);
+                tmp |= B43_PHY_OFDM61_10;
+                b43_phy_write(dev, B43_PHY_OFDM61, tmp);
+
+                tmp =
+                    b43_phy_read(dev, B43_PHY_DIVSRCHGAINBACK);
+                tmp = (tmp & 0xFF00) | 0x15;
+                b43_phy_write(dev, B43_PHY_DIVSRCHGAINBACK,
+                              tmp);
+
+                if (phy->rev == 2) {
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
+                                      8);
+                } else {
+                        tmp =
+                            b43_phy_read(dev,
+                                         B43_PHY_ADIVRELATED);
+                        tmp = (tmp & 0xFF00) | 8;
+                        b43_phy_write(dev, B43_PHY_ADIVRELATED,
+                                      tmp);
+                }
+        }
+        if (phy->rev >= 6)
+                b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
+
+        hf |= B43_HF_ANTDIVHELP;
+        b43_hf_write(dev, hf);
+}
+
+static int b43_gphy_op_interf_mitigation(struct b43_wldev *dev,
+                                         enum b43_interference_mitigation mode)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        int currentmode;
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+        if ((phy->rev == 0) || (!phy->gmode))
+                return -ENODEV;
+
+        gphy->aci_wlan_automatic = 0;
+        switch (mode) {
+        case B43_INTERFMODE_AUTOWLAN:
+                gphy->aci_wlan_automatic = 1;
+                if (gphy->aci_enable)
+                        mode = B43_INTERFMODE_MANUALWLAN;
+                else
+                        mode = B43_INTERFMODE_NONE;
+                break;
+        case B43_INTERFMODE_NONE:
+        case B43_INTERFMODE_NONWLAN:
+        case B43_INTERFMODE_MANUALWLAN:
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        currentmode = gphy->interfmode;
+        if (currentmode == mode)
+                return 0;
+        if (currentmode != B43_INTERFMODE_NONE)
+                b43_radio_interference_mitigation_disable(dev, currentmode);
+
+        if (mode == B43_INTERFMODE_NONE) {
+                gphy->aci_enable = 0;
+                gphy->aci_hw_rssi = 0;
+        } else
+                b43_radio_interference_mitigation_enable(dev, mode);
+        gphy->interfmode = mode;
+
+        return 0;
+}
+
+/* http://bcm-specs.sipsolutions.net/EstimatePowerOut
+ * This function converts a TSSI value to dBm in Q5.2
+ */
+static s8 b43_gphy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
+{
+        struct b43_phy_g *gphy = dev->phy.g;
+        s8 dbm;
+        s32 tmp;
+
+        tmp = (gphy->tgt_idle_tssi - gphy->cur_idle_tssi + tssi);
+        tmp = clamp_val(tmp, 0x00, 0x3F);
+        dbm = gphy->tssi2dbm[tmp];
+
+        return dbm;
+}
+
+static void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
+                                            int *_bbatt, int *_rfatt)
+{
+        int rfatt = *_rfatt;
+        int bbatt = *_bbatt;
+        struct b43_txpower_lo_control *lo = dev->phy.g->lo_control;
+
+        /* Get baseband and radio attenuation values into their permitted ranges.
+         * Radio attenuation affects power level 4 times as much as baseband. */
+
+        /* Range constants */
+        const int rf_min = lo->rfatt_list.min_val;
+        const int rf_max = lo->rfatt_list.max_val;
+        const int bb_min = lo->bbatt_list.min_val;
+        const int bb_max = lo->bbatt_list.max_val;
+
+        while (1) {
+                if (rfatt > rf_max && bbatt > bb_max - 4)
+                        break;  /* Can not get it into ranges */
+                if (rfatt < rf_min && bbatt < bb_min + 4)
+                        break;  /* Can not get it into ranges */
+                if (bbatt > bb_max && rfatt > rf_max - 1)
+                        break;  /* Can not get it into ranges */
+                if (bbatt < bb_min && rfatt < rf_min + 1)
+                        break;  /* Can not get it into ranges */
+
+                if (bbatt > bb_max) {
+                        bbatt -= 4;
+                        rfatt += 1;
+                        continue;
+                }
+                if (bbatt < bb_min) {
+                        bbatt += 4;
+                        rfatt -= 1;
+                        continue;
+                }
+                if (rfatt > rf_max) {
+                        rfatt -= 1;
+                        bbatt += 4;
+                        continue;
+                }
+                if (rfatt < rf_min) {
+                        rfatt += 1;
+                        bbatt -= 4;
+                        continue;
+                }
+                break;
+        }
+
+        *_rfatt = clamp_val(rfatt, rf_min, rf_max);
+        *_bbatt = clamp_val(bbatt, bb_min, bb_max);
+}
+
+static void b43_gphy_op_adjust_txpower(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        int rfatt, bbatt;
+        u8 tx_control;
+
+        spin_lock_irq(&dev->wl->irq_lock);
+
+        /* Calculate the new attenuation values. */
+        bbatt = gphy->bbatt.att;
+        bbatt += gphy->bbatt_delta;
+        rfatt = gphy->rfatt.att;
+        rfatt += gphy->rfatt_delta;
+
+        b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
+        tx_control = gphy->tx_control;
+        if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
+                if (rfatt <= 1) {
+                        if (tx_control == 0) {
+                                tx_control =
+                                    B43_TXCTL_PA2DB |
+                                    B43_TXCTL_TXMIX;
+                                rfatt += 2;
+                                bbatt += 2;
+                        } else if (dev->dev->bus->sprom.
+                                   boardflags_lo &
+                                   B43_BFL_PACTRL) {
+                                bbatt += 4 * (rfatt - 2);
+                                rfatt = 2;
+                        }
+                } else if (rfatt > 4 && tx_control) {
+                        tx_control = 0;
+                        if (bbatt < 3) {
+                                rfatt -= 3;
+                                bbatt += 2;
+                        } else {
+                                rfatt -= 2;
+                                bbatt -= 2;
+                        }
+                }
+        }
+        /* Save the control values */
+        gphy->tx_control = tx_control;
+        b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
+        gphy->rfatt.att = rfatt;
+        gphy->bbatt.att = bbatt;
+
+        /* We drop the lock early, so we can sleep during hardware
+         * adjustment. Possible races with op_recalc_txpower are harmless,
+         * as we will be called once again in case we raced. */
+        spin_unlock_irq(&dev->wl->irq_lock);
+
+        if (b43_debug(dev, B43_DBG_XMITPOWER))
+                b43dbg(dev->wl, "Adjusting TX power\n");
+
+        /* Adjust the hardware */
+        b43_phy_lock(dev);
+        b43_radio_lock(dev);
+        b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt,
+                          gphy->tx_control);
+        b43_radio_unlock(dev);
+        b43_phy_unlock(dev);
+}
+
+static enum b43_txpwr_result b43_gphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        unsigned int average_tssi;
+        int cck_result, ofdm_result;
+        int estimated_pwr, desired_pwr, pwr_adjust;
+        int rfatt_delta, bbatt_delta;
+        unsigned int max_pwr;
+
+        /* First get the average TSSI */
+        cck_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_CCK);
+        ofdm_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_OFDM_G);
+        if ((cck_result < 0) && (ofdm_result < 0)) {
+                /* No TSSI information available */
+                if (!ignore_tssi)
+                        goto no_adjustment_needed;
+                cck_result = 0;
+                ofdm_result = 0;
+        }
+        if (cck_result < 0)
+                average_tssi = ofdm_result;
+        else if (ofdm_result < 0)
+                average_tssi = cck_result;
+        else
+                average_tssi = (cck_result + ofdm_result) / 2;
+        /* Merge the average with the stored value. */
+        if (likely(gphy->average_tssi != 0xFF))
+                average_tssi = (average_tssi + gphy->average_tssi) / 2;
+        gphy->average_tssi = average_tssi;
+        B43_WARN_ON(average_tssi >= B43_TSSI_MAX);
+
+        /* Estimate the TX power emission based on the TSSI */
+        estimated_pwr = b43_gphy_estimate_power_out(dev, average_tssi);
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+        max_pwr = dev->dev->bus->sprom.maxpwr_bg;
+        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
+                max_pwr -= 3; /* minus 0.75 */
+        if (unlikely(max_pwr >= INT_TO_Q52(30/*dBm*/))) {
+                b43warn(dev->wl,
+                        "Invalid max-TX-power value in SPROM.\n");
+                max_pwr = INT_TO_Q52(20); /* fake it */
+                dev->dev->bus->sprom.maxpwr_bg = max_pwr;
+        }
+
+        /* Get desired power (in Q5.2) */
+        if (phy->desired_txpower < 0)
+                desired_pwr = INT_TO_Q52(0);
+        else
+                desired_pwr = INT_TO_Q52(phy->desired_txpower);
+        /* And limit it. max_pwr already is Q5.2 */
+        desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
+        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+                b43dbg(dev->wl,
+                       "[TX power]  current = " Q52_FMT
+                       " dBm,  desired = " Q52_FMT
+                       " dBm,  max = " Q52_FMT "\n",
+                       Q52_ARG(estimated_pwr),
+                       Q52_ARG(desired_pwr),
+                       Q52_ARG(max_pwr));
+        }
+
+        /* Calculate the adjustment delta. */
+        pwr_adjust = desired_pwr - estimated_pwr;
+        if (pwr_adjust == 0)
+                goto no_adjustment_needed;
+
+        /* RF attenuation delta. */
+        rfatt_delta = ((pwr_adjust + 7) / 8);
+        /* Lower attenuation => Bigger power output. Negate it. */
+        rfatt_delta = -rfatt_delta;
+
+        /* Baseband attenuation delta. */
+        bbatt_delta = pwr_adjust / 2;
+        /* Lower attenuation => Bigger power output. Negate it. */
+        bbatt_delta = -bbatt_delta;
+        /* RF att affects power level 4 times as much as
+         * Baseband attennuation. Subtract it. */
+        bbatt_delta -= 4 * rfatt_delta;
+
+        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+                int dbm = pwr_adjust < 0 ? -pwr_adjust : pwr_adjust;
+                b43dbg(dev->wl,
+                       "[TX power deltas]  %s" Q52_FMT " dBm   =>   "
+                       "bbatt-delta = %d,  rfatt-delta = %d\n",
+                       (pwr_adjust < 0 ? "-" : ""), Q52_ARG(dbm),
+                       bbatt_delta, rfatt_delta);
+        }
+        /* So do we finally need to adjust something in hardware? */
+        if ((rfatt_delta == 0) && (bbatt_delta == 0))
+                goto no_adjustment_needed;
+
+        /* Save the deltas for later when we adjust the power. */
+        gphy->bbatt_delta = bbatt_delta;
+        gphy->rfatt_delta = rfatt_delta;
+
+        /* We need to adjust the TX power on the device. */
+        return B43_TXPWR_RES_NEED_ADJUST;
+
+no_adjustment_needed:
+        return B43_TXPWR_RES_DONE;
+}
+
+static void b43_gphy_op_pwork_15sec(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+
+        //TODO: update_aci_moving_average
+        if (gphy->aci_enable && gphy->aci_wlan_automatic) {
+                b43_mac_suspend(dev);
+                if (!gphy->aci_enable && 1 /*TODO: not scanning? */ ) {
+                        if (0 /*TODO: bunch of conditions */ ) {
+                                phy->ops->interf_mitigation(dev,
+                                        B43_INTERFMODE_MANUALWLAN);
+                        }
+                } else if (0 /*TODO*/) {
+                           if (/*(aci_average > 1000) &&*/ !b43_gphy_aci_scan(dev))
+                                phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
+                }
+                b43_mac_enable(dev);
+        } else if (gphy->interfmode == B43_INTERFMODE_NONWLAN &&
+                   phy->rev == 1) {
+                //TODO: implement rev1 workaround
+        }
+        b43_lo_g_maintanance_work(dev);
+}
+
+static void b43_gphy_op_pwork_60sec(struct b43_wldev *dev)
+{
+        struct b43_phy *phy = &dev->phy;
+
+        if (!(dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI))
+                return;
+
+        b43_mac_suspend(dev);
+        b43_calc_nrssi_slope(dev);
+        if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
+                u8 old_chan = phy->channel;
+
+                /* VCO Calibration */
+                if (old_chan >= 8)
+                        b43_switch_channel(dev, 1);
+                else
+                        b43_switch_channel(dev, 13);
+                b43_switch_channel(dev, old_chan);
+        }
+        b43_mac_enable(dev);
+}
+
+const struct b43_phy_operations b43_phyops_g = {
+        .allocate               = b43_gphy_op_allocate,
+        .prepare                = b43_gphy_op_prepare,
+        .init                   = b43_gphy_op_init,
+        .exit                   = b43_gphy_op_exit,
+        .phy_read               = b43_gphy_op_read,
+        .phy_write              = b43_gphy_op_write,
+        .radio_read             = b43_gphy_op_radio_read,
+        .radio_write            = b43_gphy_op_radio_write,
+        .supports_hwpctl        = b43_gphy_op_supports_hwpctl,
+        .software_rfkill        = b43_gphy_op_software_rfkill,
+        .switch_channel         = b43_gphy_op_switch_channel,
+        .get_default_chan       = b43_gphy_op_get_default_chan,
+        .set_rx_antenna         = b43_gphy_op_set_rx_antenna,
+        .interf_mitigation      = b43_gphy_op_interf_mitigation,
+        .recalc_txpower         = b43_gphy_op_recalc_txpower,
+        .adjust_txpower         = b43_gphy_op_adjust_txpower,
+        .pwork_15sec            = b43_gphy_op_pwork_15sec,
+        .pwork_60sec            = b43_gphy_op_pwork_60sec,
+};
diff --git a/drivers/net/wireless/b43/phy_g.h b/drivers/net/wireless/b43/phy_g.h
new file mode 100644
index 000000000000..7f95edea1c63
--- /dev/null
+++ b/drivers/net/wireless/b43/phy_g.h
@@ -0,0 +1,209 @@
+#ifndef LINUX_B43_PHY_G_H_
+#define LINUX_B43_PHY_G_H_
+
+/* OFDM PHY registers are defined in the A-PHY header. */
+#include "phy_a.h"
+
+/* CCK (B) PHY Registers */
+#define B43_PHY_VERSION_CCK             B43_PHY_CCK(0x00)       /* Versioning register for B-PHY */
+#define B43_PHY_CCKBBANDCFG             B43_PHY_CCK(0x01)       /* Contains antenna 0/1 control bit */
+#define B43_PHY_PGACTL                  B43_PHY_CCK(0x15)       /* PGA control */
+#define  B43_PHY_PGACTL_LPF             0x1000  /* Low pass filter (?) */
+#define  B43_PHY_PGACTL_LOWBANDW        0x0040  /* Low bandwidth flag */
+#define  B43_PHY_PGACTL_UNKNOWN         0xEFA0
+#define B43_PHY_FBCTL1                  B43_PHY_CCK(0x18)       /* Frequency bandwidth control 1 */
+#define B43_PHY_ITSSI                   B43_PHY_CCK(0x29)       /* Idle TSSI */
+#define B43_PHY_LO_LEAKAGE              B43_PHY_CCK(0x2D)       /* Measured LO leakage */
+#define B43_PHY_ENERGY                  B43_PHY_CCK(0x33)       /* Energy */
+#define B43_PHY_SYNCCTL                 B43_PHY_CCK(0x35)
+#define B43_PHY_FBCTL2                  B43_PHY_CCK(0x38)       /* Frequency bandwidth control 2 */
+#define B43_PHY_DACCTL                  B43_PHY_CCK(0x60)       /* DAC control */
+#define B43_PHY_RCCALOVER               B43_PHY_CCK(0x78)       /* RC calibration override */
+
+/* Extended G-PHY Registers */
+#define B43_PHY_CLASSCTL                B43_PHY_EXTG(0x02)      /* Classify control */
+#define B43_PHY_GTABCTL                 B43_PHY_EXTG(0x03)      /* G-PHY table control (see below) */
+#define  B43_PHY_GTABOFF                0x03FF  /* G-PHY table offset (see below) */
+#define  B43_PHY_GTABNR                 0xFC00  /* G-PHY table number (see below) */
+#define  B43_PHY_GTABNR_SHIFT           10
+#define B43_PHY_GTABDATA                B43_PHY_EXTG(0x04)      /* G-PHY table data */
+#define B43_PHY_LO_MASK                 B43_PHY_EXTG(0x0F)      /* Local Oscillator control mask */
+#define B43_PHY_LO_CTL                  B43_PHY_EXTG(0x10)      /* Local Oscillator control */
+#define B43_PHY_RFOVER                  B43_PHY_EXTG(0x11)      /* RF override */
+#define B43_PHY_RFOVERVAL               B43_PHY_EXTG(0x12)      /* RF override value */
+#define  B43_PHY_RFOVERVAL_EXTLNA       0x8000
+#define  B43_PHY_RFOVERVAL_LNA          0x7000
+#define  B43_PHY_RFOVERVAL_LNA_SHIFT    12
+#define  B43_PHY_RFOVERVAL_PGA          0x0F00
+#define  B43_PHY_RFOVERVAL_PGA_SHIFT    8
+#define  B43_PHY_RFOVERVAL_UNK          0x0010  /* Unknown, always set. */
+#define  B43_PHY_RFOVERVAL_TRSWRX       0x00E0
+#define  B43_PHY_RFOVERVAL_BW           0x0003  /* Bandwidth flags */
+#define   B43_PHY_RFOVERVAL_BW_LPF      0x0001  /* Low Pass Filter */
+#define   B43_PHY_RFOVERVAL_BW_LBW      0x0002  /* Low Bandwidth (when set), high when unset */
+#define B43_PHY_ANALOGOVER              B43_PHY_EXTG(0x14)      /* Analog override */
+#define B43_PHY_ANALOGOVERVAL           B43_PHY_EXTG(0x15)      /* Analog override value */
+
+
+/*** G-PHY table numbers */
+#define B43_GTAB(number, offset)        (((number) << B43_PHY_GTABNR_SHIFT) | (offset))
+#define B43_GTAB_NRSSI                  B43_GTAB(0x00, 0)
+#define B43_GTAB_TRFEMW                 B43_GTAB(0x0C, 0x120)
+#define B43_GTAB_ORIGTR                 B43_GTAB(0x2E, 0x298)
+
+u16 b43_gtab_read(struct b43_wldev *dev, u16 table, u16 offset);
+void b43_gtab_write(struct b43_wldev *dev, u16 table, u16 offset, u16 value);
+
+
+/* Returns the boolean whether "TX Magnification" is enabled. */
+#define has_tx_magnification(phy) \
+        (((phy)->rev >= 2) &&                   \
+         ((phy)->radio_ver == 0x2050) &&        \
+         ((phy)->radio_rev == 8))
+/* Card uses the loopback gain stuff */
+#define has_loopback_gain(phy) \
+        (((phy)->rev > 1) || ((phy)->gmode))
+
+/* Radio Attenuation (RF Attenuation) */
+struct b43_rfatt {
+        u8 att;                 /* Attenuation value */
+        bool with_padmix;       /* Flag, PAD Mixer enabled. */
+};
+struct b43_rfatt_list {
+        /* Attenuation values list */
+        const struct b43_rfatt *list;
+        u8 len;
+        /* Minimum/Maximum attenuation values */
+        u8 min_val;
+        u8 max_val;
+};
+
+/* Returns true, if the values are the same. */
+static inline bool b43_compare_rfatt(const struct b43_rfatt *a,
+                                     const struct b43_rfatt *b)
+{
+        return ((a->att == b->att) &&
+                (a->with_padmix == b->with_padmix));
+}
+
+/* Baseband Attenuation */
+struct b43_bbatt {
+        u8 att;                 /* Attenuation value */
+};
+struct b43_bbatt_list {
+        /* Attenuation values list */
+        const struct b43_bbatt *list;
+        u8 len;
+        /* Minimum/Maximum attenuation values */
+        u8 min_val;
+        u8 max_val;
+};
+
+/* Returns true, if the values are the same. */
+static inline bool b43_compare_bbatt(const struct b43_bbatt *a,
+                                     const struct b43_bbatt *b)
+{
+        return (a->att == b->att);
+}
+
+/* tx_control bits. */
+#define B43_TXCTL_PA3DB         0x40    /* PA Gain 3dB */
+#define B43_TXCTL_PA2DB         0x20    /* PA Gain 2dB */
+#define B43_TXCTL_TXMIX         0x10    /* TX Mixer Gain */
+
+struct b43_txpower_lo_control;
+
+struct b43_phy_g {
+        bool initialised;
+
+        /* ACI (adjacent channel interference) flags. */
+        bool aci_enable;
+        bool aci_wlan_automatic;
+        bool aci_hw_rssi;
+
+        /* Radio switched on/off */
+        bool radio_on;
+        struct {
+                /* Values saved when turning the radio off.
+                 * They are needed when turning it on again. */
+                bool valid;
+                u16 rfover;
+                u16 rfoverval;
+        } radio_off_context;
+
+        u16 minlowsig[2];
+        u16 minlowsigpos[2];
+
+        /* Pointer to the table used to convert a
+         * TSSI value to dBm-Q5.2 */
+        const s8 *tssi2dbm;
+        /* tssi2dbm is kmalloc()ed. Only used for free()ing. */
+        bool dyn_tssi_tbl;
+        /* Target idle TSSI */
+        int tgt_idle_tssi;
+        /* Current idle TSSI */
+        int cur_idle_tssi;
+        /* The current average TSSI.
+         * Needs irq_lock, as it's updated in the IRQ path. */
+        u8 average_tssi;
+        /* Current TX power level attenuation control values */
+        struct b43_bbatt bbatt;
+        struct b43_rfatt rfatt;
+        u8 tx_control;          /* B43_TXCTL_XXX */
+        /* The calculated attenuation deltas that are used later
+         * when adjusting the actual power output. */
+        int bbatt_delta;
+        int rfatt_delta;
+
+        /* LocalOscillator control values. */
+        struct b43_txpower_lo_control *lo_control;
+        /* Values from b43_calc_loopback_gain() */
+        s16 max_lb_gain;        /* Maximum Loopback gain in hdB */
+        s16 trsw_rx_gain;       /* TRSW RX gain in hdB */
+        s16 lna_lod_gain;       /* LNA lod */
+        s16 lna_gain;           /* LNA */
+        s16 pga_gain;           /* PGA */
+
+        /* Current Interference Mitigation mode */
+        int interfmode;
+        /* 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 B43_INTERFSTACK_SIZE    26
+        u32 interfstack[B43_INTERFSTACK_SIZE];  //FIXME: use a data structure
+
+        /* Saved values from the NRSSI Slope calculation */
+        s16 nrssi[2];
+        s32 nrssislope;
+        /* In memory nrssi lookup table. */
+        s8 nrssi_lt[64];
+
+        u16 lofcal;
+
+        u16 initval;            //FIXME rename?
+
+        /* The device does address auto increment for the OFDM tables.
+         * We cache the previously used address here and omit the address
+         * write on the next table access, if possible. */
+        u16 ofdmtab_addr; /* The address currently set in hardware. */
+        enum { /* The last data flow direction. */
+                B43_OFDMTAB_DIRECTION_UNKNOWN = 0,
+                B43_OFDMTAB_DIRECTION_READ,
+                B43_OFDMTAB_DIRECTION_WRITE,
+        } ofdmtab_addr_direction;
+};
+
+void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev,
+                                       u16 baseband_attenuation);
+void b43_gphy_channel_switch(struct b43_wldev *dev,
+                             unsigned int channel,
+                             bool synthetic_pu_workaround);
+
+struct b43_phy_operations;
+extern const struct b43_phy_operations b43_phyops_g;
+
+#endif /* LINUX_B43_PHY_G_H_ */
diff --git a/drivers/net/wireless/b43/rfkill.c b/drivers/net/wireless/b43/rfkill.c
index fec5645944a4..7b9e99adb8c3 100644
--- a/drivers/net/wireless/b43/rfkill.c
+++ b/drivers/net/wireless/b43/rfkill.c
@@ -24,6 +24,7 @@
 
 #include "rfkill.h"
 #include "b43.h"
+#include "phy_common.h"
 
 #include <linux/kmod.h>
 
@@ -114,11 +115,11 @@ static int b43_rfkill_soft_toggle(void *data, enum rfkill_state state)
                         goto out_unlock;
                 }
                 if (!dev->phy.radio_on)
-                        b43_radio_turn_on(dev);
+                        b43_software_rfkill(dev, state);
                 break;
         case RFKILL_STATE_SOFT_BLOCKED:
                 if (dev->phy.radio_on)
-                        b43_radio_turn_off(dev, 0);
+                        b43_software_rfkill(dev, state);
                 break;
         default:
                 b43warn(wl, "Received unexpected rfkill state %d.\n", state);
diff --git a/drivers/net/wireless/b43/sysfs.c b/drivers/net/wireless/b43/sysfs.c
index 275095b8cbe7..5adaa3692d75 100644
--- a/drivers/net/wireless/b43/sysfs.c
+++ b/drivers/net/wireless/b43/sysfs.c
@@ -29,7 +29,7 @@
 #include "b43.h"
 #include "sysfs.h"
 #include "main.h"
-#include "phy.h"
+#include "phy_common.h"
 
 #define GENERIC_FILESIZE        64
 
@@ -59,7 +59,12 @@ static ssize_t b43_attr_interfmode_show(struct device *dev,
 
         mutex_lock(&wldev->wl->mutex);
 
-        switch (wldev->phy.interfmode) {
+        if (wldev->phy.type != B43_PHYTYPE_G) {
+                mutex_unlock(&wldev->wl->mutex);
+                return -ENOSYS;
+        }
+
+        switch (wldev->phy.g->interfmode) {
         case B43_INTERFMODE_NONE:
                 count =
                     snprintf(buf, PAGE_SIZE,
@@ -117,11 +122,15 @@ static ssize_t b43_attr_interfmode_store(struct device *dev,
         mutex_lock(&wldev->wl->mutex);
         spin_lock_irqsave(&wldev->wl->irq_lock, flags);
 
-        err = b43_radio_set_interference_mitigation(wldev, mode);
-        if (err) {
-                b43err(wldev->wl, "Interference Mitigation not "
-                       "supported by device\n");
-        }
+        if (wldev->phy.ops->interf_mitigation) {
+                err = wldev->phy.ops->interf_mitigation(wldev, mode);
+                if (err) {
+                        b43err(wldev->wl, "Interference Mitigation not "
+                               "supported by device\n");
+                }
+        } else
+                err = -ENOSYS;
+
         mmiowb();
         spin_unlock_irqrestore(&wldev->wl->irq_lock, flags);
         mutex_unlock(&wldev->wl->mutex);
diff --git a/drivers/net/wireless/b43/tables.c b/drivers/net/wireless/b43/tables.c
index 3f5ea06bf13c..1ef9a6463ec6 100644
--- a/drivers/net/wireless/b43/tables.c
+++ b/drivers/net/wireless/b43/tables.c
@@ -27,7 +27,8 @@
 
 #include "b43.h"
 #include "tables.h"
-#include "phy.h"
+#include "phy_g.h"
+
 
 const u32 b43_tab_rotor[] = {
         0xFEB93FFD, 0xFEC63FFD, /* 0 */
@@ -377,17 +378,17 @@ static inline void assert_sizes(void)
 
 u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = dev->phy.g;
         u16 addr;
 
         addr = table + offset;
-        if ((phy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_READ) ||
-            (addr - 1 != phy->ofdmtab_addr)) {
+        if ((gphy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_READ) ||
+            (addr - 1 != gphy->ofdmtab_addr)) {
                 /* The hardware has a different address in memory. Update it. */
                 b43_phy_write(dev, B43_PHY_OTABLECTL, addr);
-                phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_READ;
+                gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_READ;
         }
-        phy->ofdmtab_addr = addr;
+        gphy->ofdmtab_addr = addr;
 
         return b43_phy_read(dev, B43_PHY_OTABLEI);
 
@@ -398,34 +399,34 @@ u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset)
 void b43_ofdmtab_write16(struct b43_wldev *dev, u16 table,
                          u16 offset, u16 value)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = dev->phy.g;
         u16 addr;
 
         addr = table + offset;
-        if ((phy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_WRITE) ||
-            (addr - 1 != phy->ofdmtab_addr)) {
+        if ((gphy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_WRITE) ||
+            (addr - 1 != gphy->ofdmtab_addr)) {
                 /* The hardware has a different address in memory. Update it. */
                 b43_phy_write(dev, B43_PHY_OTABLECTL, addr);
-                phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_WRITE;
+                gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_WRITE;
         }
-        phy->ofdmtab_addr = addr;
+        gphy->ofdmtab_addr = addr;
         b43_phy_write(dev, B43_PHY_OTABLEI, value);
 }
 
 u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = dev->phy.g;
         u32 ret;
         u16 addr;
 
         addr = table + offset;
-        if ((phy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_READ) ||
-            (addr - 1 != phy->ofdmtab_addr)) {
+        if ((gphy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_READ) ||
+            (addr - 1 != gphy->ofdmtab_addr)) {
                 /* The hardware has a different address in memory. Update it. */
                 b43_phy_write(dev, B43_PHY_OTABLECTL, addr);
-                phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_READ;
+                gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_READ;
         }
-        phy->ofdmtab_addr = addr;
+        gphy->ofdmtab_addr = addr;
         ret = b43_phy_read(dev, B43_PHY_OTABLEQ);
         ret <<= 16;
         ret |= b43_phy_read(dev, B43_PHY_OTABLEI);
@@ -436,17 +437,17 @@ u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset)
 void b43_ofdmtab_write32(struct b43_wldev *dev, u16 table,
                          u16 offset, u32 value)
 {
-        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = dev->phy.g;
         u16 addr;
 
         addr = table + offset;
-        if ((phy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_WRITE) ||
-            (addr - 1 != phy->ofdmtab_addr)) {
+        if ((gphy->ofdmtab_addr_direction != B43_OFDMTAB_DIRECTION_WRITE) ||
+            (addr - 1 != gphy->ofdmtab_addr)) {
                 /* The hardware has a different address in memory. Update it. */
                 b43_phy_write(dev, B43_PHY_OTABLECTL, addr);
-                phy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_WRITE;
+                gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_WRITE;
         }
-        phy->ofdmtab_addr = addr;
+        gphy->ofdmtab_addr = addr;
 
         b43_phy_write(dev, B43_PHY_OTABLEI, value);
         b43_phy_write(dev, B43_PHY_OTABLEQ, (value >> 16));
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/b43/tables_nphy.c
index 2aa57551786a..1de2c2e2e14c 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/b43/tables_nphy.c
@@ -24,7 +24,7 @@
 
 #include "b43.h"
 #include "tables_nphy.h"
-#include "phy.h"
+#include "phy_common.h"
 #include "nphy.h"
 
 
diff --git a/drivers/net/wireless/b43/wa.c b/drivers/net/wireless/b43/wa.c
index daa94211f838..0c0fb15abb9f 100644
--- a/drivers/net/wireless/b43/wa.c
+++ b/drivers/net/wireless/b43/wa.c
@@ -27,7 +27,7 @@
 #include "b43.h"
 #include "main.h"
 #include "tables.h"
-#include "phy.h"
+#include "phy_common.h"
 #include "wa.h"
 
 static void b43_wa_papd(struct b43_wldev *dev)
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index 9dda8169f7cc..5e0b71c3ad02 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -28,7 +28,7 @@
 */
 
 #include "xmit.h"
-#include "phy.h"
+#include "phy_common.h"
 #include "dma.h"
 #include "pio.h"
 
@@ -431,6 +431,7 @@ static s8 b43_rssi_postprocess(struct b43_wldev *dev,
                                int adjust_2053, int adjust_2050)
 {
         struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
         s32 tmp;
 
         switch (phy->radio_ver) {
@@ -450,7 +451,8 @@ static s8 b43_rssi_postprocess(struct b43_wldev *dev,
                             boardflags_lo & B43_BFL_RSSI) {
                                 if (in_rssi > 63)
                                         in_rssi = 63;
-                                tmp = phy->nrssi_lt[in_rssi];
+                                B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+                                tmp = gphy->nrssi_lt[in_rssi];
                                 tmp = 31 - tmp;
                                 tmp *= -131;
                                 tmp /= 128;
@@ -678,6 +680,8 @@ void b43_handle_txstatus(struct b43_wldev *dev,
                 b43_pio_handle_txstatus(dev, status);
         else
                 b43_dma_handle_txstatus(dev, status);
+
+        b43_phy_txpower_check(dev, 0);
 }
 
 /* Fill out the mac80211 TXstatus report based on the b43-specific
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 2541c81932f0..1cb77db5c292 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -34,7 +34,6 @@
 #include <linux/moduleparam.h>
 #include <linux/if_arp.h>
 #include <linux/etherdevice.h>
-#include <linux/version.h>
 #include <linux/firmware.h>
 #include <linux/wireless.h>
 #include <linux/workqueue.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-led.c b/drivers/net/wireless/iwlwifi/iwl-3945-led.c
index d3336966b6b5..705c65bed9fd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-led.c
@@ -27,7 +27,6 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/dma-mapping.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/iwl-led.c
index cb11c4a4d691..4eee1b163cd2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-led.c
@@ -27,7 +27,6 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/init.h>
 #include <linux/pci.h>
 #include <linux/dma-mapping.h>
diff --git a/drivers/net/wireless/iwlwifi/iwl-rfkill.c b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
index e5e5846e9f25..5d642298f04c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rfkill.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
@@ -27,7 +27,6 @@
  *****************************************************************************/
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/version.h>
 #include <linux/init.h>
 
 #include <net/mac80211.h>
diff --git a/drivers/net/wireless/libertas/assoc.c b/drivers/net/wireless/libertas/assoc.c
index a267d6e65f03..4ddf44b2758b 100644
--- a/drivers/net/wireless/libertas/assoc.c
+++ b/drivers/net/wireless/libertas/assoc.c
@@ -8,6 +8,7 @@
 #include "scan.h"
 #include "cmd.h"
 
+static int lbs_adhoc_post(struct lbs_private *priv, struct cmd_header *resp);
 
 static const u8 bssid_any[ETH_ALEN]  __attribute__ ((aligned (2))) =
         { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
@@ -20,12 +21,88 @@ static const u8 bssid_off[ETH_ALEN]  __attribute__ ((aligned (2))) =
 #define CAPINFO_MASK    (~(0xda00))
 
 
+/**
+ *  @brief This function finds common rates between rates and card rates.
+ *
+ * It will fill common rates in rates as output if found.
+ *
+ * NOTE: Setting the MSB of the basic rates need to be taken
+ *   care, either before or after calling this function
+ *
+ *  @param priv     A pointer to struct lbs_private structure
+ *  @param rates       the buffer which keeps input and output
+ *  @param rates_size  the size of rate1 buffer; new size of buffer on return
+ *
+ *  @return            0 on success, or -1 on error
+ */
+static int get_common_rates(struct lbs_private *priv,
+        u8 *rates,
+        u16 *rates_size)
+{
+        u8 *card_rates = lbs_bg_rates;
+        size_t num_card_rates = sizeof(lbs_bg_rates);
+        int ret = 0, i, j;
+        u8 tmp[30];
+        size_t tmp_size = 0;
+
+        /* For each rate in card_rates that exists in rate1, copy to tmp */
+        for (i = 0; card_rates[i] && (i < num_card_rates); i++) {
+                for (j = 0; rates[j] && (j < *rates_size); j++) {
+                        if (rates[j] == card_rates[i])
+                                tmp[tmp_size++] = card_rates[i];
+                }
+        }
+
+        lbs_deb_hex(LBS_DEB_JOIN, "AP rates    ", rates, *rates_size);
+        lbs_deb_hex(LBS_DEB_JOIN, "card rates  ", card_rates, num_card_rates);
+        lbs_deb_hex(LBS_DEB_JOIN, "common rates", tmp, tmp_size);
+        lbs_deb_join("TX data rate 0x%02x\n", priv->cur_rate);
+
+        if (!priv->enablehwauto) {
+                for (i = 0; i < tmp_size; i++) {
+                        if (tmp[i] == priv->cur_rate)
+                                goto done;
+                }
+                lbs_pr_alert("Previously set fixed data rate %#x isn't "
+                       "compatible with the network.\n", priv->cur_rate);
+                ret = -1;
+                goto done;
+        }
+        ret = 0;
+
+done:
+        memset(rates, 0, *rates_size);
+        *rates_size = min_t(int, tmp_size, *rates_size);
+        memcpy(rates, tmp, *rates_size);
+        return ret;
+}
+
+
+/**
+ *  @brief Sets the MSB on basic rates as the firmware requires
+ *
+ * Scan through an array and set the MSB for basic data rates.
+ *
+ *  @param rates     buffer of data rates
+ *  @param len       size of buffer
+ */
+static void lbs_set_basic_rate_flags(u8 *rates, size_t len)
+{
+        int i;
+
+        for (i = 0; i < len; i++) {
+                if (rates[i] == 0x02 || rates[i] == 0x04 ||
+                    rates[i] == 0x0b || rates[i] == 0x16)
+                        rates[i] |= 0x80;
+        }
+}
+
 
 /**
  *  @brief Associate to a specific BSS discovered in a scan
  *
  *  @param priv      A pointer to struct lbs_private structure
- *  @param pbssdesc  Pointer to the BSS descriptor to associate with.
+ *  @param assoc_req The association request describing the BSS to associate with
  *
  *  @return          0-success, otherwise fail
  */
@@ -33,29 +110,29 @@ static int lbs_associate(struct lbs_private *priv,
         struct assoc_request *assoc_req)
 {
         int ret;
+        u8 preamble = RADIO_PREAMBLE_LONG;
 
         lbs_deb_enter(LBS_DEB_ASSOC);
 
         ret = lbs_prepare_and_send_command(priv, CMD_802_11_AUTHENTICATE,
                                     0, CMD_OPTION_WAITFORRSP,
                                     0, assoc_req->bss.bssid);
-
         if (ret)
-                goto done;
+                goto out;
 
-        /* set preamble to firmware */
+        /* Use short preamble only when both the BSS and firmware support it */
         if ((priv->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) &&
             (assoc_req->bss.capability & WLAN_CAPABILITY_SHORT_PREAMBLE))
-                priv->preamble = CMD_TYPE_SHORT_PREAMBLE;
-        else
-                priv->preamble = CMD_TYPE_LONG_PREAMBLE;
+                preamble = RADIO_PREAMBLE_SHORT;
 
-        lbs_set_radio_control(priv);
+        ret = lbs_set_radio(priv, preamble, 1);
+        if (ret)
+                goto out;
 
         ret = lbs_prepare_and_send_command(priv, CMD_802_11_ASSOCIATE,
                                     0, CMD_OPTION_WAITFORRSP, 0, assoc_req);
 
-done:
+out:
         lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
         return ret;
 }
@@ -64,17 +141,22 @@ done:
  *  @brief Join an adhoc network found in a previous scan
  *
  *  @param priv         A pointer to struct lbs_private structure
- *  @param pbssdesc     Pointer to a BSS descriptor found in a previous scan
- *                      to attempt to join
+ *  @param assoc_req    The association request describing the BSS to join
  *
- *  @return             0--success, -1--fail
+ *  @return             0 on success, error on failure
  */
-static int lbs_join_adhoc_network(struct lbs_private *priv,
+static int lbs_adhoc_join(struct lbs_private *priv,
         struct assoc_request *assoc_req)
 {
+        struct cmd_ds_802_11_ad_hoc_join cmd;
         struct bss_descriptor *bss = &assoc_req->bss;
+        u8 preamble = RADIO_PREAMBLE_LONG;
+        DECLARE_MAC_BUF(mac);
+        u16 ratesize = 0;
         int ret = 0;
 
+        lbs_deb_enter(LBS_DEB_ASSOC);
+
         lbs_deb_join("current SSID '%s', ssid length %u\n",
                 escape_essid(priv->curbssparams.ssid,
                 priv->curbssparams.ssid_len),
@@ -106,29 +188,106 @@ static int lbs_join_adhoc_network(struct lbs_private *priv,
                 goto out;
         }
 
-        /* Use shortpreamble only when both creator and card supports
-           short preamble */
-        if (!(bss->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) ||
-            !(priv->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)) {
-                lbs_deb_join("AdhocJoin: Long preamble\n");
-                priv->preamble = CMD_TYPE_LONG_PREAMBLE;
-        } else {
+        /* Use short preamble only when both the BSS and firmware support it */
+        if ((priv->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) &&
+            (bss->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)) {
                 lbs_deb_join("AdhocJoin: Short preamble\n");
-                priv->preamble = CMD_TYPE_SHORT_PREAMBLE;
+                preamble = RADIO_PREAMBLE_SHORT;
         }
 
-        lbs_set_radio_control(priv);
+        ret = lbs_set_radio(priv, preamble, 1);
+        if (ret)
+                goto out;
 
         lbs_deb_join("AdhocJoin: channel = %d\n", assoc_req->channel);
         lbs_deb_join("AdhocJoin: band = %c\n", assoc_req->band);
 
         priv->adhoccreate = 0;
+        priv->curbssparams.channel = bss->channel;
 
-        ret = lbs_prepare_and_send_command(priv, CMD_802_11_AD_HOC_JOIN,
-                                    0, CMD_OPTION_WAITFORRSP,
-                                    OID_802_11_SSID, assoc_req);
+        /* Build the join command */
+        memset(&cmd, 0, sizeof(cmd));
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+
+        cmd.bss.type = CMD_BSS_TYPE_IBSS;
+        cmd.bss.beaconperiod = cpu_to_le16(bss->beaconperiod);
+
+        memcpy(&cmd.bss.bssid, &bss->bssid, ETH_ALEN);
+        memcpy(&cmd.bss.ssid, &bss->ssid, bss->ssid_len);
+
+        memcpy(&cmd.bss.phyparamset, &bss->phyparamset,
+               sizeof(union ieeetypes_phyparamset));
+
+        memcpy(&cmd.bss.ssparamset, &bss->ssparamset,
+               sizeof(union IEEEtypes_ssparamset));
+
+        cmd.bss.capability = cpu_to_le16(bss->capability & CAPINFO_MASK);
+        lbs_deb_join("ADHOC_J_CMD: tmpcap=%4X CAPINFO_MASK=%4X\n",
+               bss->capability, CAPINFO_MASK);
+
+        /* information on BSSID descriptor passed to FW */
+        lbs_deb_join("ADHOC_J_CMD: BSSID = %s, SSID = '%s'\n",
+                        print_mac(mac, cmd.bss.bssid), cmd.bss.ssid);
+
+        /* Only v8 and below support setting these */
+        if (priv->fwrelease < 0x09000000) {
+                /* failtimeout */
+                cmd.failtimeout = cpu_to_le16(MRVDRV_ASSOCIATION_TIME_OUT);
+                /* probedelay */
+                cmd.probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
+        }
+
+        /* Copy Data rates from the rates recorded in scan response */
+        memset(cmd.bss.rates, 0, sizeof(cmd.bss.rates));
+        ratesize = min_t(u16, sizeof(cmd.bss.rates), MAX_RATES);
+        memcpy(cmd.bss.rates, bss->rates, ratesize);
+        if (get_common_rates(priv, cmd.bss.rates, &ratesize)) {
+                lbs_deb_join("ADHOC_JOIN: get_common_rates returned error.\n");
+                ret = -1;
+                goto out;
+        }
+
+        /* Copy the ad-hoc creation rates into Current BSS state structure */
+        memset(&priv->curbssparams.rates, 0, sizeof(priv->curbssparams.rates));
+        memcpy(&priv->curbssparams.rates, cmd.bss.rates, ratesize);
+
+        /* Set MSB on basic rates as the firmware requires, but _after_
+         * copying to current bss rates.
+         */
+        lbs_set_basic_rate_flags(cmd.bss.rates, ratesize);
+
+        cmd.bss.ssparamset.ibssparamset.atimwindow = cpu_to_le16(bss->atimwindow);
+
+        if (assoc_req->secinfo.wep_enabled) {
+                u16 tmp = le16_to_cpu(cmd.bss.capability);
+                tmp |= WLAN_CAPABILITY_PRIVACY;
+                cmd.bss.capability = cpu_to_le16(tmp);
+        }
+
+        if (priv->psmode == LBS802_11POWERMODEMAX_PSP) {
+                __le32 local_ps_mode = cpu_to_le32(LBS802_11POWERMODECAM);
+
+                /* wake up first */
+                ret = lbs_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
+                                                   CMD_ACT_SET, 0, 0,
+                                                   &local_ps_mode);
+                if (ret) {
+                        ret = -1;
+                        goto out;
+                }
+        }
+
+        if (lbs_parse_dnld_countryinfo_11d(priv, bss)) {
+                ret = -1;
+                goto out;
+        }
+
+        ret = lbs_cmd_with_response(priv, CMD_802_11_AD_HOC_JOIN, &cmd);
+        if (ret == 0)
+                ret = lbs_adhoc_post(priv, (struct cmd_header *) &cmd);
 
 out:
+        lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
         return ret;
 }
 
@@ -136,39 +295,131 @@ out:
  *  @brief Start an Adhoc Network
  *
  *  @param priv         A pointer to struct lbs_private structure
- *  @param adhocssid    The ssid of the Adhoc Network
- *  @return             0--success, -1--fail
+ *  @param assoc_req    The association request describing the BSS to start
+ *
+ *  @return             0 on success, error on failure
  */
-static int lbs_start_adhoc_network(struct lbs_private *priv,
+static int lbs_adhoc_start(struct lbs_private *priv,
         struct assoc_request *assoc_req)
 {
+        struct cmd_ds_802_11_ad_hoc_start cmd;
+        u8 preamble = RADIO_PREAMBLE_LONG;
+        size_t ratesize = 0;
+        u16 tmpcap = 0;
         int ret = 0;
 
-        priv->adhoccreate = 1;
+        lbs_deb_enter(LBS_DEB_ASSOC);
 
         if (priv->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) {
-                lbs_deb_join("AdhocStart: Short preamble\n");
-                priv->preamble = CMD_TYPE_SHORT_PREAMBLE;
-        } else {
-                lbs_deb_join("AdhocStart: Long preamble\n");
-                priv->preamble = CMD_TYPE_LONG_PREAMBLE;
+                lbs_deb_join("ADHOC_START: Will use short preamble\n");
+                preamble = RADIO_PREAMBLE_SHORT;
         }
 
-        lbs_set_radio_control(priv);
+        ret = lbs_set_radio(priv, preamble, 1);
+        if (ret)
+                goto out;
 
-        lbs_deb_join("AdhocStart: channel = %d\n", assoc_req->channel);
-        lbs_deb_join("AdhocStart: band = %d\n", assoc_req->band);
+        /* Build the start command */
+        memset(&cmd, 0, sizeof(cmd));
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 
-        ret = lbs_prepare_and_send_command(priv, CMD_802_11_AD_HOC_START,
-                                    0, CMD_OPTION_WAITFORRSP, 0, assoc_req);
+        memcpy(cmd.ssid, assoc_req->ssid, assoc_req->ssid_len);
+
+        lbs_deb_join("ADHOC_START: SSID '%s', ssid length %u\n",
+                escape_essid(assoc_req->ssid, assoc_req->ssid_len),
+                assoc_req->ssid_len);
 
+        cmd.bsstype = CMD_BSS_TYPE_IBSS;
+
+        if (priv->beacon_period == 0)
+                priv->beacon_period = MRVDRV_BEACON_INTERVAL;
+        cmd.beaconperiod = cpu_to_le16(priv->beacon_period);
+
+        WARN_ON(!assoc_req->channel);
+
+        /* set Physical parameter set */
+        cmd.phyparamset.dsparamset.elementid = MFIE_TYPE_DS_SET;
+        cmd.phyparamset.dsparamset.len = 1;
+        cmd.phyparamset.dsparamset.currentchan = assoc_req->channel;
+
+        /* set IBSS parameter set */
+        cmd.ssparamset.ibssparamset.elementid = MFIE_TYPE_IBSS_SET;
+        cmd.ssparamset.ibssparamset.len = 2;
+        cmd.ssparamset.ibssparamset.atimwindow = 0;
+
+        /* set capability info */
+        tmpcap = WLAN_CAPABILITY_IBSS;
+        if (assoc_req->secinfo.wep_enabled) {
+                lbs_deb_join("ADHOC_START: WEP enabled, setting privacy on\n");
+                tmpcap |= WLAN_CAPABILITY_PRIVACY;
+        } else
+                lbs_deb_join("ADHOC_START: WEP disabled, setting privacy off\n");
+
+        cmd.capability = cpu_to_le16(tmpcap);
+
+        /* Only v8 and below support setting probe delay */
+        if (priv->fwrelease < 0x09000000)
+                cmd.probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
+
+        ratesize = min(sizeof(cmd.rates), sizeof(lbs_bg_rates));
+        memcpy(cmd.rates, lbs_bg_rates, ratesize);
+
+        /* Copy the ad-hoc creating rates into Current BSS state structure */
+        memset(&priv->curbssparams.rates, 0, sizeof(priv->curbssparams.rates));
+        memcpy(&priv->curbssparams.rates, &cmd.rates, ratesize);
+
+        /* Set MSB on basic rates as the firmware requires, but _after_
+         * copying to current bss rates.
+         */
+        lbs_set_basic_rate_flags(cmd.rates, ratesize);
+
+        lbs_deb_join("ADHOC_START: rates=%02x %02x %02x %02x\n",
+               cmd.rates[0], cmd.rates[1], cmd.rates[2], cmd.rates[3]);
+
+        if (lbs_create_dnld_countryinfo_11d(priv)) {
+                lbs_deb_join("ADHOC_START: dnld_countryinfo_11d failed\n");
+                ret = -1;
+                goto out;
+        }
+
+        lbs_deb_join("ADHOC_START: Starting Ad-Hoc BSS on channel %d, band %d\n",
+                     assoc_req->channel, assoc_req->band);
+
+        priv->adhoccreate = 1;
+        priv->mode = IW_MODE_ADHOC;
+
+        ret = lbs_cmd_with_response(priv, CMD_802_11_AD_HOC_START, &cmd);
+        if (ret == 0)
+                ret = lbs_adhoc_post(priv, (struct cmd_header *) &cmd);
+
+out:
+        lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
         return ret;
 }
 
-int lbs_stop_adhoc_network(struct lbs_private *priv)
+/**
+ *  @brief Stop and Ad-Hoc network and exit Ad-Hoc mode
+ *
+ *  @param priv         A pointer to struct lbs_private structure
+ *  @return             0 on success, or an error
+ */
+int lbs_adhoc_stop(struct lbs_private *priv)
 {
-        return lbs_prepare_and_send_command(priv, CMD_802_11_AD_HOC_STOP,
-                                     0, CMD_OPTION_WAITFORRSP, 0, NULL);
+        struct cmd_ds_802_11_ad_hoc_stop cmd;
+        int ret;
+
+        lbs_deb_enter(LBS_DEB_JOIN);
+
+        memset(&cmd, 0, sizeof (cmd));
+        cmd.hdr.size = cpu_to_le16 (sizeof (cmd));
+
+        ret = lbs_cmd_with_response(priv, CMD_802_11_AD_HOC_STOP, &cmd);
+
+        /* Clean up everything even if there was an error */
+        lbs_mac_event_disconnected(priv);
+
+        lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
+        return ret;
 }
 
 static inline int match_bss_no_security(struct lbs_802_11_security *secinfo,
@@ -480,14 +731,14 @@ static int assoc_helper_essid(struct lbs_private *priv,
                 if (bss != NULL) {
                         lbs_deb_assoc("SSID found, will join\n");
                         memcpy(&assoc_req->bss, bss, sizeof(struct bss_descriptor));
-                        lbs_join_adhoc_network(priv, assoc_req);
+                        lbs_adhoc_join(priv, assoc_req);
                 } else {
                         /* else send START command */
                         lbs_deb_assoc("SSID not found, creating adhoc network\n");
                         memcpy(&assoc_req->bss.ssid, &assoc_req->ssid,
                                 IW_ESSID_MAX_SIZE);
                         assoc_req->bss.ssid_len = assoc_req->ssid_len;
-                        lbs_start_adhoc_network(priv, assoc_req);
+                        lbs_adhoc_start(priv, assoc_req);
                 }
         }
 
@@ -520,7 +771,7 @@ static int assoc_helper_bssid(struct lbs_private *priv,
                 ret = lbs_associate(priv, assoc_req);
                 lbs_deb_assoc("ASSOC: lbs_associate(bssid) returned %d\n", ret);
         } else if (assoc_req->mode == IW_MODE_ADHOC) {
-                lbs_join_adhoc_network(priv, assoc_req);
+                lbs_adhoc_join(priv, assoc_req);
         }
 
 out:
@@ -1029,7 +1280,9 @@ void lbs_association_worker(struct work_struct *work)
          */
         if (priv->mode == IW_MODE_INFRA) {
                 if (should_deauth_infrastructure(priv, assoc_req)) {
-                        ret = lbs_send_deauthentication(priv);
+                        ret = lbs_cmd_80211_deauthenticate(priv,
+                                                           priv->curbssparams.bssid,
+                                                           WLAN_REASON_DEAUTH_LEAVING);
                         if (ret) {
                                 lbs_deb_assoc("Deauthentication due to new "
                                         "configuration request failed: %d\n",
@@ -1038,7 +1291,7 @@ void lbs_association_worker(struct work_struct *work)
                 }
         } else if (priv->mode == IW_MODE_ADHOC) {
                 if (should_stop_adhoc(priv, assoc_req)) {
-                        ret = lbs_stop_adhoc_network(priv);
+                        ret = lbs_adhoc_stop(priv);
                         if (ret) {
                                 lbs_deb_assoc("Teardown of AdHoc network due to "
                                         "new configuration request failed: %d\n",
@@ -1214,94 +1467,6 @@ struct assoc_request *lbs_get_association_request(struct lbs_private *priv)
 
 
 /**
- *  @brief This function finds common rates between rate1 and card rates.
- *
- * It will fill common rates in rate1 as output if found.
- *
- * NOTE: Setting the MSB of the basic rates need to be taken
- *   care, either before or after calling this function
- *
- *  @param priv     A pointer to struct lbs_private structure
- *  @param rate1       the buffer which keeps input and output
- *  @param rate1_size  the size of rate1 buffer; new size of buffer on return
- *
- *  @return            0 or -1
- */
-static int get_common_rates(struct lbs_private *priv,
-        u8 *rates,
-        u16 *rates_size)
-{
-        u8 *card_rates = lbs_bg_rates;
-        size_t num_card_rates = sizeof(lbs_bg_rates);
-        int ret = 0, i, j;
-        u8 tmp[30];
-        size_t tmp_size = 0;
-
-        /* For each rate in card_rates that exists in rate1, copy to tmp */
-        for (i = 0; card_rates[i] && (i < num_card_rates); i++) {
-                for (j = 0; rates[j] && (j < *rates_size); j++) {
-                        if (rates[j] == card_rates[i])
-                                tmp[tmp_size++] = card_rates[i];
-                }
-        }
-
-        lbs_deb_hex(LBS_DEB_JOIN, "AP rates    ", rates, *rates_size);
-        lbs_deb_hex(LBS_DEB_JOIN, "card rates  ", card_rates, num_card_rates);
-        lbs_deb_hex(LBS_DEB_JOIN, "common rates", tmp, tmp_size);
-        lbs_deb_join("TX data rate 0x%02x\n", priv->cur_rate);
-
-        if (!priv->enablehwauto) {
-                for (i = 0; i < tmp_size; i++) {
-                        if (tmp[i] == priv->cur_rate)
-                                goto done;
-                }
-                lbs_pr_alert("Previously set fixed data rate %#x isn't "
-                       "compatible with the network.\n", priv->cur_rate);
-                ret = -1;
-                goto done;
-        }
-        ret = 0;
-
-done:
-        memset(rates, 0, *rates_size);
-        *rates_size = min_t(int, tmp_size, *rates_size);
-        memcpy(rates, tmp, *rates_size);
-        return ret;
-}
-
-
-/**
- *  @brief Sets the MSB on basic rates as the firmware requires
- *
- * Scan through an array and set the MSB for basic data rates.
- *
- *  @param rates     buffer of data rates
- *  @param len       size of buffer
- */
-static void lbs_set_basic_rate_flags(u8 *rates, size_t len)
-{
-        int i;
-
-        for (i = 0; i < len; i++) {
-                if (rates[i] == 0x02 || rates[i] == 0x04 ||
-                    rates[i] == 0x0b || rates[i] == 0x16)
-                        rates[i] |= 0x80;
-        }
-}
-
-/**
- *  @brief Send Deauthentication Request
- *
- *  @param priv      A pointer to struct lbs_private structure
- *  @return          0--success, -1--fail
- */
-int lbs_send_deauthentication(struct lbs_private *priv)
-{
-        return lbs_prepare_and_send_command(priv, CMD_802_11_DEAUTHENTICATE,
-                                     0, CMD_OPTION_WAITFORRSP, 0, NULL);
-}
-
-/**
  *  @brief This function prepares command of authenticate.
  *
  *  @param priv      A pointer to struct lbs_private structure
@@ -1353,26 +1518,37 @@ out:
         return ret;
 }
 
-int lbs_cmd_80211_deauthenticate(struct lbs_private *priv,
-                                   struct cmd_ds_command *cmd)
+/**
+ *  @brief Deauthenticate from a specific BSS
+ *
+ *  @param priv        A pointer to struct lbs_private structure
+ *  @param bssid       The specific BSS to deauthenticate from
+ *  @param reason      The 802.11 sec. 7.3.1.7 Reason Code for deauthenticating
+ *
+ *  @return            0 on success, error on failure
+ */
+int lbs_cmd_80211_deauthenticate(struct lbs_private *priv, u8 bssid[ETH_ALEN],
+                                 u16 reason)
 {
-        struct cmd_ds_802_11_deauthenticate *dauth = &cmd->params.deauth;
+        struct cmd_ds_802_11_deauthenticate cmd;
+        int ret;
 
         lbs_deb_enter(LBS_DEB_JOIN);
 
-        cmd->command = cpu_to_le16(CMD_802_11_DEAUTHENTICATE);
-        cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_deauthenticate) +
-                             S_DS_GEN);
+        memset(&cmd, 0, sizeof(cmd));
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+        memcpy(cmd.macaddr, &bssid[0], ETH_ALEN);
+        cmd.reasoncode = cpu_to_le16(reason);
 
-        /* set AP MAC address */
-        memmove(dauth->macaddr, priv->curbssparams.bssid, ETH_ALEN);
+        ret = lbs_cmd_with_response(priv, CMD_802_11_DEAUTHENTICATE, &cmd);
 
-        /* Reason code 3 = Station is leaving */
-#define REASON_CODE_STA_LEAVING 3
-        dauth->reasoncode = cpu_to_le16(REASON_CODE_STA_LEAVING);
+        /* Clean up everything even if there was an error; can't assume that
+         * we're still authenticated to the AP after trying to deauth.
+         */
+        lbs_mac_event_disconnected(priv);
 
         lbs_deb_leave(LBS_DEB_JOIN);
-        return 0;
+        return ret;
 }
 
 int lbs_cmd_80211_associate(struct lbs_private *priv,
@@ -1489,231 +1665,6 @@ done:
         return ret;
 }
 
-int lbs_cmd_80211_ad_hoc_start(struct lbs_private *priv,
-                                 struct cmd_ds_command *cmd, void *pdata_buf)
-{
-        struct cmd_ds_802_11_ad_hoc_start *adhs = &cmd->params.ads;
-        int ret = 0;
-        int cmdappendsize = 0;
-        struct assoc_request *assoc_req = pdata_buf;
-        u16 tmpcap = 0;
-        size_t ratesize = 0;
-
-        lbs_deb_enter(LBS_DEB_JOIN);
-
-        if (!priv) {
-                ret = -1;
-                goto done;
-        }
-
-        cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_START);
-
-        /*
-         * Fill in the parameters for 2 data structures:
-         *   1. cmd_ds_802_11_ad_hoc_start command
-         *   2. priv->scantable[i]
-         *
-         * Driver will fill up SSID, bsstype,IBSS param, Physical Param,
-         *   probe delay, and cap info.
-         *
-         * Firmware will fill up beacon period, DTIM, Basic rates
-         *   and operational rates.
-         */
-
-        memset(adhs->ssid, 0, IW_ESSID_MAX_SIZE);
-        memcpy(adhs->ssid, assoc_req->ssid, assoc_req->ssid_len);
-
-        lbs_deb_join("ADHOC_S_CMD: SSID '%s', ssid length %u\n",
-                escape_essid(assoc_req->ssid, assoc_req->ssid_len),
-                assoc_req->ssid_len);
-
-        /* set the BSS type */
-        adhs->bsstype = CMD_BSS_TYPE_IBSS;
-        priv->mode = IW_MODE_ADHOC;
-        if (priv->beacon_period == 0)
-                priv->beacon_period = MRVDRV_BEACON_INTERVAL;
-        adhs->beaconperiod = cpu_to_le16(priv->beacon_period);
-
-        /* set Physical param set */
-#define DS_PARA_IE_ID   3
-#define DS_PARA_IE_LEN  1
-
-        adhs->phyparamset.dsparamset.elementid = DS_PARA_IE_ID;
-        adhs->phyparamset.dsparamset.len = DS_PARA_IE_LEN;
-
-        WARN_ON(!assoc_req->channel);
-
-        lbs_deb_join("ADHOC_S_CMD: Creating ADHOC on channel %d\n",
-                     assoc_req->channel);
-
-        adhs->phyparamset.dsparamset.currentchan = assoc_req->channel;
-
-        /* set IBSS param set */
-#define IBSS_PARA_IE_ID   6
-#define IBSS_PARA_IE_LEN  2
-
-        adhs->ssparamset.ibssparamset.elementid = IBSS_PARA_IE_ID;
-        adhs->ssparamset.ibssparamset.len = IBSS_PARA_IE_LEN;
-        adhs->ssparamset.ibssparamset.atimwindow = 0;
-
-        /* set capability info */
-        tmpcap = WLAN_CAPABILITY_IBSS;
-        if (assoc_req->secinfo.wep_enabled) {
-                lbs_deb_join("ADHOC_S_CMD: WEP enabled, "
-                        "setting privacy on\n");
-                tmpcap |= WLAN_CAPABILITY_PRIVACY;
-        } else {
-                lbs_deb_join("ADHOC_S_CMD: WEP disabled, "
-                        "setting privacy off\n");
-        }
-        adhs->capability = cpu_to_le16(tmpcap);
-
-        /* probedelay */
-        adhs->probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
-
-        memset(adhs->rates, 0, sizeof(adhs->rates));
-        ratesize = min(sizeof(adhs->rates), sizeof(lbs_bg_rates));
-        memcpy(adhs->rates, lbs_bg_rates, ratesize);
-
-        /* Copy the ad-hoc creating rates into Current BSS state structure */
-        memset(&priv->curbssparams.rates, 0, sizeof(priv->curbssparams.rates));
-        memcpy(&priv->curbssparams.rates, &adhs->rates, ratesize);
-
-        /* Set MSB on basic rates as the firmware requires, but _after_
-         * copying to current bss rates.
-         */
-        lbs_set_basic_rate_flags(adhs->rates, ratesize);
-
-        lbs_deb_join("ADHOC_S_CMD: rates=%02x %02x %02x %02x \n",
-               adhs->rates[0], adhs->rates[1], adhs->rates[2], adhs->rates[3]);
-
-        lbs_deb_join("ADHOC_S_CMD: AD HOC Start command is ready\n");
-
-        if (lbs_create_dnld_countryinfo_11d(priv)) {
-                lbs_deb_join("ADHOC_S_CMD: dnld_countryinfo_11d failed\n");
-                ret = -1;
-                goto done;
-        }
-
-        cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_ad_hoc_start) +
-                                S_DS_GEN + cmdappendsize);
-
-        ret = 0;
-done:
-        lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
-        return ret;
-}
-
-int lbs_cmd_80211_ad_hoc_stop(struct cmd_ds_command *cmd)
-{
-        cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_STOP);
-        cmd->size = cpu_to_le16(S_DS_GEN);
-
-        return 0;
-}
-
-int lbs_cmd_80211_ad_hoc_join(struct lbs_private *priv,
-                                struct cmd_ds_command *cmd, void *pdata_buf)
-{
-        struct cmd_ds_802_11_ad_hoc_join *join_cmd = &cmd->params.adj;
-        struct assoc_request *assoc_req = pdata_buf;
-        struct bss_descriptor *bss = &assoc_req->bss;
-        int cmdappendsize = 0;
-        int ret = 0;
-        u16 ratesize = 0;
-        DECLARE_MAC_BUF(mac);
-
-        lbs_deb_enter(LBS_DEB_JOIN);
-
-        cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_JOIN);
-
-        join_cmd->bss.type = CMD_BSS_TYPE_IBSS;
-        join_cmd->bss.beaconperiod = cpu_to_le16(bss->beaconperiod);
-
-        memcpy(&join_cmd->bss.bssid, &bss->bssid, ETH_ALEN);
-        memcpy(&join_cmd->bss.ssid, &bss->ssid, bss->ssid_len);
-
-        memcpy(&join_cmd->bss.phyparamset, &bss->phyparamset,
-               sizeof(union ieeetypes_phyparamset));
-
-        memcpy(&join_cmd->bss.ssparamset, &bss->ssparamset,
-               sizeof(union IEEEtypes_ssparamset));
-
-        join_cmd->bss.capability = cpu_to_le16(bss->capability & CAPINFO_MASK);
-        lbs_deb_join("ADHOC_J_CMD: tmpcap=%4X CAPINFO_MASK=%4X\n",
-               bss->capability, CAPINFO_MASK);
-
-        /* information on BSSID descriptor passed to FW */
-        lbs_deb_join(
-               "ADHOC_J_CMD: BSSID = %s, SSID = '%s'\n",
-               print_mac(mac, join_cmd->bss.bssid),
-               join_cmd->bss.ssid);
-
-        /* failtimeout */
-        join_cmd->failtimeout = cpu_to_le16(MRVDRV_ASSOCIATION_TIME_OUT);
-
-        /* probedelay */
-        join_cmd->probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
-
-        priv->curbssparams.channel = bss->channel;
-
-        /* Copy Data rates from the rates recorded in scan response */
-        memset(join_cmd->bss.rates, 0, sizeof(join_cmd->bss.rates));
-        ratesize = min_t(u16, sizeof(join_cmd->bss.rates), MAX_RATES);
-        memcpy(join_cmd->bss.rates, bss->rates, ratesize);
-        if (get_common_rates(priv, join_cmd->bss.rates, &ratesize)) {
-                lbs_deb_join("ADHOC_J_CMD: get_common_rates returns error.\n");
-                ret = -1;
-                goto done;
-        }
-
-        /* Copy the ad-hoc creating rates into Current BSS state structure */
-        memset(&priv->curbssparams.rates, 0, sizeof(priv->curbssparams.rates));
-        memcpy(&priv->curbssparams.rates, join_cmd->bss.rates, ratesize);
-
-        /* Set MSB on basic rates as the firmware requires, but _after_
-         * copying to current bss rates.
-         */
-        lbs_set_basic_rate_flags(join_cmd->bss.rates, ratesize);
-
-        join_cmd->bss.ssparamset.ibssparamset.atimwindow =
-            cpu_to_le16(bss->atimwindow);
-
-        if (assoc_req->secinfo.wep_enabled) {
-                u16 tmp = le16_to_cpu(join_cmd->bss.capability);
-                tmp |= WLAN_CAPABILITY_PRIVACY;
-                join_cmd->bss.capability = cpu_to_le16(tmp);
-        }
-
-        if (priv->psmode == LBS802_11POWERMODEMAX_PSP) {
-                /* wake up first */
-                __le32 Localpsmode;
-
-                Localpsmode = cpu_to_le32(LBS802_11POWERMODECAM);
-                ret = lbs_prepare_and_send_command(priv,
-                                            CMD_802_11_PS_MODE,
-                                            CMD_ACT_SET,
-                                            0, 0, &Localpsmode);
-
-                if (ret) {
-                        ret = -1;
-                        goto done;
-                }
-        }
-
-        if (lbs_parse_dnld_countryinfo_11d(priv, bss)) {
-                ret = -1;
-                goto done;
-        }
-
-        cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_ad_hoc_join) +
-                                S_DS_GEN + cmdappendsize);
-
-done:
-        lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
-        return ret;
-}
-
 int lbs_ret_80211_associate(struct lbs_private *priv,
                               struct cmd_ds_command *resp)
 {
@@ -1815,34 +1766,19 @@ done:
         return ret;
 }
 
-int lbs_ret_80211_disassociate(struct lbs_private *priv)
-{
-        lbs_deb_enter(LBS_DEB_JOIN);
-
-        lbs_mac_event_disconnected(priv);
-
-        lbs_deb_leave(LBS_DEB_JOIN);
-        return 0;
-}
-
-int lbs_ret_80211_ad_hoc_start(struct lbs_private *priv,
-                                 struct cmd_ds_command *resp)
+static int lbs_adhoc_post(struct lbs_private *priv, struct cmd_header *resp)
 {
         int ret = 0;
         u16 command = le16_to_cpu(resp->command);
         u16 result = le16_to_cpu(resp->result);
-        struct cmd_ds_802_11_ad_hoc_result *padhocresult;
+        struct cmd_ds_802_11_ad_hoc_result *adhoc_resp;
         union iwreq_data wrqu;
         struct bss_descriptor *bss;
         DECLARE_MAC_BUF(mac);
 
         lbs_deb_enter(LBS_DEB_JOIN);
 
-        padhocresult = &resp->params.result;
-
-        lbs_deb_join("ADHOC_RESP: size = %d\n", le16_to_cpu(resp->size));
-        lbs_deb_join("ADHOC_RESP: command = %x\n", command);
-        lbs_deb_join("ADHOC_RESP: result = %x\n", result);
+        adhoc_resp = (struct cmd_ds_802_11_ad_hoc_result *) resp;
 
         if (!priv->in_progress_assoc_req) {
                 lbs_deb_join("ADHOC_RESP: no in-progress association "
@@ -1856,26 +1792,19 @@ int lbs_ret_80211_ad_hoc_start(struct lbs_private *priv,
          * Join result code 0 --> SUCCESS
          */
         if (result) {
-                lbs_deb_join("ADHOC_RESP: failed\n");
+                lbs_deb_join("ADHOC_RESP: failed (result 0x%X)\n", result);
                 if (priv->connect_status == LBS_CONNECTED)
                         lbs_mac_event_disconnected(priv);
                 ret = -1;
                 goto done;
         }
 
-        /*
-         * Now the join cmd should be successful
-         * If BSSID has changed use SSID to compare instead of BSSID
-         */
-        lbs_deb_join("ADHOC_RESP: associated to '%s'\n",
-                escape_essid(bss->ssid, bss->ssid_len));
-
         /* Send a Media Connected event, according to the Spec */
         priv->connect_status = LBS_CONNECTED;
 
         if (command == CMD_RET(CMD_802_11_AD_HOC_START)) {
                 /* Update the created network descriptor with the new BSSID */
-                memcpy(bss->bssid, padhocresult->bssid, ETH_ALEN);
+                memcpy(bss->bssid, adhoc_resp->bssid, ETH_ALEN);
         }
 
         /* Set the BSSID from the joined/started descriptor */
@@ -1894,22 +1823,13 @@ int lbs_ret_80211_ad_hoc_start(struct lbs_private *priv,
         wrqu.ap_addr.sa_family = ARPHRD_ETHER;
         wireless_send_event(priv->dev, SIOCGIWAP, &wrqu, NULL);
 
-        lbs_deb_join("ADHOC_RESP: - Joined/Started Ad Hoc\n");
-        lbs_deb_join("ADHOC_RESP: channel = %d\n", priv->curbssparams.channel);
-        lbs_deb_join("ADHOC_RESP: BSSID = %s\n",
-                     print_mac(mac, padhocresult->bssid));
+        lbs_deb_join("ADHOC_RESP: Joined/started '%s', BSSID %s, channel %d\n",
+                     escape_essid(bss->ssid, bss->ssid_len),
+                     print_mac(mac, priv->curbssparams.bssid),
+                     priv->curbssparams.channel);
 
 done:
         lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
         return ret;
 }
 
-int lbs_ret_80211_ad_hoc_stop(struct lbs_private *priv)
-{
-        lbs_deb_enter(LBS_DEB_JOIN);
-
-        lbs_mac_event_disconnected(priv);
-
-        lbs_deb_leave(LBS_DEB_JOIN);
-        return 0;
-}
diff --git a/drivers/net/wireless/libertas/assoc.h b/drivers/net/wireless/libertas/assoc.h
index c516fbe518fd..8b7336dd02a3 100644
--- a/drivers/net/wireless/libertas/assoc.h
+++ b/drivers/net/wireless/libertas/assoc.h
@@ -12,28 +12,18 @@ struct cmd_ds_command;
 int lbs_cmd_80211_authenticate(struct lbs_private *priv,
                                         struct cmd_ds_command *cmd,
                                         void *pdata_buf);
-int lbs_cmd_80211_ad_hoc_join(struct lbs_private *priv,
-                                       struct cmd_ds_command *cmd,
-                                       void *pdata_buf);
-int lbs_cmd_80211_ad_hoc_stop(struct cmd_ds_command *cmd);
-int lbs_cmd_80211_ad_hoc_start(struct lbs_private *priv,
-                                        struct cmd_ds_command *cmd,
-                                        void *pdata_buf);
+
+int lbs_adhoc_stop(struct lbs_private *priv);
+
 int lbs_cmd_80211_deauthenticate(struct lbs_private *priv,
-                                          struct cmd_ds_command *cmd);
+                                 u8 bssid[ETH_ALEN], u16 reason);
 int lbs_cmd_80211_associate(struct lbs_private *priv,
                                      struct cmd_ds_command *cmd,
                                      void *pdata_buf);
 
 int lbs_ret_80211_ad_hoc_start(struct lbs_private *priv,
                                         struct cmd_ds_command *resp);
-int lbs_ret_80211_ad_hoc_stop(struct lbs_private *priv);
-int lbs_ret_80211_disassociate(struct lbs_private *priv);
 int lbs_ret_80211_associate(struct lbs_private *priv,
                                      struct cmd_ds_command *resp);
 
-int lbs_stop_adhoc_network(struct lbs_private *priv);
-
-int lbs_send_deauthentication(struct lbs_private *priv);
-
 #endif /* _LBS_ASSOC_H */
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index af5fd709887f..802547e79671 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -614,47 +614,67 @@ static int lbs_cmd_802_11_snmp_mib(struct lbs_private *priv,
         return 0;
 }
 
-static int lbs_cmd_802_11_rf_tx_power(struct cmd_ds_command *cmd,
-                                       u16 cmd_action, void *pdata_buf)
+/**
+ *  @brief Get the min, max, and current TX power
+ *
+ *  @param priv         A pointer to struct lbs_private structure
+ *  @param curlevel     Current power level in dBm
+ *  @param minlevel     Minimum supported power level in dBm (optional)
+ *  @param maxlevel     Maximum supported power level in dBm (optional)
+ *
+ *  @return             0 on success, error on failure
+ */
+int lbs_get_tx_power(struct lbs_private *priv, s16 *curlevel, s16 *minlevel,
+                     s16 *maxlevel)
 {
-
-        struct cmd_ds_802_11_rf_tx_power *prtp = &cmd->params.txp;
+        struct cmd_ds_802_11_rf_tx_power cmd;
+        int ret;
 
         lbs_deb_enter(LBS_DEB_CMD);
 
-        cmd->size =
-            cpu_to_le16((sizeof(struct cmd_ds_802_11_rf_tx_power)) + S_DS_GEN);
-        cmd->command = cpu_to_le16(CMD_802_11_RF_TX_POWER);
-        prtp->action = cpu_to_le16(cmd_action);
+        memset(&cmd, 0, sizeof(cmd));
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+        cmd.action = cpu_to_le16(CMD_ACT_GET);
+
+        ret = lbs_cmd_with_response(priv, CMD_802_11_RF_TX_POWER, &cmd);
+        if (ret == 0) {
+                *curlevel = le16_to_cpu(cmd.curlevel);
+                if (minlevel)
+                        *minlevel = le16_to_cpu(cmd.minlevel);
+                if (maxlevel)
+                        *maxlevel = le16_to_cpu(cmd.maxlevel);
+        }
 
-        lbs_deb_cmd("RF_TX_POWER_CMD: size:%d cmd:0x%x Act:%d\n",
-                    le16_to_cpu(cmd->size), le16_to_cpu(cmd->command),
-                    le16_to_cpu(prtp->action));
+        lbs_deb_leave(LBS_DEB_CMD);
+        return ret;
+}
 
-        switch (cmd_action) {
-        case CMD_ACT_TX_POWER_OPT_GET:
-                prtp->action = cpu_to_le16(CMD_ACT_GET);
-                prtp->currentlevel = 0;
-                break;
+/**
+ *  @brief Set the TX power
+ *
+ *  @param priv         A pointer to struct lbs_private structure
+ *  @param dbm          The desired power level in dBm
+ *
+ *  @return             0 on success, error on failure
+ */
+int lbs_set_tx_power(struct lbs_private *priv, s16 dbm)
+{
+        struct cmd_ds_802_11_rf_tx_power cmd;
+        int ret;
 
-        case CMD_ACT_TX_POWER_OPT_SET_HIGH:
-                prtp->action = cpu_to_le16(CMD_ACT_SET);
-                prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_HIGH);
-                break;
+        lbs_deb_enter(LBS_DEB_CMD);
 
-        case CMD_ACT_TX_POWER_OPT_SET_MID:
-                prtp->action = cpu_to_le16(CMD_ACT_SET);
-                prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_MID);
-                break;
+        memset(&cmd, 0, sizeof(cmd));
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+        cmd.action = cpu_to_le16(CMD_ACT_SET);
+        cmd.curlevel = cpu_to_le16(dbm);
 
-        case CMD_ACT_TX_POWER_OPT_SET_LOW:
-                prtp->action = cpu_to_le16(CMD_ACT_SET);
-                prtp->currentlevel = cpu_to_le16(*((u16 *) pdata_buf));
-                break;
-        }
+        lbs_deb_cmd("SET_RF_TX_POWER: %d dBm\n", dbm);
+
+        ret = lbs_cmd_with_response(priv, CMD_802_11_RF_TX_POWER, &cmd);
 
         lbs_deb_leave(LBS_DEB_CMD);
-        return 0;
+        return ret;
 }
 
 static int lbs_cmd_802_11_monitor_mode(struct cmd_ds_command *cmd,
@@ -1269,41 +1289,47 @@ void lbs_complete_command(struct lbs_private *priv, struct cmd_ctrl_node *cmd,
         priv->cur_cmd = NULL;
 }
 
-int lbs_set_radio_control(struct lbs_private *priv)
+int lbs_set_radio(struct lbs_private *priv, u8 preamble, u8 radio_on)
 {
-        int ret = 0;
         struct cmd_ds_802_11_radio_control cmd;
+        int ret = -EINVAL;
 
         lbs_deb_enter(LBS_DEB_CMD);
 
         cmd.hdr.size = cpu_to_le16(sizeof(cmd));
         cmd.action = cpu_to_le16(CMD_ACT_SET);
 
-        switch (priv->preamble) {
-        case CMD_TYPE_SHORT_PREAMBLE:
-                cmd.control = cpu_to_le16(SET_SHORT_PREAMBLE);
-                break;
-
-        case CMD_TYPE_LONG_PREAMBLE:
-                cmd.control = cpu_to_le16(SET_LONG_PREAMBLE);
-                break;
+        /* Only v8 and below support setting the preamble */
+        if (priv->fwrelease < 0x09000000) {
+                switch (preamble) {
+                case RADIO_PREAMBLE_SHORT:
+                        if (!(priv->capability & WLAN_CAPABILITY_SHORT_PREAMBLE))
+                                goto out;
+                        /* Fall through */
+                case RADIO_PREAMBLE_AUTO:
+                case RADIO_PREAMBLE_LONG:
+                        cmd.control = cpu_to_le16(preamble);
+                        break;
+                default:
+                        goto out;
+                }
+        }
 
-        case CMD_TYPE_AUTO_PREAMBLE:
-        default:
-                cmd.control = cpu_to_le16(SET_AUTO_PREAMBLE);
-                break;
+        if (radio_on)
+                cmd.control |= cpu_to_le16(0x1);
+        else {
+                cmd.control &= cpu_to_le16(~0x1);
+                priv->txpower_cur = 0;
         }
 
-        if (priv->radioon)
-                cmd.control |= cpu_to_le16(TURN_ON_RF);
-        else
-                cmd.control &= cpu_to_le16(~TURN_ON_RF);
+        lbs_deb_cmd("RADIO_CONTROL: radio %s, preamble %d\n",
+                    radio_on ? "ON" : "OFF", preamble);
 
-        lbs_deb_cmd("RADIO_SET: radio %d, preamble %d\n", priv->radioon,
-                    priv->preamble);
+        priv->radio_on = radio_on;
 
         ret = lbs_cmd_with_response(priv, CMD_802_11_RADIO_CONTROL, &cmd);
 
+out:
         lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
         return ret;
 }
@@ -1393,14 +1419,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
                 ret = lbs_cmd_80211_associate(priv, cmdptr, pdata_buf);
                 break;
 
-        case CMD_802_11_DEAUTHENTICATE:
-                ret = lbs_cmd_80211_deauthenticate(priv, cmdptr);
-                break;
-
-        case CMD_802_11_AD_HOC_START:
-                ret = lbs_cmd_80211_ad_hoc_start(priv, cmdptr, pdata_buf);
-                break;
-
         case CMD_802_11_RESET:
                 ret = lbs_cmd_802_11_reset(cmdptr, cmd_action);
                 break;
@@ -1420,28 +1438,15 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
                 ret = lbs_cmd_reg_access(cmdptr, cmd_action, pdata_buf);
                 break;
 
-        case CMD_802_11_RF_TX_POWER:
-                ret = lbs_cmd_802_11_rf_tx_power(cmdptr,
-                                                 cmd_action, pdata_buf);
-                break;
-
         case CMD_802_11_MONITOR_MODE:
                 ret = lbs_cmd_802_11_monitor_mode(cmdptr,
                                           cmd_action, pdata_buf);
                 break;
 
-        case CMD_802_11_AD_HOC_JOIN:
-                ret = lbs_cmd_80211_ad_hoc_join(priv, cmdptr, pdata_buf);
-                break;
-
         case CMD_802_11_RSSI:
                 ret = lbs_cmd_802_11_rssi(priv, cmdptr);
                 break;
 
-        case CMD_802_11_AD_HOC_STOP:
-                ret = lbs_cmd_80211_ad_hoc_stop(cmdptr);
-                break;
-
         case CMD_802_11_SET_AFC:
         case CMD_802_11_GET_AFC:
 
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index a53b51f8bdb4..11ac996e895e 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -61,4 +61,10 @@ int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv, uint16_t cmd_action,
 int lbs_cmd_802_11_key_material(struct lbs_private *priv, uint16_t cmd_action,
                                 struct assoc_request *assoc);
 
+int lbs_get_tx_power(struct lbs_private *priv, s16 *curlevel, s16 *minlevel,
+                     s16 *maxlevel);
+int lbs_set_tx_power(struct lbs_private *priv, s16 dbm);
+
+int lbs_set_radio(struct lbs_private *priv, u8 preamble, u8 radio_on);
+
 #endif /* _LBS_CMD_H */
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 24de3c3cf877..0371c83f5661 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -188,21 +188,6 @@ static int lbs_ret_802_11_snmp_mib(struct lbs_private *priv,
         return 0;
 }
 
-static int lbs_ret_802_11_rf_tx_power(struct lbs_private *priv,
-                                       struct cmd_ds_command *resp)
-{
-        struct cmd_ds_802_11_rf_tx_power *rtp = &resp->params.txp;
-
-        lbs_deb_enter(LBS_DEB_CMD);
-
-        priv->txpowerlevel = le16_to_cpu(rtp->currentlevel);
-
-        lbs_deb_cmd("TX power currently %d\n", priv->txpowerlevel);
-
-        lbs_deb_leave(LBS_DEB_CMD);
-        return 0;
-}
-
 static int lbs_ret_802_11_rssi(struct lbs_private *priv,
                                 struct cmd_ds_command *resp)
 {
@@ -273,24 +258,10 @@ static inline int handle_cmd_response(struct lbs_private *priv,
                 ret = lbs_ret_80211_associate(priv, resp);
                 break;
 
-        case CMD_RET(CMD_802_11_DISASSOCIATE):
-        case CMD_RET(CMD_802_11_DEAUTHENTICATE):
-                ret = lbs_ret_80211_disassociate(priv);
-                break;
-
-        case CMD_RET(CMD_802_11_AD_HOC_START):
-        case CMD_RET(CMD_802_11_AD_HOC_JOIN):
-                ret = lbs_ret_80211_ad_hoc_start(priv, resp);
-                break;
-
         case CMD_RET(CMD_802_11_SNMP_MIB):
                 ret = lbs_ret_802_11_snmp_mib(priv, resp);
                 break;
 
-        case CMD_RET(CMD_802_11_RF_TX_POWER):
-                ret = lbs_ret_802_11_rf_tx_power(priv, resp);
-                break;
-
         case CMD_RET(CMD_802_11_SET_AFC):
         case CMD_RET(CMD_802_11_GET_AFC):
                 spin_lock_irqsave(&priv->driver_lock, flags);
@@ -309,10 +280,6 @@ static inline int handle_cmd_response(struct lbs_private *priv,
                 ret = lbs_ret_802_11_rssi(priv, resp);
                 break;
 
-        case CMD_RET(CMD_802_11_AD_HOC_STOP):
-                ret = lbs_ret_80211_ad_hoc_stop(priv);
-                break;
-
         case CMD_RET(CMD_802_11D_DOMAIN_INFO):
                 ret = lbs_ret_802_11d_domain_info(resp);
                 break;
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index a8ac974dacac..1a8888cceadc 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -34,7 +34,6 @@ int lbs_process_event(struct lbs_private *priv, u32 event);
 void lbs_queue_event(struct lbs_private *priv, u32 event);
 void lbs_notify_command_response(struct lbs_private *priv, u8 resp_idx);
 
-int lbs_set_radio_control(struct lbs_private *priv);
 u32 lbs_fw_index_to_data_rate(u8 index);
 u8 lbs_data_rate_to_fw_index(u32 rate);
 
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index f5bb40c54d85..fd59e1816460 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -253,7 +253,9 @@ struct lbs_private {
         u32 connect_status;
         u32 mesh_connect_status;
         u16 regioncode;
-        u16 txpowerlevel;
+        s16 txpower_cur;
+        s16 txpower_min;
+        s16 txpower_max;
 
         /** POWER MANAGEMENT AND PnP SUPPORT */
         u8 surpriseremoved;
@@ -291,8 +293,7 @@ struct lbs_private {
         u16 nextSNRNF;
         u16 numSNRNF;
 
-        u8 radioon;
-        u32 preamble;
+        u8 radio_on;
 
         /** data rate stuff */
         u8 cur_rate;
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index c92e41b4faf4..da618fc997ce 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -61,7 +61,6 @@
 #define CMD_RF_REG_MAP                          0x0023
 #define CMD_802_11_DEAUTHENTICATE               0x0024
 #define CMD_802_11_REASSOCIATE                  0x0025
-#define CMD_802_11_DISASSOCIATE                 0x0026
 #define CMD_MAC_CONTROL                         0x0028
 #define CMD_802_11_AD_HOC_START                 0x002b
 #define CMD_802_11_AD_HOC_JOIN                  0x002c
@@ -153,11 +152,6 @@
 #define CMD_ACT_MAC_ALL_MULTICAST_ENABLE        0x0100
 #define CMD_ACT_MAC_STRICT_PROTECTION_ENABLE    0x0400
 
-/* Define action or option for CMD_802_11_RADIO_CONTROL */
-#define CMD_TYPE_AUTO_PREAMBLE          0x0001
-#define CMD_TYPE_SHORT_PREAMBLE         0x0002
-#define CMD_TYPE_LONG_PREAMBLE          0x0003
-
 /* Event flags for CMD_802_11_SUBSCRIBE_EVENT */
 #define CMD_SUBSCRIBE_RSSI_LOW          0x0001
 #define CMD_SUBSCRIBE_SNR_LOW           0x0002
@@ -166,28 +160,14 @@
 #define CMD_SUBSCRIBE_RSSI_HIGH         0x0010
 #define CMD_SUBSCRIBE_SNR_HIGH          0x0020
 
-#define TURN_ON_RF                      0x01
-#define RADIO_ON                        0x01
-#define RADIO_OFF                       0x00
-
-#define SET_AUTO_PREAMBLE               0x05
-#define SET_SHORT_PREAMBLE              0x03
-#define SET_LONG_PREAMBLE               0x01
+#define RADIO_PREAMBLE_LONG     0x00
+#define RADIO_PREAMBLE_SHORT    0x02
+#define RADIO_PREAMBLE_AUTO     0x04
 
 /* Define action or option for CMD_802_11_RF_CHANNEL */
 #define CMD_OPT_802_11_RF_CHANNEL_GET   0x00
 #define CMD_OPT_802_11_RF_CHANNEL_SET   0x01
 
-/* Define action or option for CMD_802_11_RF_TX_POWER */
-#define CMD_ACT_TX_POWER_OPT_GET        0x0000
-#define CMD_ACT_TX_POWER_OPT_SET_HIGH   0x8007
-#define CMD_ACT_TX_POWER_OPT_SET_MID    0x8004
-#define CMD_ACT_TX_POWER_OPT_SET_LOW    0x8000
-
-#define CMD_ACT_TX_POWER_INDEX_HIGH     0x0007
-#define CMD_ACT_TX_POWER_INDEX_MID      0x0004
-#define CMD_ACT_TX_POWER_INDEX_LOW      0x0000
-
 /* Define action or option for CMD_802_11_DATA_RATE */
 #define CMD_ACT_SET_TX_AUTO             0x0000
 #define CMD_ACT_SET_TX_FIX_RATE         0x0001
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index 913b480211a9..d27c276b2191 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -232,7 +232,9 @@ struct cmd_ds_802_11_authenticate {
 };
 
 struct cmd_ds_802_11_deauthenticate {
-        u8 macaddr[6];
+        struct cmd_header hdr;
+
+        u8 macaddr[ETH_ALEN];
         __le16 reasoncode;
 };
 
@@ -251,20 +253,10 @@ struct cmd_ds_802_11_associate {
 #endif
 } __attribute__ ((packed));
 
-struct cmd_ds_802_11_disassociate {
-        u8 destmacaddr[6];
-        __le16 reasoncode;
-};
-
 struct cmd_ds_802_11_associate_rsp {
         struct ieeetypes_assocrsp assocRsp;
 };
 
-struct cmd_ds_802_11_ad_hoc_result {
-        u8 pad[3];
-        u8 bssid[ETH_ALEN];
-};
-
 struct cmd_ds_802_11_set_wep {
         struct cmd_header hdr;
 
@@ -435,8 +427,12 @@ struct cmd_ds_802_11_mac_address {
 };
 
 struct cmd_ds_802_11_rf_tx_power {
+        struct cmd_header hdr;
+
         __le16 action;
-        __le16 currentlevel;
+        __le16 curlevel;
+        s8 maxlevel;
+        s8 minlevel;
 };
 
 struct cmd_ds_802_11_rf_antenna {
@@ -507,10 +503,12 @@ struct cmd_ds_802_11_rate_adapt_rateset {
 };
 
 struct cmd_ds_802_11_ad_hoc_start {
+        struct cmd_header hdr;
+
         u8 ssid[IW_ESSID_MAX_SIZE];
         u8 bsstype;
         __le16 beaconperiod;
-        u8 dtimperiod;
+        u8 dtimperiod;   /* Reserved on v9 and later */
         union IEEEtypes_ssparamset ssparamset;
         union ieeetypes_phyparamset phyparamset;
         __le16 probedelay;
@@ -519,9 +517,16 @@ struct cmd_ds_802_11_ad_hoc_start {
         u8 tlv_memory_size_pad[100];
 } __attribute__ ((packed));
 
+struct cmd_ds_802_11_ad_hoc_result {
+        struct cmd_header hdr;
+
+        u8 pad[3];
+        u8 bssid[ETH_ALEN];
+};
+
 struct adhoc_bssdesc {
-        u8 bssid[6];
-        u8 ssid[32];
+        u8 bssid[ETH_ALEN];
+        u8 ssid[IW_ESSID_MAX_SIZE];
         u8 type;
         __le16 beaconperiod;
         u8 dtimperiod;
@@ -539,10 +544,15 @@ struct adhoc_bssdesc {
 } __attribute__ ((packed));
 
 struct cmd_ds_802_11_ad_hoc_join {
+        struct cmd_header hdr;
+
         struct adhoc_bssdesc bss;
-        __le16 failtimeout;
-        __le16 probedelay;
+        __le16 failtimeout;   /* Reserved on v9 and later */
+        __le16 probedelay;    /* Reserved on v9 and later */
+} __attribute__ ((packed));
 
+struct cmd_ds_802_11_ad_hoc_stop {
+        struct cmd_header hdr;
 } __attribute__ ((packed));
 
 struct cmd_ds_802_11_enable_rsn {
@@ -693,21 +703,15 @@ struct cmd_ds_command {
         union {
                 struct cmd_ds_802_11_ps_mode psmode;
                 struct cmd_ds_802_11_associate associate;
-                struct cmd_ds_802_11_deauthenticate deauth;
-                struct cmd_ds_802_11_ad_hoc_start ads;
                 struct cmd_ds_802_11_reset reset;
-                struct cmd_ds_802_11_ad_hoc_result result;
                 struct cmd_ds_802_11_authenticate auth;
                 struct cmd_ds_802_11_get_stat gstat;
                 struct cmd_ds_802_3_get_stat gstat_8023;
                 struct cmd_ds_802_11_snmp_mib smib;
-                struct cmd_ds_802_11_rf_tx_power txp;
                 struct cmd_ds_802_11_rf_antenna rant;
                 struct cmd_ds_802_11_monitor_mode monitor;
-                struct cmd_ds_802_11_ad_hoc_join adj;
                 struct cmd_ds_802_11_rssi rssi;
                 struct cmd_ds_802_11_rssi_rsp rssirsp;
-                struct cmd_ds_802_11_disassociate dassociate;
                 struct cmd_ds_mac_reg_access macreg;
                 struct cmd_ds_bbp_reg_access bbpreg;
                 struct cmd_ds_rf_reg_access rfreg;
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index bd32ac0b4e07..2436634b6b7e 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -291,9 +291,11 @@ static ssize_t lbs_rtap_set(struct device *dev,
                         if (priv->infra_open || priv->mesh_open)
                                 return -EBUSY;
                         if (priv->mode == IW_MODE_INFRA)
-                                lbs_send_deauthentication(priv);
+                                lbs_cmd_80211_deauthenticate(priv,
+                                                             priv->curbssparams.bssid,
+                                                             WLAN_REASON_DEAUTH_LEAVING);
                         else if (priv->mode == IW_MODE_ADHOC)
-                                lbs_stop_adhoc_network(priv);
+                                lbs_adhoc_stop(priv);
                         lbs_add_rtap(priv);
                 }
                 priv->monitormode = monitor_mode;
@@ -956,17 +958,24 @@ EXPORT_SYMBOL_GPL(lbs_resume);
 static int lbs_setup_firmware(struct lbs_private *priv)
 {
         int ret = -1;
+        s16 curlevel = 0, minlevel = 0, maxlevel = 0;
 
         lbs_deb_enter(LBS_DEB_FW);
 
-        /*
-         * Read MAC address from HW
-         */
+        /* Read MAC address from firmware */
         memset(priv->current_addr, 0xff, ETH_ALEN);
         ret = lbs_update_hw_spec(priv);
         if (ret)
                 goto done;
 
+        /* Read power levels if available */
+        ret = lbs_get_tx_power(priv, &curlevel, &minlevel, &maxlevel);
+        if (ret == 0) {
+                priv->txpower_cur = curlevel;
+                priv->txpower_min = minlevel;
+                priv->txpower_max = maxlevel;
+        }
+
         lbs_set_mac_control(priv);
 done:
         lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
@@ -1042,7 +1051,7 @@ static int lbs_init_adapter(struct lbs_private *priv)
         priv->mode = IW_MODE_INFRA;
         priv->curbssparams.channel = DEFAULT_AD_HOC_CHANNEL;
         priv->mac_control = CMD_ACT_MAC_RX_ON | CMD_ACT_MAC_TX_ON;
-        priv->radioon = RADIO_ON;
+        priv->radio_on = 1;
         priv->enablehwauto = 1;
         priv->capability = WLAN_CAPABILITY_SHORT_PREAMBLE;
         priv->psmode = LBS802_11POWERMODECAM;
diff --git a/drivers/net/wireless/libertas/scan.c b/drivers/net/wireless/libertas/scan.c
index 4b274562f965..8f66903641b9 100644
--- a/drivers/net/wireless/libertas/scan.c
+++ b/drivers/net/wireless/libertas/scan.c
@@ -944,6 +944,11 @@ int lbs_set_scan(struct net_device *dev, struct iw_request_info *info,
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
+        if (!priv->radio_on) {
+                ret = -EINVAL;
+                goto out;
+        }
+
         if (!netif_running(dev)) {
                 ret = -ENETDOWN;
                 goto out;
diff --git a/drivers/net/wireless/libertas/wext.c b/drivers/net/wireless/libertas/wext.c
index 8b3ed77860b3..426f1fe3bb42 100644
--- a/drivers/net/wireless/libertas/wext.c
+++ b/drivers/net/wireless/libertas/wext.c
@@ -120,34 +120,6 @@ static struct chan_freq_power *find_cfp_by_band_and_freq(
         return cfp;
 }
 
-
-/**
- *  @brief Set Radio On/OFF
- *
- *  @param priv                 A pointer to struct lbs_private structure
- *  @option                     Radio Option
- *  @return                     0 --success, otherwise fail
- */
-static int lbs_radio_ioctl(struct lbs_private *priv, u8 option)
-{
-        int ret = 0;
-
-        lbs_deb_enter(LBS_DEB_WEXT);
-
-        if (priv->radioon != option) {
-                lbs_deb_wext("switching radio %s\n", option ? "on" : "off");
-                priv->radioon = option;
-
-                ret = lbs_prepare_and_send_command(priv,
-                                            CMD_802_11_RADIO_CONTROL,
-                                            CMD_ACT_SET,
-                                            CMD_OPTION_WAITFORRSP, 0, NULL);
-        }
-
-        lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
-        return ret;
-}
-
 /**
  *  @brief Copy active data rates based on adapter mode and status
  *
@@ -420,28 +392,30 @@ static int lbs_get_txpow(struct net_device *dev,
                           struct iw_request_info *info,
                           struct iw_param *vwrq, char *extra)
 {
-        int ret = 0;
         struct lbs_private *priv = dev->priv;
+        s16 curlevel = 0;
+        int ret = 0;
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
-        ret = lbs_prepare_and_send_command(priv,
-                                    CMD_802_11_RF_TX_POWER,
-                                    CMD_ACT_TX_POWER_OPT_GET,
-                                    CMD_OPTION_WAITFORRSP, 0, NULL);
+        if (!priv->radio_on) {
+                lbs_deb_wext("tx power off\n");
+                vwrq->value = 0;
+                vwrq->disabled = 1;
+                goto out;
+        }
 
+        ret = lbs_get_tx_power(priv, &curlevel, NULL, NULL);
         if (ret)
                 goto out;
 
-        lbs_deb_wext("tx power level %d dbm\n", priv->txpowerlevel);
-        vwrq->value = priv->txpowerlevel;
+        lbs_deb_wext("tx power level %d dbm\n", curlevel);
+        priv->txpower_cur = curlevel;
+
+        vwrq->value = curlevel;
         vwrq->fixed = 1;
-        if (priv->radioon) {
-                vwrq->disabled = 0;
-                vwrq->flags = IW_TXPOW_DBM;
-        } else {
-                vwrq->disabled = 1;
-        }
+        vwrq->disabled = 0;
+        vwrq->flags = IW_TXPOW_DBM;
 
 out:
         lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -693,22 +667,12 @@ static int lbs_get_range(struct net_device *dev, struct iw_request_info *info,
 
         range->sensitivity = 0;
 
-        /*
-         * Setup the supported power level ranges
-         */
+        /* Setup the supported power level ranges */
         memset(range->txpower, 0, sizeof(range->txpower));
-        range->txpower[0] = 5;
-        range->txpower[1] = 7;
-        range->txpower[2] = 9;
-        range->txpower[3] = 11;
-        range->txpower[4] = 13;
-        range->txpower[5] = 15;
-        range->txpower[6] = 17;
-        range->txpower[7] = 19;
-
-        range->num_txpower = 8;
-        range->txpower_capa = IW_TXPOW_DBM;
-        range->txpower_capa |= IW_TXPOW_RANGE;
+        range->txpower_capa = IW_TXPOW_DBM | IW_TXPOW_RANGE;
+        range->txpower[0] = priv->txpower_min;
+        range->txpower[1] = priv->txpower_max;
+        range->num_txpower = 2;
 
         range->event_capa[0] = (IW_EVENT_CAPA_K_0 |
                                 IW_EVENT_CAPA_MASK(SIOCGIWAP) |
@@ -998,9 +962,11 @@ static int lbs_mesh_set_freq(struct net_device *dev,
         if (fwrq->m != priv->curbssparams.channel) {
                 lbs_deb_wext("mesh channel change forces eth disconnect\n");
                 if (priv->mode == IW_MODE_INFRA)
-                        lbs_send_deauthentication(priv);
+                        lbs_cmd_80211_deauthenticate(priv,
+                                                     priv->curbssparams.bssid,
+                                                     WLAN_REASON_DEAUTH_LEAVING);
                 else if (priv->mode == IW_MODE_ADHOC)
-                        lbs_stop_adhoc_network(priv);
+                        lbs_adhoc_stop(priv);
         }
         lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START, fwrq->m);
         lbs_update_channel(priv);
@@ -1844,39 +1810,50 @@ static int lbs_set_txpow(struct net_device *dev, struct iw_request_info *info,
 {
         int ret = 0;
         struct lbs_private *priv = dev->priv;
-
-        u16 dbm;
+        s16 dbm = (s16) vwrq->value;
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
         if (vwrq->disabled) {
-                lbs_radio_ioctl(priv, RADIO_OFF);
-                return 0;
+                lbs_set_radio(priv, RADIO_PREAMBLE_AUTO, 0);
+                goto out;
         }
 
-        priv->preamble = CMD_TYPE_AUTO_PREAMBLE;
-
-        lbs_radio_ioctl(priv, RADIO_ON);
+        if (vwrq->fixed == 0) {
+                /* Auto power control */
+                dbm = priv->txpower_max;
+        } else {
+                /* Userspace check in iwrange if it should use dBm or mW,
+                 * therefore this should never happen... Jean II */
+                if ((vwrq->flags & IW_TXPOW_TYPE) != IW_TXPOW_DBM) {
+                        ret = -EOPNOTSUPP;
+                        goto out;
+                }
 
-        /* Userspace check in iwrange if it should use dBm or mW,
-         * therefore this should never happen... Jean II */
-        if ((vwrq->flags & IW_TXPOW_TYPE) == IW_TXPOW_MWATT) {
-                return -EOPNOTSUPP;
-        } else
-                dbm = (u16) vwrq->value;
+                /* Validate requested power level against firmware allowed levels */
+                if (priv->txpower_min && (dbm < priv->txpower_min)) {
+                        ret = -EINVAL;
+                        goto out;
+                }
 
-        /* auto tx power control */
+                if (priv->txpower_max && (dbm > priv->txpower_max)) {
+                        ret = -EINVAL;
+                        goto out;
+                }
+        }
 
-        if (vwrq->fixed == 0)
-                dbm = 0xffff;
+        /* If the radio was off, turn it on */
+        if (!priv->radio_on) {
+                ret = lbs_set_radio(priv, RADIO_PREAMBLE_AUTO, 1);
+                if (ret)
+                        goto out;
+        }
 
-        lbs_deb_wext("txpower set %d dbm\n", dbm);
+        lbs_deb_wext("txpower set %d dBm\n", dbm);
 
-        ret = lbs_prepare_and_send_command(priv,
-                                    CMD_802_11_RF_TX_POWER,
-                                    CMD_ACT_TX_POWER_OPT_SET_LOW,
-                                    CMD_OPTION_WAITFORRSP, 0, (void *)&dbm);
+        ret = lbs_set_tx_power(priv, dbm);
 
+out:
         lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
         return ret;
 }
@@ -1928,6 +1905,11 @@ static int lbs_set_essid(struct net_device *dev, struct iw_request_info *info,
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
+        if (!priv->radio_on) {
+                ret = -EINVAL;
+                goto out;
+        }
+
         /* Check the size of the string */
         if (in_ssid_len > IW_ESSID_MAX_SIZE) {
                 ret = -E2BIG;
@@ -2005,6 +1987,11 @@ static int lbs_mesh_set_essid(struct net_device *dev,
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
+        if (!priv->radio_on) {
+                ret = -EINVAL;
+                goto out;
+        }
+
         /* Check the size of the string */
         if (dwrq->length > IW_ESSID_MAX_SIZE) {
                 ret = -E2BIG;
@@ -2046,6 +2033,9 @@ static int lbs_set_wap(struct net_device *dev, struct iw_request_info *info,
 
         lbs_deb_enter(LBS_DEB_WEXT);
 
+        if (!priv->radio_on)
+                return -EINVAL;
+
         if (awrq->sa_family != ARPHRD_ETHER)
                 return -EINVAL;
 
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index 2c1d680d2c55..c948021bff6a 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -477,9 +477,9 @@ int lbtf_rx(struct lbtf_private *priv, struct sk_buff *skb)
 {
         struct ieee80211_rx_status stats;
         struct rxpd *prxpd;
-        bool is_qos, is_4addr, is_amsdu, need_padding;
+        int need_padding;
         unsigned int flags;
-        u16 fc, fc_le;
+        struct ieee80211_hdr *hdr;
 
         prxpd = (struct rxpd *) skb->data;
 
@@ -497,19 +497,15 @@ int lbtf_rx(struct lbtf_private *priv, struct sk_buff *skb)
         stats.rate_idx = prxpd->rx_rate;
         skb_pull(skb, sizeof(struct rxpd));
 
-        fc_le = *((__le16 *) skb->data);
-        fc = le16_to_cpu(fc_le);
+        hdr = (struct ieee80211_hdr *)skb->data;
         flags = le32_to_cpu(*(__le32 *)(skb->data + 4));
 
-        is_qos = ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
-                 (fc & IEEE80211_STYPE_QOS_DATA);
-        is_4addr = (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-                   (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS);
-        is_amsdu =  ((fc & 0x8C) == 0x88) &&
-                    (*(skb->data + ieee80211_hdrlen(fc_le) - QOS_CONTROL_LEN)
-                     & IEEE80211_QOS_CONTROL_A_MSDU_PRESENT);
+        need_padding = ieee80211_is_data_qos(hdr->frame_control);
+        need_padding ^= ieee80211_has_a4(hdr->frame_control);
+        need_padding ^= ieee80211_is_data_qos(hdr->frame_control) &&
+                        (*ieee80211_get_qos_ctl(hdr) &
+                         IEEE80211_QOS_CONTROL_A_MSDU_PRESENT);
 
-        need_padding = is_qos ^ is_4addr ^ is_amsdu;
         if (need_padding) {
                 memmove(skb->data + 2, skb->data, skb->len);
                 skb_reserve(skb, 2);
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 248d31a7aa33..732429d49122 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -446,7 +446,8 @@ static int __init init_mac80211_hwsim(void)
                 SET_IEEE80211_PERM_ADDR(hw, addr);
 
                 hw->channel_change_time = 1;
-                hw->queues = 1;
+                hw->queues = 4;
+                hw->ampdu_queues = 1;
 
                 memcpy(data->channels, hwsim_channels, sizeof(hwsim_channels));
                 memcpy(data->rates, hwsim_rates, sizeof(hwsim_rates));
@@ -454,6 +455,19 @@ static int __init init_mac80211_hwsim(void)
                 data->band.n_channels = ARRAY_SIZE(hwsim_channels);
                 data->band.bitrates = data->rates;
                 data->band.n_bitrates = ARRAY_SIZE(hwsim_rates);
+                data->band.ht_info.ht_supported = 1;
+                data->band.ht_info.cap = IEEE80211_HT_CAP_SUP_WIDTH |
+                        IEEE80211_HT_CAP_GRN_FLD |
+                        IEEE80211_HT_CAP_SGI_40 |
+                        IEEE80211_HT_CAP_DSSSCCK40;
+                data->band.ht_info.ampdu_factor = 0x3;
+                data->band.ht_info.ampdu_density = 0x6;
+                memset(data->band.ht_info.supp_mcs_set, 0,
+                       sizeof(data->band.ht_info.supp_mcs_set));
+                data->band.ht_info.supp_mcs_set[0] = 0xff;
+                data->band.ht_info.supp_mcs_set[1] = 0xff;
+                data->band.ht_info.supp_mcs_set[12] =
+                        IEEE80211_HT_CAP_MCS_TX_DEFINED;
                 hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &data->band;
 
                 err = ieee80211_register_hw(hw);
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 10df07de9e52..fca8762fa069 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -66,7 +66,7 @@ struct p54_common {
         unsigned int tx_hdr_len;
         void *cached_vdcf;
         unsigned int fw_var;
-        struct ieee80211_tx_queue_stats tx_stats[4];
+        struct ieee80211_tx_queue_stats tx_stats[8];
 };
 
 int p54_rx(struct ieee80211_hw *dev, struct sk_buff *skb);
diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 29be3dc8ee09..17e06bbc996a 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -146,23 +146,23 @@ void p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw)
 
         if (priv->fw_var >= 0x300) {
                 /* Firmware supports QoS, use it! */
-                priv->tx_stats[0].limit = 3;
-                priv->tx_stats[1].limit = 4;
-                priv->tx_stats[2].limit = 3;
-                priv->tx_stats[3].limit = 1;
+                priv->tx_stats[4].limit = 3;
+                priv->tx_stats[5].limit = 4;
+                priv->tx_stats[6].limit = 3;
+                priv->tx_stats[7].limit = 1;
                 dev->queues = 4;
         }
 }
 EXPORT_SYMBOL_GPL(p54_parse_firmware);
 
-static int p54_convert_rev0_to_rev1(struct ieee80211_hw *dev,
-                                    struct pda_pa_curve_data *curve_data)
+static int p54_convert_rev0(struct ieee80211_hw *dev,
+                            struct pda_pa_curve_data *curve_data)
 {
         struct p54_common *priv = dev->priv;
-        struct pda_pa_curve_data_sample_rev1 *rev1;
-        struct pda_pa_curve_data_sample_rev0 *rev0;
+        struct p54_pa_curve_data_sample *dst;
+        struct pda_pa_curve_data_sample_rev0 *src;
         size_t cd_len = sizeof(*curve_data) +
-                (curve_data->points_per_channel*sizeof(*rev1) + 2) *
+                (curve_data->points_per_channel*sizeof(*dst) + 2) *
                  curve_data->channels;
         unsigned int i, j;
         void *source, *target;
@@ -180,27 +180,63 @@ static int p54_convert_rev0_to_rev1(struct ieee80211_hw *dev,
                 *((__le16 *)target) = *freq;
                 target += sizeof(__le16);
                 for (j = 0; j < curve_data->points_per_channel; j++) {
-                        rev1 = target;
-                        rev0 = source;
+                        dst = target;
+                        src = source;
 
-                        rev1->rf_power = rev0->rf_power;
-                        rev1->pa_detector = rev0->pa_detector;
-                        rev1->data_64qam = rev0->pcv;
+                        dst->rf_power = src->rf_power;
+                        dst->pa_detector = src->pa_detector;
+                        dst->data_64qam = src->pcv;
                         /* "invent" the points for the other modulations */
 #define SUB(x,y) (u8)((x) - (y)) > (x) ? 0 : (x) - (y)
-                        rev1->data_16qam = SUB(rev0->pcv, 12);
-                        rev1->data_qpsk  = SUB(rev1->data_16qam, 12);
-                        rev1->data_bpsk  = SUB(rev1->data_qpsk, 12);
-                        rev1->data_barker= SUB(rev1->data_bpsk, 14);
+                        dst->data_16qam = SUB(src->pcv, 12);
+                        dst->data_qpsk = SUB(dst->data_16qam, 12);
+                        dst->data_bpsk = SUB(dst->data_qpsk, 12);
+                        dst->data_barker = SUB(dst->data_bpsk, 14);
 #undef SUB
-                        target += sizeof(*rev1);
-                        source += sizeof(*rev0);
+                        target += sizeof(*dst);
+                        source += sizeof(*src);
                 }
         }
 
         return 0;
 }
 
+static int p54_convert_rev1(struct ieee80211_hw *dev,
+                            struct pda_pa_curve_data *curve_data)
+{
+        struct p54_common *priv = dev->priv;
+        struct p54_pa_curve_data_sample *dst;
+        struct pda_pa_curve_data_sample_rev1 *src;
+        size_t cd_len = sizeof(*curve_data) +
+                (curve_data->points_per_channel*sizeof(*dst) + 2) *
+                 curve_data->channels;
+        unsigned int i, j;
+        void *source, *target;
+
+        priv->curve_data = kmalloc(cd_len, GFP_KERNEL);
+        if (!priv->curve_data)
+                return -ENOMEM;
+
+        memcpy(priv->curve_data, curve_data, sizeof(*curve_data));
+        source = curve_data->data;
+        target = priv->curve_data->data;
+        for (i = 0; i < curve_data->channels; i++) {
+                __le16 *freq = source;
+                source += sizeof(__le16);
+                *((__le16 *)target) = *freq;
+                target += sizeof(__le16);
+                for (j = 0; j < curve_data->points_per_channel; j++) {
+                        memcpy(target, source, sizeof(*src));
+
+                        target += sizeof(*dst);
+                        source += sizeof(*src);
+                }
+                source++;
+        }
+
+        return 0;
+}
+
 int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
 {
         struct p54_common *priv = dev->priv;
@@ -250,27 +286,32 @@ int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
                                entry->data[1]*sizeof(*priv->output_limit));
                         priv->output_limit_len = entry->data[1];
                         break;
-                case PDR_PRISM_PA_CAL_CURVE_DATA:
-                        if (data_len < sizeof(struct pda_pa_curve_data)) {
+                case PDR_PRISM_PA_CAL_CURVE_DATA: {
+                        struct pda_pa_curve_data *curve_data =
+                                (struct pda_pa_curve_data *)entry->data;
+                        if (data_len < sizeof(*curve_data)) {
                                 err = -EINVAL;
                                 goto err;
                         }
 
-                        if (((struct pda_pa_curve_data *)entry->data)->cal_method_rev) {
-                                priv->curve_data = kmalloc(data_len, GFP_KERNEL);
-                                if (!priv->curve_data) {
-                                        err = -ENOMEM;
-                                        goto err;
-                                }
-
-                                memcpy(priv->curve_data, entry->data, data_len);
-                        } else {
-                                err = p54_convert_rev0_to_rev1(dev, (struct pda_pa_curve_data *)entry->data);
-                                if (err)
-                                        goto err;
+                        switch (curve_data->cal_method_rev) {
+                        case 0:
+                                err = p54_convert_rev0(dev, curve_data);
+                                break;
+                        case 1:
+                                err = p54_convert_rev1(dev, curve_data);
+                                break;
+                        default:
+                                printk(KERN_ERR "p54: unknown curve data "
+                                                "revision %d\n",
+                                                curve_data->cal_method_rev);
+                                err = -ENODEV;
+                                break;
                         }
+                        if (err)
+                                goto err;
 
-                        break;
+                }
                 case PDR_PRISM_ZIF_TX_IQ_CALIBRATION:
                         priv->iq_autocal = kmalloc(data_len, GFP_KERNEL);
                         if (!priv->iq_autocal) {
@@ -377,7 +418,7 @@ static void inline p54_wake_free_queues(struct ieee80211_hw *dev)
         int i;
 
         for (i = 0; i < dev->queues; i++)
-                if (priv->tx_stats[i].len < priv->tx_stats[i].limit)
+                if (priv->tx_stats[i + 4].len < priv->tx_stats[i + 4].limit)
                         ieee80211_wake_queue(dev, i);
 }
 
@@ -391,7 +432,9 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
         struct memrecord *range = NULL;
         u32 freed = 0;
         u32 last_addr = priv->rx_start;
+        unsigned long flags;
 
+        spin_lock_irqsave(&priv->tx_queue.lock, flags);
         while (entry != (struct sk_buff *)&priv->tx_queue) {
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
                 range = (void *)info->driver_data;
@@ -412,13 +455,15 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
 
                         last_addr = range->end_addr;
                         __skb_unlink(entry, &priv->tx_queue);
+                        spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
+
                         memset(&info->status, 0, sizeof(info->status));
                         entry_hdr = (struct p54_control_hdr *) entry->data;
                         entry_data = (struct p54_tx_control_allocdata *) entry_hdr->data;
                         if ((entry_hdr->magic1 & cpu_to_le16(0x4000)) != 0)
                                 pad = entry_data->align[0];
 
-                        priv->tx_stats[entry_data->hw_queue - 4].len--;
+                        priv->tx_stats[entry_data->hw_queue].len--;
                         if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                                 if (!(payload->status & 0x01))
                                         info->flags |= IEEE80211_TX_STAT_ACK;
@@ -429,12 +474,14 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
                         info->status.ack_signal = le16_to_cpu(payload->ack_rssi);
                         skb_pull(entry, sizeof(*hdr) + pad + sizeof(*entry_data));
                         ieee80211_tx_status_irqsafe(dev, entry);
-                        break;
+                        goto out;
                 } else
                         last_addr = range->end_addr;
                 entry = entry->next;
         }
+        spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
 
+out:
         if (freed >= IEEE80211_MAX_RTS_THRESHOLD + 0x170 +
             sizeof(struct p54_control_hdr))
                 p54_wake_free_queues(dev);
@@ -559,7 +606,7 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
         u8 rate;
         u8 cts_rate = 0x20;
 
-        current_queue = &priv->tx_stats[skb_get_queue_mapping(skb)];
+        current_queue = &priv->tx_stats[skb_get_queue_mapping(skb) + 4];
         if (unlikely(current_queue->len > current_queue->limit))
                 return NETDEV_TX_BUSY;
         current_queue->len++;
@@ -672,12 +719,9 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
         struct p54_control_hdr *hdr;
         struct p54_tx_control_channel *chan;
         unsigned int i;
-        size_t payload_len = sizeof(*chan) + sizeof(u32)*2 +
-                             sizeof(*chan->curve_data) *
-                             priv->curve_data->points_per_channel;
         void *entry;
 
-        hdr = kzalloc(sizeof(*hdr) + payload_len +
+        hdr = kzalloc(sizeof(*hdr) + sizeof(*chan) +
                       priv->tx_hdr_len, GFP_KERNEL);
         if (!hdr)
                 return -ENOMEM;
@@ -689,10 +733,10 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
         hdr->magic1 = cpu_to_le16(0x8001);
         hdr->len = cpu_to_le16(sizeof(*chan));
         hdr->type = cpu_to_le16(P54_CONTROL_TYPE_CHANNEL_CHANGE);
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*chan));
 
-        chan->magic1 = cpu_to_le16(0x1);
-        chan->magic2 = cpu_to_le16(0x0);
+        chan->flags = cpu_to_le16(0x1);
+        chan->dwell = cpu_to_le16(0x0);
 
         for (i = 0; i < priv->iq_autocal_len; i++) {
                 if (priv->iq_autocal[i].freq != freq)
@@ -710,35 +754,41 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
                         continue;
 
                 chan->val_barker = 0x38;
-                chan->val_bpsk = priv->output_limit[i].val_bpsk;
-                chan->val_qpsk = priv->output_limit[i].val_qpsk;
-                chan->val_16qam = priv->output_limit[i].val_16qam;
-                chan->val_64qam = priv->output_limit[i].val_64qam;
+                chan->val_bpsk = chan->dup_bpsk =
+                        priv->output_limit[i].val_bpsk;
+                chan->val_qpsk = chan->dup_qpsk =
+                        priv->output_limit[i].val_qpsk;
+                chan->val_16qam = chan->dup_16qam =
+                        priv->output_limit[i].val_16qam;
+                chan->val_64qam = chan->dup_64qam =
+                        priv->output_limit[i].val_64qam;
                 break;
         }
         if (i == priv->output_limit_len)
                 goto err;
 
-        chan->pa_points_per_curve = priv->curve_data->points_per_channel;
-
         entry = priv->curve_data->data;
         for (i = 0; i < priv->curve_data->channels; i++) {
                 if (*((__le16 *)entry) != freq) {
                         entry += sizeof(__le16);
-                        entry += sizeof(struct pda_pa_curve_data_sample_rev1) *
-                                 chan->pa_points_per_curve;
+                        entry += sizeof(struct p54_pa_curve_data_sample) *
+                                 priv->curve_data->points_per_channel;
                         continue;
                 }
 
                 entry += sizeof(__le16);
+                chan->pa_points_per_curve =
+                        min(priv->curve_data->points_per_channel, (u8) 8);
+
                 memcpy(chan->curve_data, entry, sizeof(*chan->curve_data) *
                        chan->pa_points_per_curve);
                 break;
         }
 
-        memcpy(hdr->data + payload_len - 4, &chan->val_bpsk, 4);
+        chan->rssical_mul = cpu_to_le16(130);
+        chan->rssical_add = cpu_to_le16(0xfe70);        /* -400 */
 
-        priv->tx(dev, hdr, sizeof(*hdr) + payload_len, 1);
+        priv->tx(dev, hdr, sizeof(*hdr) + sizeof(*chan), 1);
         return 0;
 
  err:
@@ -987,7 +1037,7 @@ static int p54_get_tx_stats(struct ieee80211_hw *dev,
 {
         struct p54_common *priv = dev->priv;
 
-        memcpy(stats, &priv->tx_stats, sizeof(stats[0]) * dev->queues);
+        memcpy(stats, &priv->tx_stats[4], sizeof(stats[0]) * dev->queues);
 
         return 0;
 }
@@ -1025,7 +1075,11 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
         dev->channel_change_time = 1000;        /* TODO: find actual value */
         dev->max_signal = 127;
 
-        priv->tx_stats[0].limit = 5;
+        priv->tx_stats[0].limit = 1;
+        priv->tx_stats[1].limit = 1;
+        priv->tx_stats[2].limit = 1;
+        priv->tx_stats[3].limit = 1;
+        priv->tx_stats[4].limit = 5;
         dev->queues = 1;
 
         dev->extra_tx_headroom = sizeof(struct p54_control_hdr) + 4 +
diff --git a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
index 9bc2a1cf4b57..a79c1a146917 100644
--- a/drivers/net/wireless/p54/p54common.h
+++ b/drivers/net/wireless/p54/p54common.h
@@ -89,6 +89,16 @@ struct pda_pa_curve_data_sample_rev1 {
         u8 data_qpsk;
         u8 data_16qam;
         u8 data_64qam;
+} __attribute__ ((packed));
+
+struct p54_pa_curve_data_sample {
+        u8 rf_power;
+        u8 pa_detector;
+        u8 data_barker;
+        u8 data_bpsk;
+        u8 data_qpsk;
+        u8 data_16qam;
+        u8 data_64qam;
         u8 padding;
 } __attribute__ ((packed));
 
@@ -212,8 +222,8 @@ struct p54_tx_control_filter {
 } __attribute__ ((packed));
 
 struct p54_tx_control_channel {
-        __le16 magic1;
-        __le16 magic2;
+        __le16 flags;
+        __le16 dwell;
         u8 padding1[20];
         struct pda_iq_autocal_entry iq_autocal;
         u8 pa_points_per_curve;
@@ -222,8 +232,13 @@ struct p54_tx_control_channel {
         u8 val_qpsk;
         u8 val_16qam;
         u8 val_64qam;
-        struct pda_pa_curve_data_sample_rev1 curve_data[0];
-        /* additional padding/data after curve_data */
+        struct pda_pa_curve_data_sample_rev1 curve_data[8];
+        u8 dup_bpsk;
+        u8 dup_qpsk;
+        u8 dup_16qam;
+        u8 dup_64qam;
+        __le16 rssical_mul;
+        __le16 rssical_add;
 } __attribute__ ((packed));
 
 struct p54_tx_control_led {
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index ea2dc3d93c4d..e9db4495c626 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -3,6 +3,7 @@
  * Linux device driver for PCI based Prism54
  *
  * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2008, Christian Lamparter <chunkeey@web.de>
  *
  * Based on the islsm (softmac prism54) driver, which is:
  * Copyright 2004-2006 Jean-Baptiste Note <jean-baptiste.note@m4x.org>, et al.
@@ -237,20 +238,22 @@ static int p54p_read_eeprom(struct ieee80211_hw *dev)
         return err;
 }
 
-static void p54p_refill_rx_ring(struct ieee80211_hw *dev)
+static void p54p_refill_rx_ring(struct ieee80211_hw *dev,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        struct sk_buff **rx_buf)
 {
         struct p54p_priv *priv = dev->priv;
         struct p54p_ring_control *ring_control = priv->ring_control;
-        u32 limit, host_idx, idx;
+        u32 limit, idx, i;
 
-        host_idx = le32_to_cpu(ring_control->host_idx[0]);
-        limit = host_idx;
-        limit -= le32_to_cpu(ring_control->device_idx[0]);
-        limit = ARRAY_SIZE(ring_control->rx_data) - limit;
+        idx = le32_to_cpu(ring_control->host_idx[ring_index]);
+        limit = idx;
+        limit -= le32_to_cpu(ring_control->device_idx[ring_index]);
+        limit = ring_limit - limit;
 
-        idx = host_idx % ARRAY_SIZE(ring_control->rx_data);
+        i = idx % ring_limit;
         while (limit-- > 1) {
-                struct p54p_desc *desc = &ring_control->rx_data[idx];
+                struct p54p_desc *desc = &ring[i];
 
                 if (!desc->host_addr) {
                         struct sk_buff *skb;
@@ -267,16 +270,106 @@ static void p54p_refill_rx_ring(struct ieee80211_hw *dev)
                         desc->device_addr = 0;  // FIXME: necessary?
                         desc->len = cpu_to_le16(MAX_RX_SIZE);
                         desc->flags = 0;
-                        priv->rx_buf[idx] = skb;
+                        rx_buf[i] = skb;
                 }
 
+                i++;
                 idx++;
-                host_idx++;
-                idx %= ARRAY_SIZE(ring_control->rx_data);
+                i %= ring_limit;
         }
 
         wmb();
-        ring_control->host_idx[0] = cpu_to_le32(host_idx);
+        ring_control->host_idx[ring_index] = cpu_to_le32(idx);
+}
+
+static void p54p_check_rx_ring(struct ieee80211_hw *dev, u32 *index,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        struct sk_buff **rx_buf)
+{
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+        struct p54p_desc *desc;
+        u32 idx, i;
+
+        i = (*index) % ring_limit;
+        (*index) = idx = le32_to_cpu(ring_control->device_idx[ring_index]);
+        idx %= ring_limit;
+        while (i != idx) {
+                u16 len;
+                struct sk_buff *skb;
+                desc = &ring[i];
+                len = le16_to_cpu(desc->len);
+                skb = rx_buf[i];
+
+                if (!skb)
+                        continue;
+
+                skb_put(skb, len);
+
+                if (p54_rx(dev, skb)) {
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+                        rx_buf[i] = NULL;
+                        desc->host_addr = 0;
+                } else {
+                        skb_trim(skb, 0);
+                        desc->len = cpu_to_le16(MAX_RX_SIZE);
+                }
+
+                i++;
+                i %= ring_limit;
+        }
+
+        p54p_refill_rx_ring(dev, ring_index, ring, ring_limit, rx_buf);
+}
+
+/* caller must hold priv->lock */
+static void p54p_check_tx_ring(struct ieee80211_hw *dev, u32 *index,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        void **tx_buf)
+{
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+        struct p54p_desc *desc;
+        u32 idx, i;
+
+        i = (*index) % ring_limit;
+        (*index) = idx = le32_to_cpu(ring_control->device_idx[1]);
+        idx %= ring_limit;
+
+        while (i != idx) {
+                desc = &ring[i];
+                kfree(tx_buf[i]);
+                tx_buf[i] = NULL;
+
+                pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+                                 le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+
+                desc->host_addr = 0;
+                desc->device_addr = 0;
+                desc->len = 0;
+                desc->flags = 0;
+
+                i++;
+                i %= ring_limit;
+        }
+}
+
+static void p54p_rx_tasklet(unsigned long dev_id)
+{
+        struct ieee80211_hw *dev = (struct ieee80211_hw *)dev_id;
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+
+        p54p_check_rx_ring(dev, &priv->rx_idx_mgmt, 2, ring_control->rx_mgmt,
+                ARRAY_SIZE(ring_control->rx_mgmt), priv->rx_buf_mgmt);
+
+        p54p_check_rx_ring(dev, &priv->rx_idx_data, 0, ring_control->rx_data,
+                ARRAY_SIZE(ring_control->rx_data), priv->rx_buf_data);
+
+        wmb();
+        P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
 }
 
 static irqreturn_t p54p_interrupt(int irq, void *dev_id)
@@ -298,65 +391,18 @@ static irqreturn_t p54p_interrupt(int irq, void *dev_id)
         reg &= P54P_READ(int_enable);
 
         if (reg & cpu_to_le32(ISL38XX_INT_IDENT_UPDATE)) {
-                struct p54p_desc *desc;
-                u32 idx, i;
-                i = priv->tx_idx;
-                i %= ARRAY_SIZE(ring_control->tx_data);
-                priv->tx_idx = idx = le32_to_cpu(ring_control->device_idx[1]);
-                idx %= ARRAY_SIZE(ring_control->tx_data);
-
-                while (i != idx) {
-                        desc = &ring_control->tx_data[i];
-                        if (priv->tx_buf[i]) {
-                                kfree(priv->tx_buf[i]);
-                                priv->tx_buf[i] = NULL;
-                        }
-
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
-                                         le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+                p54p_check_tx_ring(dev, &priv->tx_idx_mgmt,
+                                   3, ring_control->tx_mgmt,
+                                   ARRAY_SIZE(ring_control->tx_mgmt),
+                                   priv->tx_buf_mgmt);
 
-                        desc->host_addr = 0;
-                        desc->device_addr = 0;
-                        desc->len = 0;
-                        desc->flags = 0;
+                p54p_check_tx_ring(dev, &priv->tx_idx_data,
+                                   1, ring_control->tx_data,
+                                   ARRAY_SIZE(ring_control->tx_data),
+                                   priv->tx_buf_data);
 
-                        i++;
-                        i %= ARRAY_SIZE(ring_control->tx_data);
-                }
-
-                i = priv->rx_idx;
-                i %= ARRAY_SIZE(ring_control->rx_data);
-                priv->rx_idx = idx = le32_to_cpu(ring_control->device_idx[0]);
-                idx %= ARRAY_SIZE(ring_control->rx_data);
-                while (i != idx) {
-                        u16 len;
-                        struct sk_buff *skb;
-                        desc = &ring_control->rx_data[i];
-                        len = le16_to_cpu(desc->len);
-                        skb = priv->rx_buf[i];
-
-                        skb_put(skb, len);
-
-                        if (p54_rx(dev, skb)) {
-                                pci_unmap_single(priv->pdev,
-                                                 le32_to_cpu(desc->host_addr),
-                                                 MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
-
-                                priv->rx_buf[i] = NULL;
-                                desc->host_addr = 0;
-                        } else {
-                                skb_trim(skb, 0);
-                                desc->len = cpu_to_le16(MAX_RX_SIZE);
-                        }
-
-                        i++;
-                        i %= ARRAY_SIZE(ring_control->rx_data);
-                }
+                tasklet_schedule(&priv->rx_tasklet);
 
-                p54p_refill_rx_ring(dev);
-
-                wmb();
-                P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
         } else if (reg & cpu_to_le32(ISL38XX_INT_IDENT_INIT))
                 complete(&priv->boot_comp);
 
@@ -392,7 +438,7 @@ static void p54p_tx(struct ieee80211_hw *dev, struct p54_control_hdr *data,
         ring_control->host_idx[1] = cpu_to_le32(idx + 1);
 
         if (free_on_tx)
-                priv->tx_buf[i] = data;
+                priv->tx_buf_data[i] = data;
 
         spin_unlock_irqrestore(&priv->lock, flags);
 
@@ -420,8 +466,14 @@ static int p54p_open(struct ieee80211_hw *dev)
         }
 
         memset(priv->ring_control, 0, sizeof(*priv->ring_control));
-        priv->rx_idx = priv->tx_idx = 0;
-        p54p_refill_rx_ring(dev);
+        priv->rx_idx_data = priv->tx_idx_data = 0;
+        priv->rx_idx_mgmt = priv->tx_idx_mgmt = 0;
+
+        p54p_refill_rx_ring(dev, 0, priv->ring_control->rx_data,
+                ARRAY_SIZE(priv->ring_control->rx_data), priv->rx_buf_data);
+
+        p54p_refill_rx_ring(dev, 2, priv->ring_control->rx_mgmt,
+                ARRAY_SIZE(priv->ring_control->rx_mgmt), priv->rx_buf_mgmt);
 
         p54p_upload_firmware(dev);
 
@@ -465,6 +517,8 @@ static void p54p_stop(struct ieee80211_hw *dev)
         unsigned int i;
         struct p54p_desc *desc;
 
+        tasklet_kill(&priv->rx_tasklet);
+
         P54P_WRITE(int_enable, cpu_to_le32(0));
         P54P_READ(int_enable);
         udelay(10);
@@ -473,26 +527,51 @@ static void p54p_stop(struct ieee80211_hw *dev)
 
         P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
 
-        for (i = 0; i < ARRAY_SIZE(priv->rx_buf); i++) {
+        for (i = 0; i < ARRAY_SIZE(priv->rx_buf_data); i++) {
                 desc = &ring_control->rx_data[i];
                 if (desc->host_addr)
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
                                          MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
-                kfree_skb(priv->rx_buf[i]);
-                priv->rx_buf[i] = NULL;
+                kfree_skb(priv->rx_buf_data[i]);
+                priv->rx_buf_data[i] = NULL;
         }
 
-        for (i = 0; i < ARRAY_SIZE(priv->tx_buf); i++) {
+        for (i = 0; i < ARRAY_SIZE(priv->rx_buf_mgmt); i++) {
+                desc = &ring_control->rx_mgmt[i];
+                if (desc->host_addr)
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+                kfree_skb(priv->rx_buf_mgmt[i]);
+                priv->rx_buf_mgmt[i] = NULL;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(priv->tx_buf_data); i++) {
                 desc = &ring_control->tx_data[i];
                 if (desc->host_addr)
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
-                                         le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         le16_to_cpu(desc->len),
+                                         PCI_DMA_TODEVICE);
+
+                kfree(priv->tx_buf_data[i]);
+                priv->tx_buf_data[i] = NULL;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(priv->tx_buf_mgmt); i++) {
+                desc = &ring_control->tx_mgmt[i];
+                if (desc->host_addr)
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         le16_to_cpu(desc->len),
+                                         PCI_DMA_TODEVICE);
 
-                kfree(priv->tx_buf[i]);
-                priv->tx_buf[i] = NULL;
+                kfree(priv->tx_buf_mgmt[i]);
+                priv->tx_buf_mgmt[i] = NULL;
         }
 
-        memset(ring_control, 0, sizeof(ring_control));
+        memset(ring_control, 0, sizeof(*ring_control));
 }
 
 static int __devinit p54p_probe(struct pci_dev *pdev,
@@ -585,6 +664,7 @@ static int __devinit p54p_probe(struct pci_dev *pdev,
         priv->common.tx = p54p_tx;
 
         spin_lock_init(&priv->lock);
+        tasklet_init(&priv->rx_tasklet, p54p_rx_tasklet, (unsigned long)dev);
 
         err = ieee80211_register_hw(dev);
         if (err) {
diff --git a/drivers/net/wireless/p54/p54pci.h b/drivers/net/wireless/p54/p54pci.h
index 07678ef5ddc8..4a6778070afc 100644
--- a/drivers/net/wireless/p54/p54pci.h
+++ b/drivers/net/wireless/p54/p54pci.h
@@ -92,13 +92,17 @@ struct p54p_priv {
         struct p54_common common;
         struct pci_dev *pdev;
         struct p54p_csr __iomem *map;
+        struct tasklet_struct rx_tasklet;
 
         spinlock_t lock;
         struct p54p_ring_control *ring_control;
         dma_addr_t ring_control_dma;
-        u32 rx_idx, tx_idx;
-        struct sk_buff *rx_buf[8];
-        void *tx_buf[32];
+        u32 rx_idx_data, tx_idx_data;
+        u32 rx_idx_mgmt, tx_idx_mgmt;
+        struct sk_buff *rx_buf_data[8];
+        struct sk_buff *rx_buf_mgmt[4];
+        void *tx_buf_data[32];
+        void *tx_buf_mgmt[4];
         struct completion boot_comp;
 };
 
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 0107cec18b26..18b703c3fc2c 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -1241,7 +1241,7 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
         if (!reg)
                 return IRQ_NONE;
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return IRQ_HANDLED;
 
         /*
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index e0ff76ff490d..2a96a011f2ad 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -1316,6 +1316,8 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
+        else
+                rxdesc->dev_flags |= RXDONE_SIGNAL_BITRATE;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 }
@@ -1377,7 +1379,7 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
         if (!reg)
                 return IRQ_NONE;
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return IRQ_HANDLED;
 
         /*
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 3b90ed622148..0e008b606f70 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1114,8 +1114,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
                            test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
-        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT,
-                           skb->len - skbdesc->desc_len);
+        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skb->len);
         rt2x00_set_field32(&word, TXD_W0_CIPHER, CIPHER_NONE);
         rt2x00_desc_write(txd, 0, word);
 }
@@ -1280,6 +1279,8 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
+        else
+                rxdesc->dev_flags |= RXDONE_SIGNAL_BITRATE;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 
@@ -1297,7 +1298,7 @@ static void rt2500usb_beacondone(struct urb *urb)
         struct queue_entry *entry = (struct queue_entry *)urb->context;
         struct queue_entry_priv_usb_bcn *bcn_priv = entry->priv_data;
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &entry->queue->rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &entry->queue->rt2x00dev->flags))
                 return;
 
         /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 3fa3d5b006a6..6f296cef76ad 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -44,7 +44,7 @@
 /*
  * Module information.
  */
-#define DRV_VERSION     "2.2.0"
+#define DRV_VERSION     "2.2.1"
 #define DRV_PROJECT     "http://rt2x00.serialmonkey.com"
 
 /*
@@ -629,14 +629,13 @@ enum rt2x00_flags {
         /*
          * Device state flags
          */
-        DEVICE_PRESENT,
-        DEVICE_REGISTERED_HW,
-        DEVICE_INITIALIZED,
-        DEVICE_STARTED,
-        DEVICE_STARTED_SUSPEND,
-        DEVICE_ENABLED_RADIO,
-        DEVICE_DISABLED_RADIO_HW,
-        DEVICE_DIRTY_CONFIG,
+        DEVICE_STATE_PRESENT,
+        DEVICE_STATE_REGISTERED_HW,
+        DEVICE_STATE_INITIALIZED,
+        DEVICE_STATE_STARTED,
+        DEVICE_STATE_STARTED_SUSPEND,
+        DEVICE_STATE_ENABLED_RADIO,
+        DEVICE_STATE_DISABLED_RADIO_HW,
 
         /*
          * Driver requirements
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index ea37c7962043..ca051f50ef10 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -121,7 +121,7 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
          * Antenna setup changes require the RX to be disabled,
          * else the changes will be ignored by the device.
          */
-        if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF_LINK);
 
         /*
@@ -136,7 +136,7 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
         rt2x00dev->link.ant.active.rx = libconf.ant.rx;
         rt2x00dev->link.ant.active.tx = libconf.ant.tx;
 
-        if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON_LINK);
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 328ff8bc4c16..369b0b2d8643 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -34,7 +34,7 @@
  */
 void rt2x00lib_reset_link_tuner(struct rt2x00_dev *rt2x00dev)
 {
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return;
 
         /*
@@ -94,8 +94,8 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
          * Don't enable the radio twice.
          * And check if the hardware button has been disabled.
          */
-        if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
-            test_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags) ||
+            test_bit(DEVICE_STATE_DISABLED_RADIO_HW, &rt2x00dev->flags))
                 return 0;
 
         /*
@@ -117,7 +117,7 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
         rt2x00leds_led_radio(rt2x00dev, true);
         rt2x00led_led_activity(rt2x00dev, true);
 
-        __set_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags);
 
         /*
          * Enable RX.
@@ -134,7 +134,7 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
 
 void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-        if (!__test_and_clear_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_and_clear_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return;
 
         /*
@@ -354,7 +354,7 @@ static void rt2x00lib_link_tuner(struct work_struct *work)
          * When the radio is shutting down we should
          * immediately cease all link tuning.
          */
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return;
 
         /*
@@ -431,7 +431,7 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
          * note that in the spinlock protected area above the delayed_flags
          * have been cleared correctly.
          */
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return;
 
         if (delayed_flags & DELAYED_UPDATE_BEACON)
@@ -484,7 +484,7 @@ static void rt2x00lib_beacondone_iter(void *data, u8 *mac,
 
 void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
 {
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return;
 
         ieee80211_iterate_active_interfaces_atomic(rt2x00dev->hw,
@@ -572,7 +572,7 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 
         rt2x00dev->ops->lib->init_txentry(rt2x00dev, entry);
 
-        __clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+        clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
         rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
 
         /*
@@ -653,7 +653,7 @@ void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
 
                 if (((rxdesc.dev_flags & RXDONE_SIGNAL_PLCP) &&
                      (rate->plcp == rxdesc.signal)) ||
-                    (!(rxdesc.dev_flags & RXDONE_SIGNAL_PLCP) &&
+                    ((rxdesc.dev_flags & RXDONE_SIGNAL_BITRATE) &&
                       (rate->bitrate == rxdesc.signal))) {
                         idx = i;
                         break;
@@ -888,7 +888,7 @@ static int rt2x00lib_probe_hw_modes(struct rt2x00_dev *rt2x00dev,
 
 static void rt2x00lib_remove_hw(struct rt2x00_dev *rt2x00dev)
 {
-        if (test_bit(DEVICE_REGISTERED_HW, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_REGISTERED_HW, &rt2x00dev->flags))
                 ieee80211_unregister_hw(rt2x00dev->hw);
 
         if (likely(rt2x00dev->hw->wiphy->bands[IEEE80211_BAND_2GHZ])) {
@@ -906,6 +906,9 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
         struct hw_mode_spec *spec = &rt2x00dev->spec;
         int status;
 
+        if (test_bit(DEVICE_STATE_REGISTERED_HW, &rt2x00dev->flags))
+                return 0;
+
         /*
          * Initialize HW modes.
          */
@@ -927,7 +930,7 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
                 return status;
         }
 
-        __set_bit(DEVICE_REGISTERED_HW, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_REGISTERED_HW, &rt2x00dev->flags);
 
         return 0;
 }
@@ -937,7 +940,7 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
  */
 static void rt2x00lib_uninitialize(struct rt2x00_dev *rt2x00dev)
 {
-        if (!__test_and_clear_bit(DEVICE_INITIALIZED, &rt2x00dev->flags))
+        if (!test_and_clear_bit(DEVICE_STATE_INITIALIZED, &rt2x00dev->flags))
                 return;
 
         /*
@@ -960,7 +963,7 @@ static int rt2x00lib_initialize(struct rt2x00_dev *rt2x00dev)
 {
         int status;
 
-        if (test_bit(DEVICE_INITIALIZED, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_INITIALIZED, &rt2x00dev->flags))
                 return 0;
 
         /*
@@ -979,7 +982,7 @@ static int rt2x00lib_initialize(struct rt2x00_dev *rt2x00dev)
                 return status;
         }
 
-        __set_bit(DEVICE_INITIALIZED, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_INITIALIZED, &rt2x00dev->flags);
 
         /*
          * Register the extra components.
@@ -993,7 +996,7 @@ int rt2x00lib_start(struct rt2x00_dev *rt2x00dev)
 {
         int retval;
 
-        if (test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+        if (test_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags))
                 return 0;
 
         /*
@@ -1011,28 +1014,18 @@ int rt2x00lib_start(struct rt2x00_dev *rt2x00dev)
         if (retval)
                 return retval;
 
-        /*
-         * Enable radio.
-         */
-        retval = rt2x00lib_enable_radio(rt2x00dev);
-        if (retval) {
-                rt2x00lib_uninitialize(rt2x00dev);
-                return retval;
-        }
-
         rt2x00dev->intf_ap_count = 0;
         rt2x00dev->intf_sta_count = 0;
         rt2x00dev->intf_associated = 0;
 
-        __set_bit(DEVICE_STARTED, &rt2x00dev->flags);
-        __set_bit(DEVICE_DIRTY_CONFIG, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags);
 
         return 0;
 }
 
 void rt2x00lib_stop(struct rt2x00_dev *rt2x00dev)
 {
-        if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+        if (!test_and_clear_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags))
                 return;
 
         /*
@@ -1044,8 +1037,6 @@ void rt2x00lib_stop(struct rt2x00_dev *rt2x00dev)
         rt2x00dev->intf_ap_count = 0;
         rt2x00dev->intf_sta_count = 0;
         rt2x00dev->intf_associated = 0;
-
-        __clear_bit(DEVICE_STARTED, &rt2x00dev->flags);
 }
 
 /*
@@ -1100,7 +1091,7 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
         rt2x00rfkill_allocate(rt2x00dev);
         rt2x00debug_register(rt2x00dev);
 
-        __set_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags);
 
         return 0;
 
@@ -1113,7 +1104,7 @@ EXPORT_SYMBOL_GPL(rt2x00lib_probe_dev);
 
 void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 {
-        __clear_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+        clear_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags);
 
         /*
          * Disable radio.
@@ -1158,14 +1149,15 @@ int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
         int retval;
 
         NOTICE(rt2x00dev, "Going to sleep.\n");
-        __clear_bit(DEVICE_PRESENT, &rt2x00dev->flags);
 
         /*
          * Only continue if mac80211 has open interfaces.
          */
-        if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+        if (!test_and_clear_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) ||
+            !test_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags))
                 goto exit;
-        __set_bit(DEVICE_STARTED_SUSPEND, &rt2x00dev->flags);
+
+        set_bit(DEVICE_STATE_STARTED_SUSPEND, &rt2x00dev->flags);
 
         /*
          * Disable radio.
@@ -1237,7 +1229,7 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
         /*
          * Only continue if mac80211 had open interfaces.
          */
-        if (!__test_and_clear_bit(DEVICE_STARTED_SUSPEND, &rt2x00dev->flags))
+        if (!test_and_clear_bit(DEVICE_STATE_STARTED_SUSPEND, &rt2x00dev->flags))
                 return 0;
 
         /*
@@ -1264,7 +1256,7 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
         /*
          * We are ready again to receive requests from mac80211.
          */
-        __set_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+        set_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags);
 
         /*
          * It is possible that during that mac80211 has attempted
@@ -1284,7 +1276,7 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
         return 0;
 
 exit:
-        rt2x00lib_disable_radio(rt2x00dev);
+        rt2x00lib_stop(rt2x00dev);
         rt2x00lib_uninitialize(rt2x00dev);
         rt2x00debug_deregister(rt2x00dev);
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 3af427339417..56829fad3471 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -117,7 +117,7 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
          * Note that we can only stop the TX queues inside the TX path
          * due to possible race conditions in mac80211.
          */
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
                 goto exit_fail;
 
         /*
@@ -175,7 +175,7 @@ int rt2x00mac_start(struct ieee80211_hw *hw)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
 
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
                 return 0;
 
         return rt2x00lib_start(rt2x00dev);
@@ -186,7 +186,7 @@ void rt2x00mac_stop(struct ieee80211_hw *hw)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
 
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
                 return;
 
         rt2x00lib_stop(rt2x00dev);
@@ -206,8 +206,8 @@ int rt2x00mac_add_interface(struct ieee80211_hw *hw,
          * Don't allow interfaces to be added
          * the device has disappeared.
          */
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags) ||
-            !test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) ||
+            !test_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags))
                 return -ENODEV;
 
         switch (conf->type) {
@@ -256,7 +256,7 @@ int rt2x00mac_add_interface(struct ieee80211_hw *hw,
          */
         for (i = 0; i < queue->limit; i++) {
                 entry = &queue->entries[i];
-                if (!__test_and_set_bit(ENTRY_BCN_ASSIGNED, &entry->flags))
+                if (!test_and_set_bit(ENTRY_BCN_ASSIGNED, &entry->flags))
                         break;
         }
 
@@ -310,7 +310,7 @@ void rt2x00mac_remove_interface(struct ieee80211_hw *hw,
          * either the device has disappeared or when
          * no interface is present.
          */
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags) ||
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) ||
             (conf->type == IEEE80211_IF_TYPE_AP && !rt2x00dev->intf_ap_count) ||
             (conf->type != IEEE80211_IF_TYPE_AP && !rt2x00dev->intf_sta_count))
                 return;
@@ -324,7 +324,7 @@ void rt2x00mac_remove_interface(struct ieee80211_hw *hw,
          * Release beacon entry so it is available for
          * new interfaces again.
          */
-        __clear_bit(ENTRY_BCN_ASSIGNED, &intf->beacon->flags);
+        clear_bit(ENTRY_BCN_ASSIGNED, &intf->beacon->flags);
 
         /*
          * Make sure the bssid and mac address registers
@@ -338,45 +338,45 @@ EXPORT_SYMBOL_GPL(rt2x00mac_remove_interface);
 int rt2x00mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
-        int force_reconfig;
+        int radio_on;
+        int status;
 
         /*
          * Mac80211 might be calling this function while we are trying
          * to remove the device or perhaps suspending it.
          */
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
                 return 0;
 
         /*
-         * Check if we need to disable the radio,
-         * if this is not the case, at least the RX must be disabled.
+         * Only change device state when the radio is enabled. It does not
+         * matter what parameters we have configured when the radio is disabled
+         * because we won't be able to send or receive anyway. Also note that
+         * some configuration parameters (e.g. channel and antenna values) can
+         * only be set when the radio is enabled.
          */
-        if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags)) {
-                if (!conf->radio_enabled)
-                        rt2x00lib_disable_radio(rt2x00dev);
-                else
-                        rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
-        }
+        radio_on = test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags);
+        if (conf->radio_enabled) {
+                /* For programming the values, we have to turn RX off */
+                rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
 
-        /*
-         * When the DEVICE_DIRTY_CONFIG flag is set, the device has recently
-         * been started and the configuration must be forced upon the hardware.
-         * Otherwise registers will not be intialized correctly and could
-         * result in non-working hardware because essential registers aren't
-         * initialized.
-         */
-        force_reconfig =
-            __test_and_clear_bit(DEVICE_DIRTY_CONFIG, &rt2x00dev->flags);
+                /* Enable the radio */
+                status = rt2x00lib_enable_radio(rt2x00dev);
+                if (unlikely(status))
+                        return status;
 
-        rt2x00lib_config(rt2x00dev, conf, force_reconfig);
+                /*
+                 * When we've just turned on the radio, we want to reprogram
+                 * everything to ensure a consistent state
+                 */
+                rt2x00lib_config(rt2x00dev, conf, !radio_on);
 
-        /*
-         * Reenable RX only if the radio should be on.
-         */
-        if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+                /* Turn RX back on */
                 rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-        else if (conf->radio_enabled)
-                return rt2x00lib_enable_radio(rt2x00dev);
+        } else {
+                /* Disable the radio */
+                rt2x00lib_disable_radio(rt2x00dev);
+        }
 
         return 0;
 }
@@ -395,7 +395,7 @@ int rt2x00mac_config_interface(struct ieee80211_hw *hw,
          * Mac80211 might be calling this function while we are trying
          * to remove the device or perhaps suspending it.
          */
-        if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
                 return 0;
 
         spin_lock(&intf->lock);
@@ -666,10 +666,11 @@ int rt2x00mac_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
                 queue->cw_max = 10; /* cw_min: 2^10 = 1024. */
 
         queue->aifs = params->aifs;
+        queue->txop = params->txop;
 
         INFO(rt2x00dev,
-             "Configured TX queue %d - CWmin: %d, CWmax: %d, Aifs: %d.\n",
-             queue_idx, queue->cw_min, queue->cw_max, queue->aifs);
+             "Configured TX queue %d - CWmin: %d, CWmax: %d, Aifs: %d, TXop: %d.\n",
+             queue_idx, queue->cw_min, queue->cw_max, queue->aifs, queue->txop);
 
         return 0;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index c0f97c53e5ce..a5e965068c83 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -100,8 +100,21 @@ void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
 
-        skbdesc->skb_dma = dma_map_single(rt2x00dev->dev, skb->data, skb->len,
-                                          DMA_TO_DEVICE);
+        /*
+         * If device has requested headroom, we should make sure that
+         * is also mapped to the DMA so it can be used for transfering
+         * additional descriptor information to the hardware.
+         */
+        skb_push(skb, rt2x00dev->hw->extra_tx_headroom);
+
+        skbdesc->skb_dma =
+            dma_map_single(rt2x00dev->dev, skb->data, skb->len, DMA_TO_DEVICE);
+
+        /*
+         * Restore data pointer to original location again.
+         */
+        skb_pull(skb, rt2x00dev->hw->extra_tx_headroom);
+
         skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
 }
 EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);
@@ -117,7 +130,12 @@ void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
         }
 
         if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
-                dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
+                /*
+                 * Add headroom to the skb length, it has been removed
+                 * by the driver, but it was actually mapped to DMA.
+                 */
+                dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma,
+                                 skb->len + rt2x00dev->hw->extra_tx_headroom,
                                  DMA_TO_DEVICE);
                 skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
         }
@@ -356,7 +374,7 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         if (unlikely(rt2x00queue_full(queue)))
                 return -EINVAL;
 
-        if (__test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
+        if (test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
                 ERROR(queue->rt2x00dev,
                       "Arrived at non-free entry in the non-full queue %d.\n"
                       "Please file bug report to %s.\n",
@@ -396,7 +414,7 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
          * the frame to mac80211 because the skb->cb has now been tainted.
          */
         if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
-                __clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+                clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
                 dev_kfree_skb_any(entry->skb);
                 entry->skb = NULL;
                 return 0;
@@ -405,7 +423,7 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
         if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
                 rt2x00queue_map_txskb(queue->rt2x00dev, skb);
 
-        __set_bit(ENTRY_DATA_PENDING, &entry->flags);
+        set_bit(ENTRY_DATA_PENDING, &entry->flags);
 
         rt2x00queue_index_inc(queue, Q_INDEX);
         rt2x00queue_write_tx_descriptor(entry, &txdesc);
@@ -718,6 +736,7 @@ static void rt2x00queue_init(struct rt2x00_dev *rt2x00dev,
 
         queue->rt2x00dev = rt2x00dev;
         queue->qid = qid;
+        queue->txop = 0;
         queue->aifs = 2;
         queue->cw_min = 5;
         queue->cw_max = 10;
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 37f3f98d58a2..9dbf04f0f04c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -140,13 +140,14 @@ static inline struct skb_frame_desc* get_skb_frame_desc(struct sk_buff *skb)
 /**
  * enum rxdone_entry_desc_flags: Flags for &struct rxdone_entry_desc
  *
- * @RXDONE_SIGNAL_PLCP: Does the signal field contain the plcp value,
- *      or does it contain the bitrate itself.
+ * @RXDONE_SIGNAL_PLCP: Signal field contains the plcp value.
+ * @RXDONE_SIGNAL_BITRATE: Signal field contains the bitrate value.
  * @RXDONE_MY_BSS: Does this frame originate from device's BSS.
  */
 enum rxdone_entry_desc_flags {
         RXDONE_SIGNAL_PLCP = 1 << 0,
-        RXDONE_MY_BSS = 1 << 1,
+        RXDONE_SIGNAL_BITRATE = 1 << 1,
+        RXDONE_MY_BSS = 1 << 2,
 };
 
 /**
@@ -368,6 +369,7 @@ enum queue_index {
  * @length: Number of frames in queue.
  * @index: Index pointers to entry positions in the queue,
  *      use &enum queue_index to get a specific index field.
+ * @txop: maximum burst time.
  * @aifs: The aifs value for outgoing frames (field ignored in RX queue).
  * @cw_min: The cw min value for outgoing frames (field ignored in RX queue).
  * @cw_max: The cw max value for outgoing frames (field ignored in RX queue).
@@ -387,6 +389,7 @@ struct data_queue {
         unsigned short length;
         unsigned short index[Q_INDEX_MAX];
 
+        unsigned short txop;
         unsigned short aifs;
         unsigned short cw_min;
         unsigned short cw_max;
diff --git a/drivers/net/wireless/rt2x00/rt2x00rfkill.c b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
index 04b29716d356..8a2fefb365b7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00rfkill.c
+++ b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
@@ -41,16 +41,16 @@ static int rt2x00rfkill_toggle_radio(void *data, enum rfkill_state state)
         /*
          * Only continue if there are enabled interfaces.
          */
-        if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_STARTED, &rt2x00dev->flags))
                 return 0;
 
         if (state == RFKILL_STATE_UNBLOCKED) {
                 INFO(rt2x00dev, "Hardware button pressed, enabling radio.\n");
-                __clear_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags);
+                clear_bit(DEVICE_STATE_DISABLED_RADIO_HW, &rt2x00dev->flags);
                 retval = rt2x00lib_enable_radio(rt2x00dev);
         } else if (state == RFKILL_STATE_SOFT_BLOCKED) {
                 INFO(rt2x00dev, "Hardware button pressed, disabling radio.\n");
-                __set_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags);
+                set_bit(DEVICE_STATE_DISABLED_RADIO_HW, &rt2x00dev->flags);
                 rt2x00lib_disable_radio(rt2x00dev);
         } else {
                 WARNING(rt2x00dev, "Received unexpected rfkill state %d.\n",
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 2050227ea530..b73a7e0aeed4 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -163,16 +163,11 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
         struct txdone_entry_desc txdesc;
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags) ||
             !test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
                 return;
 
         /*
-         * Remove the descriptor data from the buffer.
-         */
-        skb_pull(entry->skb, entry->queue->desc_size);
-
-        /*
          * Obtain the status about this packet.
          * Note that when the status is 0 it does not mean the
          * frame was send out correctly. It only means the frame
@@ -224,6 +219,12 @@ int rt2x00usb_write_tx_data(struct queue_entry *entry)
                           entry->skb->data, length,
                           rt2x00usb_interrupt_txdone, entry);
 
+        /*
+         * Make sure the skb->data pointer points to the frame, not the
+         * descriptor.
+         */
+        skb_pull(entry->skb, entry->queue->desc_size);
+
         return 0;
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_write_tx_data);
@@ -232,7 +233,7 @@ static inline void rt2x00usb_kick_tx_entry(struct queue_entry *entry)
 {
         struct queue_entry_priv_usb *entry_priv = entry->priv_data;
 
-        if (__test_and_clear_bit(ENTRY_DATA_PENDING, &entry->flags))
+        if (test_and_clear_bit(ENTRY_DATA_PENDING, &entry->flags))
                 usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
 }
 
@@ -283,7 +284,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
         u8 rxd[32];
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags) ||
             !test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
                 return;
 
@@ -293,7 +294,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
          * a problem.
          */
         if (urb->actual_length < entry->queue->desc_size || urb->status) {
-                __set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+                set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
                 usb_submit_urb(urb, GFP_ATOMIC);
                 return;
         }
@@ -361,7 +362,7 @@ void rt2x00usb_init_rxentry(struct rt2x00_dev *rt2x00dev,
                           entry->skb->data, entry->skb->len,
                           rt2x00usb_interrupt_rxdone, entry);
 
-        __set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+        set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
         usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_init_rxentry);
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index 23cf93dfda06..d740f560ccd0 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -1478,16 +1478,6 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 
         rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
 
-        rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC0_TX_OP, 0);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC1_TX_OP, 0);
-        rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
-
-        rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC2_TX_OP, 192);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC3_TX_OP, 48);
-        rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
-
         /*
          * Clear all beacons
          * For the Beacon base registers we only need to clear
@@ -2001,6 +1991,8 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
+        else
+                rxdesc->dev_flags |= RXDONE_SIGNAL_BITRATE;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 }
@@ -2121,7 +2113,7 @@ static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
         if (!reg && !reg_mcu)
                 return IRQ_NONE;
 
-        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
                 return IRQ_HANDLED;
 
         /*
@@ -2652,6 +2644,63 @@ static int rt61pci_set_retry_limit(struct ieee80211_hw *hw,
         return 0;
 }
 
+static int rt61pci_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
+                           const struct ieee80211_tx_queue_params *params)
+{
+        struct rt2x00_dev *rt2x00dev = hw->priv;
+        struct data_queue *queue;
+        struct rt2x00_field32 field;
+        int retval;
+        u32 reg;
+
+        /*
+         * First pass the configuration through rt2x00lib, that will
+         * update the queue settings and validate the input. After that
+         * we are free to update the registers based on the value
+         * in the queue parameter.
+         */
+        retval = rt2x00mac_conf_tx(hw, queue_idx, params);
+        if (retval)
+                return retval;
+
+        queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+
+        /* Update WMM TXOP register */
+        if (queue_idx < 2) {
+                field.bit_offset = queue_idx * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
+        } else if (queue_idx < 4) {
+                field.bit_offset = (queue_idx - 2) * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
+        }
+
+        /* Update WMM registers */
+        field.bit_offset = queue_idx * 4;
+        field.bit_mask = 0xf << field.bit_offset;
+
+        rt2x00pci_register_read(rt2x00dev, AIFSN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->aifs);
+        rt2x00pci_register_write(rt2x00dev, AIFSN_CSR, reg);
+
+        rt2x00pci_register_read(rt2x00dev, CWMIN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_min);
+        rt2x00pci_register_write(rt2x00dev, CWMIN_CSR, reg);
+
+        rt2x00pci_register_read(rt2x00dev, CWMAX_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_max);
+        rt2x00pci_register_write(rt2x00dev, CWMAX_CSR, reg);
+
+        return 0;
+}
+
 static u64 rt61pci_get_tsf(struct ieee80211_hw *hw)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -2679,7 +2728,7 @@ static const struct ieee80211_ops rt61pci_mac80211_ops = {
         .get_stats              = rt2x00mac_get_stats,
         .set_retry_limit        = rt61pci_set_retry_limit,
         .bss_info_changed       = rt2x00mac_bss_info_changed,
-        .conf_tx                = rt2x00mac_conf_tx,
+        .conf_tx                = rt61pci_conf_tx,
         .get_tx_stats           = rt2x00mac_get_tx_stats,
         .get_tsf                = rt61pci_get_tsf,
 };
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index f58fd059c9a0..e698ae0efbce 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -1277,16 +1277,6 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
         rt73usb_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
         rt73usb_register_write(rt2x00dev, PHY_CSR7, 0x00000408);
 
-        rt73usb_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC0_TX_OP, 0);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC1_TX_OP, 0);
-        rt73usb_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
-
-        rt73usb_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC2_TX_OP, 192);
-        rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC3_TX_OP, 48);
-        rt73usb_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
-
         rt73usb_register_read(rt2x00dev, MAC_CSR9, &reg);
         rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
         rt73usb_register_write(rt2x00dev, MAC_CSR9, reg);
@@ -1566,8 +1556,7 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W0_KEY_TABLE,
                            test_bit(ENTRY_TXD_ENCRYPT_PAIRWISE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_KEY_INDEX, txdesc->key_idx);
-        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT,
-                           skb->len - skbdesc->desc_len);
+        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skb->len);
         rt2x00_set_field32(&word, TXD_W0_BURST2,
                            test_bit(ENTRY_TXD_BURST, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, txdesc->cipher);
@@ -1776,6 +1765,8 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
+        else
+                rxdesc->dev_flags |= RXDONE_SIGNAL_BITRATE;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 
@@ -2246,6 +2237,63 @@ static int rt73usb_set_retry_limit(struct ieee80211_hw *hw,
         return 0;
 }
 
+static int rt73usb_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
+                           const struct ieee80211_tx_queue_params *params)
+{
+        struct rt2x00_dev *rt2x00dev = hw->priv;
+        struct data_queue *queue;
+        struct rt2x00_field32 field;
+        int retval;
+        u32 reg;
+
+        /*
+         * First pass the configuration through rt2x00lib, that will
+         * update the queue settings and validate the input. After that
+         * we are free to update the registers based on the value
+         * in the queue parameter.
+         */
+        retval = rt2x00mac_conf_tx(hw, queue_idx, params);
+        if (retval)
+                return retval;
+
+        queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
+
+        /* Update WMM TXOP register */
+        if (queue_idx < 2) {
+                field.bit_offset = queue_idx * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt73usb_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt73usb_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
+        } else if (queue_idx < 4) {
+                field.bit_offset = (queue_idx - 2) * 16;
+                field.bit_mask = 0xffff << field.bit_offset;
+
+                rt73usb_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
+                rt2x00_set_field32(&reg, field, queue->txop);
+                rt73usb_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
+        }
+
+        /* Update WMM registers */
+        field.bit_offset = queue_idx * 4;
+        field.bit_mask = 0xf << field.bit_offset;
+
+        rt73usb_register_read(rt2x00dev, AIFSN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->aifs);
+        rt73usb_register_write(rt2x00dev, AIFSN_CSR, reg);
+
+        rt73usb_register_read(rt2x00dev, CWMIN_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_min);
+        rt73usb_register_write(rt2x00dev, CWMIN_CSR, reg);
+
+        rt73usb_register_read(rt2x00dev, CWMAX_CSR, &reg);
+        rt2x00_set_field32(&reg, field, queue->cw_max);
+        rt73usb_register_write(rt2x00dev, CWMAX_CSR, reg);
+
+        return 0;
+}
+
 #if 0
 /*
  * Mac80211 demands get_tsf must be atomic.
@@ -2283,7 +2331,7 @@ static const struct ieee80211_ops rt73usb_mac80211_ops = {
         .get_stats              = rt2x00mac_get_stats,
         .set_retry_limit        = rt73usb_set_retry_limit,
         .bss_info_changed       = rt2x00mac_bss_info_changed,
-        .conf_tx                = rt2x00mac_conf_tx,
+        .conf_tx                = rt73usb_conf_tx,
         .get_tx_stats           = rt2x00mac_get_tx_stats,
         .get_tsf                = rt73usb_get_tsf,
 };
diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c
index f883dcfffe06..d5cde051806b 100644
--- a/drivers/ssb/pci.c
+++ b/drivers/ssb/pci.c
@@ -327,11 +327,9 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
         s8 gain;
         u16 loc[3];
 
-        if (out->revision == 3) {                       /* rev 3 moved MAC */
+        if (out->revision == 3)                 /* rev 3 moved MAC */
                 loc[0] = SSB_SPROM3_IL0MAC;
-                loc[1] = SSB_SPROM3_ET0MAC;
-                loc[2] = SSB_SPROM3_ET1MAC;
-        } else {
+        else {
                 loc[0] = SSB_SPROM1_IL0MAC;
                 loc[1] = SSB_SPROM1_ET0MAC;
                 loc[2] = SSB_SPROM1_ET1MAC;
@@ -340,13 +338,15 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
                 v = in[SPOFF(loc[0]) + i];
                 *(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
         }
-        for (i = 0; i < 3; i++) {
-                v = in[SPOFF(loc[1]) + i];
-                *(((__be16 *)out->et0mac) + i) = cpu_to_be16(v);
-        }
-        for (i = 0; i < 3; i++) {
-                v = in[SPOFF(loc[2]) + i];
-                *(((__be16 *)out->et1mac) + i) = cpu_to_be16(v);
+        if (out->revision < 3) {        /* only rev 1-2 have et0, et1 */
+                for (i = 0; i < 3; i++) {
+                        v = in[SPOFF(loc[1]) + i];
+                        *(((__be16 *)out->et0mac) + i) = cpu_to_be16(v);
+                }
+                for (i = 0; i < 3; i++) {
+                        v = in[SPOFF(loc[2]) + i];
+                        *(((__be16 *)out->et1mac) + i) = cpu_to_be16(v);
+                }
         }
         SPEX(et0phyaddr, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET0A, 0);
         SPEX(et1phyaddr, SSB_SPROM1_ETHPHY, SSB_SPROM1_ETHPHY_ET1A,
@@ -399,30 +399,33 @@ static void sprom_extract_r123(struct ssb_sprom *out, const u16 *in)
         out->antenna_gain.ghz5.a3 = gain;
 }
 
-static void sprom_extract_r4(struct ssb_sprom *out, const u16 *in)
+static void sprom_extract_r45(struct ssb_sprom *out, const u16 *in)
 {
         int i;
         u16 v;
+        u16 il0mac_offset;
 
-        /* extract the equivalent of the r1 variables */
+        if (out->revision == 4)
+                il0mac_offset = SSB_SPROM4_IL0MAC;
+        else
+                il0mac_offset = SSB_SPROM5_IL0MAC;
+        /* extract the MAC address */
         for (i = 0; i < 3; i++) {
-                v = in[SPOFF(SSB_SPROM4_IL0MAC) + i];
+                v = in[SPOFF(il0mac_offset) + i];
                 *(((__be16 *)out->il0mac) + i) = cpu_to_be16(v);
         }
-        for (i = 0; i < 3; i++) {
-                v = in[SPOFF(SSB_SPROM4_ET0MAC) + i];
-                *(((__be16 *)out->et0mac) + i) = cpu_to_be16(v);
-        }
-        for (i = 0; i < 3; i++) {
-                v = in[SPOFF(SSB_SPROM4_ET1MAC) + i];
-                *(((__be16 *)out->et1mac) + i) = cpu_to_be16(v);
-        }
         SPEX(et0phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET0A, 0);
         SPEX(et1phyaddr, SSB_SPROM4_ETHPHY, SSB_SPROM4_ETHPHY_ET1A,
              SSB_SPROM4_ETHPHY_ET1A_SHIFT);
-        SPEX(country_code, SSB_SPROM4_CCODE, 0xFFFF, 0);
-        SPEX(boardflags_lo, SSB_SPROM4_BFLLO, 0xFFFF, 0);
-        SPEX(boardflags_hi, SSB_SPROM4_BFLHI, 0xFFFF, 0);
+        if (out->revision == 4) {
+                SPEX(country_code, SSB_SPROM4_CCODE, 0xFFFF, 0);
+                SPEX(boardflags_lo, SSB_SPROM4_BFLLO, 0xFFFF, 0);
+                SPEX(boardflags_hi, SSB_SPROM4_BFLHI, 0xFFFF, 0);
+        } else {
+                SPEX(country_code, SSB_SPROM5_CCODE, 0xFFFF, 0);
+                SPEX(boardflags_lo, SSB_SPROM5_BFLLO, 0xFFFF, 0);
+                SPEX(boardflags_hi, SSB_SPROM5_BFLHI, 0xFFFF, 0);
+        }
         SPEX(ant_available_a, SSB_SPROM4_ANTAVAIL, SSB_SPROM4_ANTAVAIL_A,
              SSB_SPROM4_ANTAVAIL_A_SHIFT);
         SPEX(ant_available_bg, SSB_SPROM4_ANTAVAIL, SSB_SPROM4_ANTAVAIL_BG,
@@ -433,12 +436,21 @@ static void sprom_extract_r4(struct ssb_sprom *out, const u16 *in)
         SPEX(maxpwr_a, SSB_SPROM4_MAXP_A, SSB_SPROM4_MAXP_A_MASK, 0);
         SPEX(itssi_a, SSB_SPROM4_MAXP_A, SSB_SPROM4_ITSSI_A,
              SSB_SPROM4_ITSSI_A_SHIFT);
-        SPEX(gpio0, SSB_SPROM4_GPIOA, SSB_SPROM4_GPIOA_P0, 0);
-        SPEX(gpio1, SSB_SPROM4_GPIOA, SSB_SPROM4_GPIOA_P1,
-             SSB_SPROM4_GPIOA_P1_SHIFT);
-        SPEX(gpio2, SSB_SPROM4_GPIOB, SSB_SPROM4_GPIOB_P2, 0);
-        SPEX(gpio3, SSB_SPROM4_GPIOB, SSB_SPROM4_GPIOB_P3,
-             SSB_SPROM4_GPIOB_P3_SHIFT);
+        if (out->revision == 4) {
+                SPEX(gpio0, SSB_SPROM4_GPIOA, SSB_SPROM4_GPIOA_P0, 0);
+                SPEX(gpio1, SSB_SPROM4_GPIOA, SSB_SPROM4_GPIOA_P1,
+                     SSB_SPROM4_GPIOA_P1_SHIFT);
+                SPEX(gpio2, SSB_SPROM4_GPIOB, SSB_SPROM4_GPIOB_P2, 0);
+                SPEX(gpio3, SSB_SPROM4_GPIOB, SSB_SPROM4_GPIOB_P3,
+                     SSB_SPROM4_GPIOB_P3_SHIFT);
+        } else {
+                SPEX(gpio0, SSB_SPROM5_GPIOA, SSB_SPROM5_GPIOA_P0, 0);
+                SPEX(gpio1, SSB_SPROM5_GPIOA, SSB_SPROM5_GPIOA_P1,
+                     SSB_SPROM5_GPIOA_P1_SHIFT);
+                SPEX(gpio2, SSB_SPROM5_GPIOB, SSB_SPROM5_GPIOB_P2, 0);
+                SPEX(gpio3, SSB_SPROM5_GPIOB, SSB_SPROM5_GPIOB_P3,
+                     SSB_SPROM5_GPIOB_P3_SHIFT);
+        }
 
         /* Extract the antenna gain values. */
         SPEX(antenna_gain.ghz24.a0, SSB_SPROM4_AGAIN01,
@@ -462,6 +474,8 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
 
         out->revision = in[size - 1] & 0x00FF;
         ssb_dprintk(KERN_DEBUG PFX "SPROM revision %d detected.\n", out->revision);
+        memset(out->et0mac, 0xFF, 6);           /* preset et0 and et1 mac */
+        memset(out->et1mac, 0xFF, 6);
         if ((bus->chip_id & 0xFF00) == 0x4400) {
                 /* Workaround: The BCM44XX chip has a stupid revision
                  * number stored in the SPROM.
@@ -471,16 +485,16 @@ static int sprom_extract(struct ssb_bus *bus, struct ssb_sprom *out,
         } else if (bus->chip_id == 0x4321) {
                 /* the BCM4328 has a chipid == 0x4321 and a rev 4 SPROM */
                 out->revision = 4;
-                sprom_extract_r4(out, in);
+                sprom_extract_r45(out, in);
         } else {
                 if (out->revision == 0)
                         goto unsupported;
                 if (out->revision >= 1 && out->revision <= 3) {
                         sprom_extract_r123(out, in);
                 }
-                if (out->revision == 4)
-                        sprom_extract_r4(out, in);
-                if (out->revision >= 5)
+                if (out->revision == 4 || out->revision == 5)
+                        sprom_extract_r45(out, in);
+                if (out->revision > 5)
                         goto unsupported;
         }