45 files changed, 1687 insertions, 713 deletions
diff --git a/drivers/net/wireless/ath/ath5k/phy.c b/drivers/net/wireless/ath/ath5k/phy.c
index 78c26fdccad1..d673ab2f6cda 100644
--- a/drivers/net/wireless/ath/ath5k/phy.c
+++ b/drivers/net/wireless/ath/ath5k/phy.c
@@ -282,6 +282,34 @@ int ath5k_hw_phy_disable(struct ath5k_hw *ah)
        return 0;
 }
+/*
+ * Wait for synth to settle
+ */
+static void ath5k_hw_wait_for_synth(struct ath5k_hw *ah,
+                        struct ieee80211_channel *channel)
+{
+        /*
+         * On 5211+ read activation -> rx delay
+         * and use it (100ns steps).
+         */
+        if (ah->ah_version != AR5K_AR5210) {
+                u32 delay;
+                delay = ath5k_hw_reg_read(ah, AR5K_PHY_RX_DELAY) &
+                        AR5K_PHY_RX_DELAY_M;
+                delay = (channel->hw_value & CHANNEL_CCK) ?
+                        ((delay << 2) / 22) : (delay / 10);
+                if (ah->ah_bwmode == AR5K_BWMODE_10MHZ)
+                        delay = delay << 1;
+                if (ah->ah_bwmode == AR5K_BWMODE_5MHZ)
+                        delay = delay << 2;
+                /* XXX: /2 on turbo ? Let's be safe
+                 * for now */
+                udelay(100 + delay);
+        } else {
+                mdelay(1);
+        }
+}
 /**********************\
 * RF Gain optimization *
@@ -3237,6 +3265,13 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
                /* Failed */
                if (i >= 100)
                        return -EIO;
+                /* Set channel and wait for synth */
+                ret = ath5k_hw_channel(ah, channel);
+                if (ret)
+                        return ret;
+                ath5k_hw_wait_for_synth(ah, channel);
        }
        /*
@@ -3251,13 +3286,53 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
        if (ret)
                return ret;
+        /* Write OFDM timings on 5212*/
+        if (ah->ah_version == AR5K_AR5212 &&
+                channel->hw_value & CHANNEL_OFDM) {
+                ret = ath5k_hw_write_ofdm_timings(ah, channel);
+                if (ret)
+                        return ret;
+                /* Spur info is available only from EEPROM versions
+                 * greater than 5.3, but the EEPROM routines will use
+                 * static values for older versions */
+                if (ah->ah_mac_srev >= AR5K_SREV_AR5424)
+                        ath5k_hw_set_spur_mitigation_filter(ah,
+                                                            channel);
+        }
+        /* If we used fast channel switching
+         * we are done, release RF bus and
+         * fire up NF calibration.
+         *
+         * Note: Only NF calibration due to
+         * channel change, not AGC calibration
+         * since AGC is still running !
+         */
+        if (fast) {
+                /*
+                 * Release RF Bus grant
+                 */
+                AR5K_REG_DISABLE_BITS(ah, AR5K_PHY_RFBUS_REQ,
+                                    AR5K_PHY_RFBUS_REQ_REQUEST);
+                /*
+                 * Start NF calibration
+                 */
+                AR5K_REG_ENABLE_BITS(ah, AR5K_PHY_AGCCTL,
+                                        AR5K_PHY_AGCCTL_NF);
+                return ret;
+        }
        /*
         * For 5210 we do all initialization using
         * initvals, so we don't have to modify
         * any settings (5210 also only supports
         * a/aturbo modes)
         */
-        if ((ah->ah_version != AR5K_AR5210) && !fast) {
+        if (ah->ah_version != AR5K_AR5210) {
                /*
                 * Write initial RF gain settings
@@ -3276,22 +3351,6 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
                if (ret)
                        return ret;
-                /* Write OFDM timings on 5212*/
-                if (ah->ah_version == AR5K_AR5212 &&
-                        channel->hw_value & CHANNEL_OFDM) {
-                        ret = ath5k_hw_write_ofdm_timings(ah, channel);
-                        if (ret)
-                                return ret;
-                        /* Spur info is available only from EEPROM versions
-                         * greater than 5.3, but the EEPROM routines will use
-                         * static values for older versions */
-                        if (ah->ah_mac_srev >= AR5K_SREV_AR5424)
-                                ath5k_hw_set_spur_mitigation_filter(ah,
-                                                                    channel);
-                }
                /*Enable/disable 802.11b mode on 5111
                (enable 2111 frequency converter + CCK)*/
                if (ah->ah_radio == AR5K_RF5111) {
@@ -3322,47 +3381,20 @@ int ath5k_hw_phy_init(struct ath5k_hw *ah, struct ieee80211_channel *channel,
         */
        ath5k_hw_reg_write(ah, AR5K_PHY_ACT_ENABLE, AR5K_PHY_ACT);
+        ath5k_hw_wait_for_synth(ah, channel);
        /*
-         * On 5211+ read activation -> rx delay
+         * Perform ADC test to see if baseband is ready
-         * and use it.
+         * Set tx hold and check adc test register
         */
-        if (ah->ah_version != AR5K_AR5210) {
+        phy_tst1 = ath5k_hw_reg_read(ah, AR5K_PHY_TST1);
-                u32 delay;
+        ath5k_hw_reg_write(ah, AR5K_PHY_TST1_TXHOLD, AR5K_PHY_TST1);
-                delay = ath5k_hw_reg_read(ah, AR5K_PHY_RX_DELAY) &
+        for (i = 0; i <= 20; i++) {
-                        AR5K_PHY_RX_DELAY_M;
+                if (!(ath5k_hw_reg_read(ah, AR5K_PHY_ADC_TEST) & 0x10))
-                delay = (channel->hw_value & CHANNEL_CCK) ?
+                        break;
-                        ((delay << 2) / 22) : (delay / 10);
+                udelay(200);
-                if (ah->ah_bwmode == AR5K_BWMODE_10MHZ)
-                        delay = delay << 1;
-                if (ah->ah_bwmode == AR5K_BWMODE_5MHZ)
-                        delay = delay << 2;
-                /* XXX: /2 on turbo ? Let's be safe
-                 * for now */
-                udelay(100 + delay);
-        } else {
-                mdelay(1);
-        }
-        if (fast)
-                /*
-                 * Release RF Bus grant
-                 */
-                AR5K_REG_DISABLE_BITS(ah, AR5K_PHY_RFBUS_REQ,
-                                    AR5K_PHY_RFBUS_REQ_REQUEST);
-        else {
-                /*
-                 * Perform ADC test to see if baseband is ready
-                 * Set tx hold and check adc test register
-                 */
-                phy_tst1 = ath5k_hw_reg_read(ah, AR5K_PHY_TST1);
-                ath5k_hw_reg_write(ah, AR5K_PHY_TST1_TXHOLD, AR5K_PHY_TST1);
-                for (i = 0; i <= 20; i++) {
-                        if (!(ath5k_hw_reg_read(ah, AR5K_PHY_ADC_TEST) & 0x10))
-                                break;
-                        udelay(200);
-                }
-                ath5k_hw_reg_write(ah, phy_tst1, AR5K_PHY_TST1);
        }
+        ath5k_hw_reg_write(ah, phy_tst1, AR5K_PHY_TST1);
        /*
         * Start automatic gain control calibration
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index bd85e311c51b..7c8409e53598 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -602,6 +602,8 @@ struct ath_softc {
        struct completion paprd_complete;
        bool paprd_pending;
+        unsigned int hw_busy_count;
        u32 intrstatus;
        u32 sc_flags; /* SC_OP_* */
        u16 ps_flags; /* PS_* */
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 87ba44c06692..fcb36abfc309 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -721,8 +721,9 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
                cur_conf->beacon_interval = 100;
        /*
-         * Some times we dont parse dtim period from mac80211, in that case
+         * We don't parse dtim period from mac80211 during the driver
-         * use a default value
+         * initialization as it breaks association with hidden-ssid
+         * AP and it causes latency in roaming
         */
        if (cur_conf->dtim_period == 0)
                cur_conf->dtim_period = 1;
diff --git a/drivers/net/wireless/ath/ath9k/common.c b/drivers/net/wireless/ath/ath9k/common.c
index df1998d48253..615e68276e72 100644
--- a/drivers/net/wireless/ath/ath9k/common.c
+++ b/drivers/net/wireless/ath/ath9k/common.c
@@ -189,6 +189,17 @@ void ath9k_cmn_btcoex_bt_stomp(struct ath_common *common,
 }
 EXPORT_SYMBOL(ath9k_cmn_btcoex_bt_stomp);
+void ath9k_cmn_update_txpow(struct ath_hw *ah, u16 cur_txpow,
+                            u16 new_txpow, u16 *txpower)
+{
+        if (cur_txpow != new_txpow) {
+                ath9k_hw_set_txpowerlimit(ah, new_txpow, false);
+                /* read back in case value is clamped */
+                *txpower = ath9k_hw_regulatory(ah)->power_limit;
+        }
+}
+EXPORT_SYMBOL(ath9k_cmn_update_txpow);
 static int __init ath9k_cmn_init(void)
 {
        return 0;
diff --git a/drivers/net/wireless/ath/ath9k/common.h b/drivers/net/wireless/ath/ath9k/common.h
index 4c7020b3a5a0..b2f7b5f89097 100644
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -68,3 +68,5 @@ struct ath9k_channel *ath9k_cmn_get_curchannel(struct ieee80211_hw *hw,
 int ath9k_cmn_count_streams(unsigned int chainmask, int max);
 void ath9k_cmn_btcoex_bt_stomp(struct ath_common *common,
                                  enum ath_stomp_type stomp_type);
+void ath9k_cmn_update_txpow(struct ath_hw *ah, u16 cur_txpow,
+                            u16 new_txpow, u16 *txpower);
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index 9cdc41b0ec44..5cfcf8c235a4 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -381,21 +381,40 @@ static const struct file_operations fops_interrupt = {
        .llseek = default_llseek,
 };
+static const char *channel_type_str(enum nl80211_channel_type t)
+{
+        switch (t) {
+        case NL80211_CHAN_NO_HT:
+                return "no ht";
+        case NL80211_CHAN_HT20:
+                return "ht20";
+        case NL80211_CHAN_HT40MINUS:
+                return "ht40-";
+        case NL80211_CHAN_HT40PLUS:
+                return "ht40+";
+        default:
+                return "???";
+        }
+}
 static ssize_t read_file_wiphy(struct file *file, char __user *user_buf,
                               size_t count, loff_t *ppos)
 {
        struct ath_softc *sc = file->private_data;
        struct ieee80211_channel *chan = sc->hw->conf.channel;
+        struct ieee80211_conf *conf = &(sc->hw->conf);
        char buf[512];
        unsigned int len = 0;
        u8 addr[ETH_ALEN];
        u32 tmp;
        len += snprintf(buf + len, sizeof(buf) - len,
-                        "%s (chan=%d ht=%d)\n",
+                        "%s (chan=%d  center-freq: %d MHz  channel-type: %d (%s))\n",
                        wiphy_name(sc->hw->wiphy),
                        ieee80211_frequency_to_channel(chan->center_freq),
-                        conf_is_ht(&sc->hw->conf));
+                        chan->center_freq,
+                        conf->channel_type,
+                        channel_type_str(conf->channel_type));
        put_unaligned_le32(REG_READ_D(sc->sc_ah, AR_STA_ID0), addr);
        put_unaligned_le16(REG_READ_D(sc->sc_ah, AR_STA_ID1) & 0xffff, addr + 4);
diff --git a/drivers/net/wireless/ath/ath9k/htc.h b/drivers/net/wireless/ath/ath9k/htc.h
index 63549868e686..0cb504d7b8c4 100644
--- a/drivers/net/wireless/ath/ath9k/htc.h
+++ b/drivers/net/wireless/ath/ath9k/htc.h
@@ -460,7 +460,6 @@ void ath9k_htc_ps_restore(struct ath9k_htc_priv *priv);
 void ath9k_ps_work(struct work_struct *work);
 bool ath9k_htc_setpower(struct ath9k_htc_priv *priv,
                        enum ath9k_power_mode mode);
-void ath_update_txpow(struct ath9k_htc_priv *priv);
 void ath9k_start_rfkill_poll(struct ath9k_htc_priv *priv);
 void ath9k_htc_rfkill_poll_state(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
index fe70f67aa088..7e630a81b453 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_gpio.c
@@ -389,7 +389,8 @@ void ath9k_htc_radio_enable(struct ieee80211_hw *hw)
                        ret, ah->curchan->channel);
        }
-        ath_update_txpow(priv);
+        ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+                               &priv->curtxpow);
        /* Start RX */
        WMI_CMD(WMI_START_RECV_CMDID);
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index a702089f18d0..953036a4ed53 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -24,17 +24,6 @@ static struct dentry *ath9k_debugfs_root;
 /* Utilities */
 /*************/
-void ath_update_txpow(struct ath9k_htc_priv *priv)
-{
-        struct ath_hw *ah = priv->ah;
-        if (priv->curtxpow != priv->txpowlimit) {
-                ath9k_hw_set_txpowerlimit(ah, priv->txpowlimit, false);
-                /* read back in case value is clamped */
-                priv->curtxpow = ath9k_hw_regulatory(ah)->power_limit;
-        }
-}
 /* HACK Alert: Use 11NG for 2.4, use 11NA for 5 */
 static enum htc_phymode ath9k_htc_get_curmode(struct ath9k_htc_priv *priv,
                                              struct ath9k_channel *ichan)
@@ -147,7 +136,8 @@ void ath9k_htc_reset(struct ath9k_htc_priv *priv)
                        channel->center_freq, ret);
        }
-        ath_update_txpow(priv);
+        ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+                               &priv->curtxpow);
        WMI_CMD(WMI_START_RECV_CMDID);
        ath9k_host_rx_init(priv);
@@ -212,7 +202,8 @@ static int ath9k_htc_set_channel(struct ath9k_htc_priv *priv,
                goto err;
        }
-        ath_update_txpow(priv);
+        ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+                               &priv->curtxpow);
        WMI_CMD(WMI_START_RECV_CMDID);
        if (ret)
@@ -988,7 +979,8 @@ static int ath9k_htc_start(struct ieee80211_hw *hw)
                return ret;
        }
-        ath_update_txpow(priv);
+        ath9k_cmn_update_txpow(ah, priv->curtxpow, priv->txpowlimit,
+                               &priv->curtxpow);
        mode = ath9k_htc_get_curmode(priv, init_channel);
        htc_mode = cpu_to_be16(mode);
@@ -1052,6 +1044,7 @@ static void ath9k_htc_stop(struct ieee80211_hw *hw)
        cancel_work_sync(&priv->fatal_work);
        cancel_work_sync(&priv->ps_work);
        cancel_delayed_work_sync(&priv->ath9k_led_blink_work);
+        cancel_delayed_work_sync(&priv->ath9k_ani_work);
        ath9k_led_stop_brightness(priv);
        mutex_lock(&priv->mutex);
@@ -1253,7 +1246,8 @@ static int ath9k_htc_config(struct ieee80211_hw *hw, u32 changed)
        if (changed & IEEE80211_CONF_CHANGE_POWER) {
                priv->txpowlimit = 2 * conf->power_level;
-                ath_update_txpow(priv);
+                ath9k_cmn_update_txpow(priv->ah, priv->curtxpow,
+                                       priv->txpowlimit, &priv->curtxpow);
        }
        if (changed & IEEE80211_CONF_CHANGE_IDLE) {
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index 1447b55a8d0a..2d4e9b861b60 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -18,17 +18,6 @@
 #include "ath9k.h"
 #include "btcoex.h"
-static void ath_update_txpow(struct ath_softc *sc)
-{
-        struct ath_hw *ah = sc->sc_ah;
-        if (sc->curtxpow != sc->config.txpowlimit) {
-                ath9k_hw_set_txpowerlimit(ah, sc->config.txpowlimit, false);
-                /* read back in case value is clamped */
-                sc->curtxpow = ath9k_hw_regulatory(ah)->power_limit;
-        }
-}
 static u8 parse_mpdudensity(u8 mpdudensity)
 {
        /*
@@ -64,19 +53,6 @@ static u8 parse_mpdudensity(u8 mpdudensity)
        }
 }
-static struct ath9k_channel *ath_get_curchannel(struct ath_softc *sc,
-                                                struct ieee80211_hw *hw)
-{
-        struct ieee80211_channel *curchan = hw->conf.channel;
-        struct ath9k_channel *channel;
-        u8 chan_idx;
-        chan_idx = curchan->hw_value;
-        channel = &sc->sc_ah->channels[chan_idx];
-        ath9k_cmn_update_ichannel(channel, curchan, hw->conf.channel_type);
-        return channel;
-}
 bool ath9k_setpower(struct ath_softc *sc, enum ath9k_power_mode mode)
 {
        unsigned long flags;
@@ -177,7 +153,12 @@ static void ath_update_survey_nf(struct ath_softc *sc, int channel)
        }
 }
-static void ath_update_survey_stats(struct ath_softc *sc)
+/*
+ * Updates the survey statistics and returns the busy time since last
+ * update in %, if the measurement duration was long enough for the
+ * result to be useful, -1 otherwise.
+ */
+static int ath_update_survey_stats(struct ath_softc *sc)
 {
        struct ath_hw *ah = sc->sc_ah;
        struct ath_common *common = ath9k_hw_common(ah);
@@ -185,9 +166,10 @@ static void ath_update_survey_stats(struct ath_softc *sc)
        struct survey_info *survey = &sc->survey[pos];
        struct ath_cycle_counters *cc = &common->cc_survey;
        unsigned int div = common->clockrate * 1000;
+        int ret = 0;
        if (!ah->curchan)
-                return;
+                return -1;
        if (ah->power_mode == ATH9K_PM_AWAKE)
                ath_hw_cycle_counters_update(common);
@@ -202,9 +184,18 @@ static void ath_update_survey_stats(struct ath_softc *sc)
                survey->channel_time_rx += cc->rx_frame / div;
                survey->channel_time_tx += cc->tx_frame / div;
        }
+        if (cc->cycles < div)
+                return -1;
+        if (cc->cycles > 0)
+                ret = cc->rx_busy * 100 / cc->cycles;
        memset(cc, 0, sizeof(*cc));
        ath_update_survey_nf(sc, pos);
+        return ret;
 }
 /*
@@ -226,6 +217,8 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
        if (sc->sc_flags & SC_OP_INVALID)
                return -EIO;
+        sc->hw_busy_count = 0;
        del_timer_sync(&common->ani.timer);
        cancel_work_sync(&sc->paprd_work);
        cancel_work_sync(&sc->hw_check_work);
@@ -284,7 +277,8 @@ int ath_set_channel(struct ath_softc *sc, struct ieee80211_hw *hw,
                goto ps_restore;
        }
-        ath_update_txpow(sc);
+        ath9k_cmn_update_txpow(ah, sc->curtxpow,
+                               sc->config.txpowlimit, &sc->curtxpow);
        ath9k_hw_set_interrupts(ah, ah->imask);
        if (!(sc->sc_flags & (SC_OP_OFFCHANNEL))) {
@@ -592,17 +586,25 @@ static void ath_node_detach(struct ath_softc *sc, struct ieee80211_sta *sta)
 void ath_hw_check(struct work_struct *work)
 {
        struct ath_softc *sc = container_of(work, struct ath_softc, hw_check_work);
-        int i;
+        struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+        unsigned long flags;
+        int busy;
        ath9k_ps_wakeup(sc);
+        if (ath9k_hw_check_alive(sc->sc_ah))
+                goto out;
-        for (i = 0; i < 3; i++) {
+        spin_lock_irqsave(&common->cc_lock, flags);
-                if (ath9k_hw_check_alive(sc->sc_ah))
+        busy = ath_update_survey_stats(sc);
-                        goto out;
+        spin_unlock_irqrestore(&common->cc_lock, flags);
-                msleep(1);
+        ath_dbg(common, ATH_DBG_RESET, "Possible baseband hang, "
-        }
+                "busy=%d (try %d)\n", busy, sc->hw_busy_count + 1);
-        ath_reset(sc, true);
+        if (busy >= 99) {
+                if (++sc->hw_busy_count >= 3)
+                        ath_reset(sc, true);
+        } else if (busy >= 0)
+                sc->hw_busy_count = 0;
 out:
        ath9k_ps_restore(sc);
@@ -867,7 +869,7 @@ void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw)
        ath9k_hw_configpcipowersave(ah, 0, 0);
        if (!ah->curchan)
-                ah->curchan = ath_get_curchannel(sc, sc->hw);
+                ah->curchan = ath9k_cmn_get_curchannel(sc->hw, ah);
        r = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
        if (r) {
@@ -876,7 +878,8 @@ void ath_radio_enable(struct ath_softc *sc, struct ieee80211_hw *hw)
                        channel->center_freq, r);
        }
-        ath_update_txpow(sc);
+        ath9k_cmn_update_txpow(ah, sc->curtxpow,
+                               sc->config.txpowlimit, &sc->curtxpow);
        if (ath_startrecv(sc) != 0) {
                ath_err(common, "Unable to restart recv logic\n");
                goto out;
@@ -928,7 +931,7 @@ void ath_radio_disable(struct ath_softc *sc, struct ieee80211_hw *hw)
        ath_flushrecv(sc);              /* flush recv queue */
        if (!ah->curchan)
-                ah->curchan = ath_get_curchannel(sc, hw);
+                ah->curchan = ath9k_cmn_get_curchannel(hw, ah);
        r = ath9k_hw_reset(ah, ah->curchan, ah->caldata, false);
        if (r) {
@@ -952,6 +955,8 @@ int ath_reset(struct ath_softc *sc, bool retry_tx)
        struct ieee80211_hw *hw = sc->hw;
        int r;
+        sc->hw_busy_count = 0;
        /* Stop ANI */
        del_timer_sync(&common->ani.timer);
@@ -979,7 +984,8 @@ int ath_reset(struct ath_softc *sc, bool retry_tx)
         * that changes the channel so update any state that
         * might change as a result.
         */
-        ath_update_txpow(sc);
+        ath9k_cmn_update_txpow(ah, sc->curtxpow,
+                               sc->config.txpowlimit, &sc->curtxpow);
        if ((sc->sc_flags & SC_OP_BEACONS) || !(sc->sc_flags & (SC_OP_OFFCHANNEL)))
                ath_beacon_config(sc, NULL);    /* restart beacons */
@@ -1029,7 +1035,7 @@ static int ath9k_start(struct ieee80211_hw *hw)
        /* setup initial channel */
        sc->chan_idx = curchan->hw_value;
-        init_channel = ath_get_curchannel(sc, hw);
+        init_channel = ath9k_cmn_get_curchannel(hw, ah);
        /* Reset SERDES registers */
        ath9k_hw_configpcipowersave(ah, 0, 0);
@@ -1055,7 +1061,8 @@ static int ath9k_start(struct ieee80211_hw *hw)
         * This is needed only to setup initial state
         * but it's best done after a reset.
         */
-        ath_update_txpow(sc);
+        ath9k_cmn_update_txpow(ah, sc->curtxpow,
+                        sc->config.txpowlimit, &sc->curtxpow);
        /*
         * Setup the hardware after reset:
@@ -1374,6 +1381,7 @@ static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
        ath9k_calculate_iter_data(hw, vif, &iter_data);
+        ath9k_ps_wakeup(sc);
        /* Set BSSID mask. */
        memcpy(common->bssidmask, iter_data.mask, ETH_ALEN);
        ath_hw_setbssidmask(common);
@@ -1408,6 +1416,7 @@ static void ath9k_calculate_summary_state(struct ieee80211_hw *hw,
        }
        ath9k_hw_set_interrupts(ah, ah->imask);
+        ath9k_ps_restore(sc);
        /* Set up ANI */
        if ((iter_data.naps + iter_data.nadhocs) > 0) {
@@ -1437,9 +1446,7 @@ static void ath9k_do_vif_add_setup(struct ieee80211_hw *hw,
                 * there.
                 */
                error = ath_beacon_alloc(sc, vif);
-                if (error)
+                if (!error)
-                        ath9k_reclaim_beacon(sc, vif);
-                else
                        ath_beacon_config(sc, vif);
        }
 }
@@ -1720,7 +1727,8 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
        if (changed & IEEE80211_CONF_CHANGE_POWER) {
                sc->config.txpowlimit = 2 * conf->power_level;
                ath9k_ps_wakeup(sc);
-                ath_update_txpow(sc);
+                ath9k_cmn_update_txpow(ah, sc->curtxpow,
+                                       sc->config.txpowlimit, &sc->curtxpow);
                ath9k_ps_restore(sc);
        }
diff --git a/drivers/net/wireless/ath/key.c b/drivers/net/wireless/ath/key.c
index 5d465e5fcf24..37b8e115375a 100644
--- a/drivers/net/wireless/ath/key.c
+++ b/drivers/net/wireless/ath/key.c
@@ -58,8 +58,11 @@ bool ath_hw_keyreset(struct ath_common *common, u16 entry)
                REG_WRITE(ah, AR_KEYTABLE_KEY1(micentry), 0);
                REG_WRITE(ah, AR_KEYTABLE_KEY2(micentry), 0);
                REG_WRITE(ah, AR_KEYTABLE_KEY3(micentry), 0);
-                if (common->crypt_caps & ATH_CRYPT_CAP_MIC_COMBINED)
+                if (common->crypt_caps & ATH_CRYPT_CAP_MIC_COMBINED) {
                        REG_WRITE(ah, AR_KEYTABLE_KEY4(micentry), 0);
+                        REG_WRITE(ah, AR_KEYTABLE_TYPE(micentry),
+                                  AR_KEYTABLE_TYPE_CLR);
+                }
        }
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index 61915f371416..da60faee74fc 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -1397,7 +1397,7 @@ static int ipw2100_power_cycle_adapter(struct ipw2100_priv *priv)
 }
 /*
- * Send the CARD_DISABLE_PHY_OFF comamnd to the card to disable it
+ * Send the CARD_DISABLE_PHY_OFF command to the card to disable it
 *
 * After disabling, if the card was associated, a STATUS_ASSN_LOST will be sent.
 *
diff --git a/drivers/net/wireless/iwlwifi/iwl-1000.c b/drivers/net/wireless/iwlwifi/iwl-1000.c
index 127723e6319f..ba78bc8a259f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-1000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-1000.c
@@ -270,7 +270,6 @@ static struct iwl_base_params iwl1000_base_params = {
        .ucode_tracing = true,
        .sensitivity_calib_by_driver = true,
        .chain_noise_calib_by_driver = true,
-        .supports_idle = true,
 };
 static struct iwl_ht_params iwl1000_ht_params = {
        .ht_greenfield_support = true,
diff --git a/drivers/net/wireless/iwlwifi/iwl-2000.c b/drivers/net/wireless/iwlwifi/iwl-2000.c
index 3c9e1b5724c7..3c5dd36ff417 100644
--- a/drivers/net/wireless/iwlwifi/iwl-2000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-2000.c
@@ -97,6 +97,10 @@ static void iwl2000_nic_config(struct iwl_priv *priv)
                    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
                    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+        if (priv->cfg->iq_invert)
+                iwl_set_bit(priv, CSR_GP_DRIVER_REG,
+                            CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER);
 }
 static struct iwl_sensitivity_ranges iwl2000_sensitivity = {
@@ -364,7 +368,6 @@ static struct iwl_base_params iwl2000_base_params = {
        .shadow_ram_support = true,
        .led_compensation = 51,
        .chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-        .supports_idle = true,
        .adv_thermal_throttle = true,
        .support_ct_kill_exit = true,
        .plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -389,7 +392,6 @@ static struct iwl_base_params iwl2030_base_params = {
        .shadow_ram_support = true,
        .led_compensation = 57,
        .chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-        .supports_idle = true,
        .adv_thermal_throttle = true,
        .support_ct_kill_exit = true,
        .plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -428,7 +430,8 @@ static struct iwl_bt_params iwl2030_bt_params = {
        .base_params = &iwl2000_base_params,                    \
        .need_dc_calib = true,                                  \
        .need_temp_offset_calib = true,                         \
-        .led_mode = IWL_LED_RF_STATE                            \
+        .led_mode = IWL_LED_RF_STATE,                           \
+        .iq_invert = true                                       \
 struct iwl_cfg iwl2000_2bgn_cfg = {
        .name = "2000 Series 2x2 BGN",
@@ -454,17 +457,18 @@ struct iwl_cfg iwl2000_2bg_cfg = {
        .need_dc_calib = true,                                  \
        .need_temp_offset_calib = true,                         \
        .led_mode = IWL_LED_RF_STATE,                           \
-        .adv_pm = true                                          \
+        .adv_pm = true,                                         \
+        .iq_invert = true                                       \
 struct iwl_cfg iwl2030_2bgn_cfg = {
        .name = "2000 Series 2x2 BGN/BT",
-        IWL_DEVICE_2000,
+        IWL_DEVICE_2030,
        .ht_params = &iwl2000_ht_params,
 };
 struct iwl_cfg iwl2030_2bg_cfg = {
        .name = "2000 Series 2x2 BG/BT",
-        IWL_DEVICE_2000,
+        IWL_DEVICE_2030,
 };
 #define IWL_DEVICE_6035                                         \
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index 7c14eb31d954..8998ed134d1a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -251,14 +251,6 @@ static int iwl4965_set_ucode_ptrs(struct iwl_priv *priv)
 */
 static void iwl4965_init_alive_start(struct iwl_priv *priv)
 {
-        /* Check alive response for "valid" sign from uCode */
-        if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
-                /* We had an error bringing up the hardware, so take it
-                 * all the way back down so we can try again */
-                IWL_DEBUG_INFO(priv, "Initialize Alive failed.\n");
-                goto restart;
-        }
        /* Bootstrap uCode has loaded initialize uCode ... verify inst image.
         * This is a paranoid check, because we would not have gotten the
         * "initialize" alive if code weren't properly loaded.  */
@@ -2274,6 +2266,29 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
        spin_unlock_irqrestore(&priv->sta_lock, flags);
 }
+static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
+                                    struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = rxb_addr(rxb);
+        struct iwl4965_beacon_notif *beacon = (void *)pkt->u.raw;
+#ifdef CONFIG_IWLWIFI_DEBUG
+        u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+        IWL_DEBUG_RX(priv, "beacon status %#x, retries:%d ibssmgr:%d "
+                "tsf:0x%.8x%.8x rate:%d\n",
+                le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
+                beacon->beacon_notify_hdr.failure_frame,
+                le32_to_cpu(beacon->ibss_mgr_status),
+                le32_to_cpu(beacon->high_tsf),
+                le32_to_cpu(beacon->low_tsf), rate);
+#endif
+        priv->ibss_manager = le32_to_cpu(beacon->ibss_mgr_status);
+        if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+                queue_work(priv->workqueue, &priv->beacon_update);
+}
 static int iwl4965_calc_rssi(struct iwl_priv *priv,
                             struct iwl_rx_phy_res *rx_resp)
 {
@@ -2316,6 +2331,7 @@ static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
        priv->rx_handlers[REPLY_RX] = iwlagn_rx_reply_rx;
        /* Tx response */
        priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
+        priv->rx_handlers[BEACON_NOTIFICATION] = iwl4965_rx_beacon_notif;
        /* set up notification wait support */
        spin_lock_init(&priv->_agn.notif_wait_lock);
diff --git a/drivers/net/wireless/iwlwifi/iwl-6000.c b/drivers/net/wireless/iwlwifi/iwl-6000.c
index c195674454f4..aa32b1e05dff 100644
--- a/drivers/net/wireless/iwlwifi/iwl-6000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-6000.c
@@ -479,7 +479,6 @@ static struct iwl_base_params iwl6000_base_params = {
        .shadow_ram_support = true,
        .led_compensation = 51,
        .chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-        .supports_idle = true,
        .adv_thermal_throttle = true,
        .support_ct_kill_exit = true,
        .plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -503,7 +502,6 @@ static struct iwl_base_params iwl6050_base_params = {
        .shadow_ram_support = true,
        .led_compensation = 51,
        .chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-        .supports_idle = true,
        .adv_thermal_throttle = true,
        .support_ct_kill_exit = true,
        .plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
@@ -526,7 +524,6 @@ static struct iwl_base_params iwl6000_g2_base_params = {
        .shadow_ram_support = true,
        .led_compensation = 57,
        .chain_noise_num_beacons = IWL_CAL_NUM_BEACONS,
-        .supports_idle = true,
        .adv_thermal_throttle = true,
        .support_ct_kill_exit = true,
        .plcp_delta_threshold = IWL_MAX_PLCP_ERR_THRESHOLD_DEF,
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
index c7d03874b380..3aa486437509 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-lib.c
@@ -1395,15 +1395,12 @@ int iwlagn_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                u32 extra;
                u32 suspend_time = 100;
                u32 scan_suspend_time = 100;
-                unsigned long flags;
                IWL_DEBUG_INFO(priv, "Scanning while associated...\n");
-                spin_lock_irqsave(&priv->lock, flags);
                if (priv->is_internal_short_scan)
                        interval = 0;
                else
                        interval = vif->bss_conf.beacon_int;
-                spin_unlock_irqrestore(&priv->lock, flags);
                scan->suspend_time = 0;
                scan->max_out_time = cpu_to_le32(200 * 1024);
@@ -1863,21 +1860,6 @@ void iwlagn_send_advance_bt_config(struct iwl_priv *priv)
        if (iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG, sizeof(bt_cmd), &bt_cmd))
                IWL_ERR(priv, "failed to send BT Coex Config\n");
-        /*
-         * When we are doing a restart, need to also reconfigure BT
-         * SCO to the device. If not doing a restart, bt_sco_active
-         * will always be false, so there's no need to have an extra
-         * variable to check for it.
-         */
-        if (priv->bt_sco_active) {
-                struct iwlagn_bt_sco_cmd sco_cmd = { .flags = 0 };
-                if (priv->bt_sco_active)
-                        sco_cmd.flags |= IWLAGN_BT_SCO_ACTIVE;
-                if (iwl_send_cmd_pdu(priv, REPLY_BT_COEX_SCO,
-                                     sizeof(sco_cmd), &sco_cmd))
-                        IWL_ERR(priv, "failed to send BT SCO command\n");
-        }
 }
 static void iwlagn_bt_traffic_change_work(struct work_struct *work)
@@ -2038,7 +2020,6 @@ void iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
        unsigned long flags;
        struct iwl_rx_packet *pkt = rxb_addr(rxb);
        struct iwl_bt_coex_profile_notif *coex = &pkt->u.bt_coex_profile_notif;
-        struct iwlagn_bt_sco_cmd sco_cmd = { .flags = 0 };
        struct iwl_bt_uart_msg *uart_msg = &coex->last_bt_uart_msg;
        IWL_DEBUG_NOTIF(priv, "BT Coex notification:\n");
@@ -2069,15 +2050,6 @@ void iwlagn_bt_coex_profile_notif(struct iwl_priv *priv,
                        queue_work(priv->workqueue,
                                   &priv->bt_traffic_change_work);
                }
-                if (priv->bt_sco_active !=
-                    (uart_msg->frame3 & BT_UART_MSG_FRAME3SCOESCO_MSK)) {
-                        priv->bt_sco_active = uart_msg->frame3 &
-                                BT_UART_MSG_FRAME3SCOESCO_MSK;
-                        if (priv->bt_sco_active)
-                                sco_cmd.flags |= IWLAGN_BT_SCO_ACTIVE;
-                        iwl_send_cmd_pdu_async(priv, REPLY_BT_COEX_SCO,
-                                       sizeof(sco_cmd), &sco_cmd, NULL);
-                }
        }
        iwlagn_set_kill_msk(priv, uart_msg);
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
index 75fcd30a7c13..d03b4734c892 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-rs.c
@@ -179,31 +179,31 @@ static s32 expected_tpt_legacy[IWL_RATE_COUNT] = {
 };
 static s32 expected_tpt_siso20MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0, 42, 0,  76, 102, 124, 158, 183, 193, 202}, /* Norm */
+        {0, 0, 0, 0, 42, 0,  76, 102, 124, 159, 183, 193, 202}, /* Norm */
-        {0, 0, 0, 0, 46, 0,  82, 110, 132, 167, 192, 202, 210}, /* SGI */
+        {0, 0, 0, 0, 46, 0,  82, 110, 132, 168, 192, 202, 210}, /* SGI */
-        {0, 0, 0, 0, 48, 0,  93, 135, 176, 251, 319, 351, 381}, /* AGG */
+        {0, 0, 0, 0, 47, 0,  91, 133, 171, 242, 305, 334, 362}, /* AGG */
-        {0, 0, 0, 0, 53, 0, 102, 149, 193, 275, 348, 381, 413}, /* AGG+SGI */
+        {0, 0, 0, 0, 52, 0, 101, 145, 187, 264, 330, 361, 390}, /* AGG+SGI */
 };
 static s32 expected_tpt_siso40MHz[4][IWL_RATE_COUNT] = {
        {0, 0, 0, 0,  77, 0, 127, 160, 184, 220, 242, 250, 257}, /* Norm */
        {0, 0, 0, 0,  83, 0, 135, 169, 193, 229, 250, 257, 264}, /* SGI */
-        {0, 0, 0, 0,  96, 0, 182, 259, 328, 451, 553, 598, 640}, /* AGG */
+        {0, 0, 0, 0,  94, 0, 177, 249, 313, 423, 512, 550, 586}, /* AGG */
-        {0, 0, 0, 0, 106, 0, 199, 282, 357, 487, 593, 640, 683}, /* AGG+SGI */
+        {0, 0, 0, 0, 104, 0, 193, 270, 338, 454, 545, 584, 620}, /* AGG+SGI */
 };
 static s32 expected_tpt_mimo2_20MHz[4][IWL_RATE_COUNT] = {
-        {0, 0, 0, 0,  74, 0, 123, 155, 179, 213, 235, 243, 250}, /* Norm */
+        {0, 0, 0, 0,  74, 0, 123, 155, 179, 214, 236, 244, 251}, /* Norm */
-        {0, 0, 0, 0,  81, 0, 131, 164, 187, 221, 242, 250, 256}, /* SGI */
+        {0, 0, 0, 0,  81, 0, 131, 164, 188, 223, 243, 251, 257}, /* SGI */
-        {0, 0, 0, 0,  92, 0, 175, 250, 317, 436, 534, 578, 619}, /* AGG */
+        {0, 0, 0, 0,  89, 0, 167, 235, 296, 402, 488, 526, 560}, /* AGG */
-        {0, 0, 0, 0, 102, 0, 192, 273, 344, 470, 573, 619, 660}, /* AGG+SGI*/
+        {0, 0, 0, 0,  97, 0, 182, 255, 320, 431, 520, 558, 593}, /* AGG+SGI*/
 };
 static s32 expected_tpt_mimo2_40MHz[4][IWL_RATE_COUNT] = {
        {0, 0, 0, 0, 123, 0, 182, 214, 235, 264, 279, 285, 289}, /* Norm */
        {0, 0, 0, 0, 131, 0, 191, 222, 242, 270, 284, 289, 293}, /* SGI */
-        {0, 0, 0, 0, 180, 0, 327, 446, 545, 708, 828, 878, 922}, /* AGG */
+        {0, 0, 0, 0, 171, 0, 305, 410, 496, 634, 731, 771, 805}, /* AGG */
-        {0, 0, 0, 0, 197, 0, 355, 481, 584, 752, 872, 922, 966}, /* AGG+SGI */
+        {0, 0, 0, 0, 186, 0, 329, 439, 527, 667, 764, 803, 838}, /* AGG+SGI */
 };
 static s32 expected_tpt_mimo3_20MHz[4][IWL_RATE_COUNT] = {
@@ -2890,6 +2890,8 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
        u8 ant_toggle_cnt = 0;
        u8 use_ht_possible = 1;
        u8 valid_tx_ant = 0;
+        struct iwl_station_priv *sta_priv =
+                container_of(lq_sta, struct iwl_station_priv, lq_sta);
        struct iwl_link_quality_cmd *lq_cmd = &lq_sta->lq;
        /* Override starting rate (index 0) if needed for debug purposes */
@@ -3008,7 +3010,8 @@ static void rs_fill_link_cmd(struct iwl_priv *priv,
                repeat_rate--;
        }
-        lq_cmd->agg_params.agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+        lq_cmd->agg_params.agg_frame_cnt_limit =
+                sta_priv->max_agg_bufsize ?: LINK_QUAL_AGG_FRAME_LIMIT_DEF;
        lq_cmd->agg_params.agg_dis_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
        lq_cmd->agg_params.agg_time_limit =
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
index 24dabcd2a36c..d807e5e2b718 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn-ucode.c
@@ -308,14 +308,6 @@ void iwlagn_init_alive_start(struct iwl_priv *priv)
 {
        int ret = 0;
-        /* Check alive response for "valid" sign from uCode */
-        if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
-                /* We had an error bringing up the hardware, so take it
-                 * all the way back down so we can try again */
-                IWL_DEBUG_INFO(priv, "Initialize Alive failed.\n");
-                goto restart;
-        }
        /* initialize uCode was loaded... verify inst image.
         * This is a paranoid check, because we would not have gotten the
         * "initialize" alive if code weren't properly loaded.  */
diff --git a/drivers/net/wireless/iwlwifi/iwl-agn.c b/drivers/net/wireless/iwlwifi/iwl-agn.c
index 646ccb2430b4..096f8ad0f1b1 100644
--- a/drivers/net/wireless/iwlwifi/iwl-agn.c
+++ b/drivers/net/wireless/iwlwifi/iwl-agn.c
@@ -462,8 +462,12 @@ static void iwl_rx_reply_alive(struct iwl_priv *priv,
        if (palive->is_valid == UCODE_VALID_OK)
                queue_delayed_work(priv->workqueue, pwork,
                                   msecs_to_jiffies(5));
-        else
+        else {
-                IWL_WARN(priv, "uCode did not respond OK.\n");
+                IWL_WARN(priv, "%s uCode did not respond OK.\n",
+                        (palive->ver_subtype == INITIALIZE_SUBTYPE) ?
+                        "init" : "runtime");
+                queue_work(priv->workqueue, &priv->restart);
+        }
 }
 static void iwl_bg_beacon_update(struct work_struct *work)
@@ -700,18 +704,18 @@ static void iwl_bg_ucode_trace(unsigned long data)
        }
 }
-static void iwl_rx_beacon_notif(struct iwl_priv *priv,
+static void iwlagn_rx_beacon_notif(struct iwl_priv *priv,
-                                struct iwl_rx_mem_buffer *rxb)
+                                   struct iwl_rx_mem_buffer *rxb)
 {
        struct iwl_rx_packet *pkt = rxb_addr(rxb);
-        struct iwl4965_beacon_notif *beacon =
+        struct iwlagn_beacon_notif *beacon = (void *)pkt->u.raw;
-                (struct iwl4965_beacon_notif *)pkt->u.raw;
 #ifdef CONFIG_IWLWIFI_DEBUG
+        u16 status = le16_to_cpu(beacon->beacon_notify_hdr.status.status);
        u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
-        IWL_DEBUG_RX(priv, "beacon status %x retries %d iss %d "
+        IWL_DEBUG_RX(priv, "beacon status %#x, retries:%d ibssmgr:%d "
-                "tsf %d %d rate %d\n",
+                "tsf:0x%.8x%.8x rate:%d\n",
-                le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
+                status & TX_STATUS_MSK,
                beacon->beacon_notify_hdr.failure_frame,
                le32_to_cpu(beacon->ibss_mgr_status),
                le32_to_cpu(beacon->high_tsf),
@@ -814,7 +818,7 @@ static void iwl_setup_rx_handlers(struct iwl_priv *priv)
        priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_rx_pm_sleep_notif;
        priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
            iwl_rx_pm_debug_statistics_notif;
-        priv->rx_handlers[BEACON_NOTIFICATION] = iwl_rx_beacon_notif;
+        priv->rx_handlers[BEACON_NOTIFICATION] = iwlagn_rx_beacon_notif;
        /*
         * The same handler is used for both the REPLY to a discrete
@@ -2648,13 +2652,6 @@ static void iwl_alive_start(struct iwl_priv *priv)
        IWL_DEBUG_INFO(priv, "Runtime Alive received.\n");
-        if (priv->card_alive.is_valid != UCODE_VALID_OK) {
-                /* We had an error bringing up the hardware, so take it
-                 * all the way back down so we can try again */
-                IWL_DEBUG_INFO(priv, "Alive failed.\n");
-                goto restart;
-        }
        /* Initialize uCode has loaded Runtime uCode ... verify inst image.
         * This is a paranoid check, because we would not have gotten the
         * "runtime" alive if code weren't properly loaded.  */
@@ -2783,7 +2780,6 @@ static void __iwl_down(struct iwl_priv *priv)
                         priv->cfg->bt_params->bt_init_traffic_load;
        else
                priv->bt_traffic_load = 0;
-        priv->bt_sco_active = false;
        priv->bt_full_concurrent = false;
        priv->bt_ci_compliance = 0;
@@ -3102,7 +3098,7 @@ static void iwl_bg_restart(struct work_struct *data)
        if (test_and_clear_bit(STATUS_FW_ERROR, &priv->status)) {
                struct iwl_rxon_context *ctx;
-                bool bt_sco, bt_full_concurrent;
+                bool bt_full_concurrent;
                u8 bt_ci_compliance;
                u8 bt_load;
                u8 bt_status;
@@ -3121,7 +3117,6 @@ static void iwl_bg_restart(struct work_struct *data)
                 * re-configure the hw when we reconfigure the BT
                 * command.
                 */
-                bt_sco = priv->bt_sco_active;
                bt_full_concurrent = priv->bt_full_concurrent;
                bt_ci_compliance = priv->bt_ci_compliance;
                bt_load = priv->bt_traffic_load;
@@ -3129,7 +3124,6 @@ static void iwl_bg_restart(struct work_struct *data)
                __iwl_down(priv);
-                priv->bt_sco_active = bt_sco;
                priv->bt_full_concurrent = bt_full_concurrent;
                priv->bt_ci_compliance = bt_ci_compliance;
                priv->bt_traffic_load = bt_load;
@@ -3191,6 +3185,8 @@ static int iwl_mac_setup_register(struct iwl_priv *priv,
                    IEEE80211_HW_SPECTRUM_MGMT |
                    IEEE80211_HW_REPORTS_TX_ACK_STATUS;
+        hw->max_tx_aggregation_subframes = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
        if (!priv->cfg->base_params->broken_powersave)
                hw->flags |= IEEE80211_HW_SUPPORTS_PS |
                             IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
@@ -3210,7 +3206,8 @@ static int iwl_mac_setup_register(struct iwl_priv *priv,
        hw->wiphy->max_remain_on_channel_duration = 1000;
        hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
-                            WIPHY_FLAG_DISABLE_BEACON_HINTS;
+                            WIPHY_FLAG_DISABLE_BEACON_HINTS |
+                            WIPHY_FLAG_IBSS_RSN;
        /*
         * For now, disable PS by default because it affects
@@ -3362,6 +3359,14 @@ int iwlagn_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
                return -EOPNOTSUPP;
        }
+        /*
+         * To support IBSS RSN, don't program group keys in IBSS, the
+         * hardware will then not attempt to decrypt the frames.
+         */
+        if (vif->type == NL80211_IFTYPE_ADHOC &&
+            !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+                return -EOPNOTSUPP;
        sta_id = iwl_sta_id_or_broadcast(priv, vif_priv->ctx, sta);
        if (sta_id == IWL_INVALID_STATION)
                return -EINVAL;
@@ -3421,6 +3426,7 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
 {
        struct iwl_priv *priv = hw->priv;
        int ret = -EINVAL;
+        struct iwl_station_priv *sta_priv = (void *) sta->drv_priv;
        IWL_DEBUG_HT(priv, "A-MPDU action on addr %pM tid %d\n",
                     sta->addr, tid);
@@ -3475,11 +3481,28 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
                }
                break;
        case IEEE80211_AMPDU_TX_OPERATIONAL:
+                /*
+                 * If the limit is 0, then it wasn't initialised yet,
+                 * use the default. We can do that since we take the
+                 * minimum below, and we don't want to go above our
+                 * default due to hardware restrictions.
+                 */
+                if (sta_priv->max_agg_bufsize == 0)
+                        sta_priv->max_agg_bufsize =
+                                LINK_QUAL_AGG_FRAME_LIMIT_DEF;
+                /*
+                 * Even though in theory the peer could have different
+                 * aggregation reorder buffer sizes for different sessions,
+                 * our ucode doesn't allow for that and has a global limit
+                 * for each station. Therefore, use the minimum of all the
+                 * aggregation sessions and our default value.
+                 */
+                sta_priv->max_agg_bufsize =
+                        min(sta_priv->max_agg_bufsize, buf_size);
                if (priv->cfg->ht_params &&
                    priv->cfg->ht_params->use_rts_for_aggregation) {
-                        struct iwl_station_priv *sta_priv =
-                                (void *) sta->drv_priv;
                        /*
                         * switch to RTS/CTS if it is the prefer protection
                         * method for HT traffic
@@ -3487,9 +3510,13 @@ int iwlagn_mac_ampdu_action(struct ieee80211_hw *hw,
                        sta_priv->lq_sta.lq.general_params.flags |=
                                LINK_QUAL_FLAGS_SET_STA_TLC_RTS_MSK;
-                        iwl_send_lq_cmd(priv, iwl_rxon_ctx_from_vif(vif),
-                                        &sta_priv->lq_sta.lq, CMD_ASYNC, false);
                }
+                sta_priv->lq_sta.lq.agg_params.agg_frame_cnt_limit =
+                        sta_priv->max_agg_bufsize;
+                iwl_send_lq_cmd(priv, iwl_rxon_ctx_from_vif(vif),
+                                &sta_priv->lq_sta.lq, CMD_ASYNC, false);
                ret = 0;
                break;
        }
@@ -3744,7 +3771,7 @@ static void iwlagn_disable_roc(struct iwl_priv *priv)
        priv->_agn.hw_roc_channel = NULL;
-        iwlagn_commit_rxon(priv, ctx);
+        iwlcore_commit_rxon(priv, ctx);
        ctx->is_active = false;
 }
@@ -3760,6 +3787,7 @@ static void iwlagn_bg_roc_done(struct work_struct *work)
        mutex_unlock(&priv->mutex);
 }
+#ifdef CONFIG_IWL5000
 static int iwl_mac_remain_on_channel(struct ieee80211_hw *hw,
                                     struct ieee80211_channel *channel,
                                     enum nl80211_channel_type channel_type,
@@ -3787,7 +3815,7 @@ static int iwl_mac_remain_on_channel(struct ieee80211_hw *hw,
        priv->_agn.hw_roc_channel = channel;
        priv->_agn.hw_roc_chantype = channel_type;
        priv->_agn.hw_roc_duration = DIV_ROUND_UP(duration * 1000, 1024);
-        iwlagn_commit_rxon(priv, &priv->contexts[IWL_RXON_CTX_PAN]);
+        iwlcore_commit_rxon(priv, &priv->contexts[IWL_RXON_CTX_PAN]);
        queue_delayed_work(priv->workqueue, &priv->_agn.hw_roc_work,
                           msecs_to_jiffies(duration + 20));
@@ -3815,6 +3843,7 @@ static int iwl_mac_cancel_remain_on_channel(struct ieee80211_hw *hw)
        return 0;
 }
+#endif
 /*****************************************************************************
 *
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index 935b19e2c260..0a1d4aeb36aa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -178,7 +178,6 @@ enum {
        REPLY_BT_COEX_PRIO_TABLE = 0xcc,
        REPLY_BT_COEX_PROT_ENV = 0xcd,
        REPLY_BT_COEX_PROFILE_NOTIF = 0xce,
-        REPLY_BT_COEX_SCO = 0xcf,
        /* PAN commands */
        REPLY_WIPAN_PARAMS = 0xb2,
@@ -3083,6 +3082,13 @@ struct iwl4965_beacon_notif {
        __le32 ibss_mgr_status;
 } __packed;
+struct iwlagn_beacon_notif {
+        struct iwlagn_tx_resp beacon_notify_hdr;
+        __le32 low_tsf;
+        __le32 high_tsf;
+        __le32 ibss_mgr_status;
+} __packed;
 /*
 * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
 */
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index c83fcc60ccc5..e0ec17079dc0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -305,7 +305,6 @@ struct iwl_base_params {
        u16 led_compensation;
        const bool broken_powersave;
        int chain_noise_num_beacons;
-        const bool supports_idle;
        bool adv_thermal_throttle;
        bool support_ct_kill_exit;
        const bool support_wimax_coexist;
@@ -364,6 +363,7 @@ struct iwl_ht_params {
 * @adv_pm: advance power management
 * @rx_with_siso_diversity: 1x1 device with rx antenna diversity
 * @internal_wimax_coex: internal wifi/wimax combo device
+ * @iq_invert: I/Q inversion
 *
 * We enable the driver to be backward compatible wrt API version. The
 * driver specifies which APIs it supports (with @ucode_api_max being the
@@ -413,6 +413,7 @@ struct iwl_cfg {
        const bool adv_pm;
        const bool rx_with_siso_diversity;
        const bool internal_wimax_coex;
+        const bool iq_invert;
 };
 /***************************
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 6c2b2df7ee7e..f52bc040bcbf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -382,6 +382,8 @@
 #define CSR_GP_DRIVER_REG_BIT_CALIB_VERSION6        (0x00000004)
 #define CSR_GP_DRIVER_REG_BIT_6050_1x2              (0x00000008)
+#define CSR_GP_DRIVER_REG_BIT_RADIO_IQ_INVER        (0x00000080)
 /* GIO Chicken Bits (PCI Express bus link power management) */
 #define CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX  (0x00800000)
 #define CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER  (0x20000000)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
index bdcb74279f1e..bc7a965c18f9 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -1587,10 +1587,9 @@ static ssize_t iwl_dbgfs_bt_traffic_read(struct file *file,
                         "last traffic notif: %d\n",
                priv->bt_status ? "On" : "Off", priv->last_bt_traffic_load);
        pos += scnprintf(buf + pos, bufsz - pos, "ch_announcement: %d, "
-                         "sco_active: %d, kill_ack_mask: %x, "
+                         "kill_ack_mask: %x, kill_cts_mask: %x\n",
-                         "kill_cts_mask: %x\n",
+                priv->bt_ch_announce, priv->kill_ack_mask,
-                priv->bt_ch_announce, priv->bt_sco_active,
+                priv->kill_cts_mask);
-                priv->kill_ack_mask, priv->kill_cts_mask);
        pos += scnprintf(buf + pos, bufsz - pos, "bluetooth traffic load: ");
        switch (priv->bt_traffic_load) {
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index b5f21e041953..ecfbef402781 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -509,6 +509,7 @@ struct iwl_station_priv {
        atomic_t pending_frames;
        bool client;
        bool asleep;
+        u8 max_agg_bufsize;
 };
 /**
@@ -1503,7 +1504,6 @@ struct iwl_priv {
        u8 bt_status;
        u8 bt_traffic_load, last_bt_traffic_load;
        bool bt_ch_announce;
-        bool bt_sco_active;
        bool bt_full_concurrent;
        bool bt_ant_couple_ok;
        __le32 kill_ack_mask;
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
index 25f7d474f346..1d1bf3234d8d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-power.c
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -356,8 +356,7 @@ static void iwl_power_build_cmd(struct iwl_priv *priv,
        if (priv->cfg->base_params->broken_powersave)
                iwl_power_sleep_cam_cmd(priv, cmd);
-        else if (priv->cfg->base_params->supports_idle &&
+        else if (priv->hw->conf.flags & IEEE80211_CONF_IDLE)
-                 priv->hw->conf.flags & IEEE80211_CONF_IDLE)
                iwl_static_sleep_cmd(priv, cmd, IWL_POWER_INDEX_5, 20);
        else if (priv->cfg->ops->lib->tt_ops.lower_power_detection &&
                 priv->cfg->ops->lib->tt_ops.tt_power_mode &&
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index 2945acd955f0..adcef735180a 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -2860,16 +2860,13 @@ int iwl3945_request_scan(struct iwl_priv *priv, struct ieee80211_vif *vif)
                u32 extra;
                u32 suspend_time = 100;
                u32 scan_suspend_time = 100;
-                unsigned long flags;
                IWL_DEBUG_INFO(priv, "Scanning while associated...\n");
-                spin_lock_irqsave(&priv->lock, flags);
                if (priv->is_internal_short_scan)
                        interval = 0;
                else
                        interval = vif->bss_conf.beacon_int;
-                spin_unlock_irqrestore(&priv->lock, flags);
                scan->suspend_time = 0;
                scan->max_out_time = cpu_to_le32(200 * 1024);
@@ -3286,6 +3283,14 @@ static int iwl3945_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
                return -EOPNOTSUPP;
        }
+        /*
+         * To support IBSS RSN, don't program group keys in IBSS, the
+         * hardware will then not attempt to decrypt the frames.
+         */
+        if (vif->type == NL80211_IFTYPE_ADHOC &&
+            !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+                return -EOPNOTSUPP;
        static_key = !iwl_is_associated(priv, IWL_RXON_CTX_BSS);
        if (!static_key) {
@@ -3915,7 +3920,8 @@ static int iwl3945_setup_mac(struct iwl_priv *priv)
                priv->contexts[IWL_RXON_CTX_BSS].interface_modes;
        hw->wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY |
-                            WIPHY_FLAG_DISABLE_BEACON_HINTS;
+                            WIPHY_FLAG_DISABLE_BEACON_HINTS |
+                            WIPHY_FLAG_IBSS_RSN;
        hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX_3945;
        /* we create the 802.11 header and a zero-length SSID element */
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 78c4da150a74..7e8a658b7670 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -145,9 +145,13 @@ int lbs_update_hw_spec(struct lbs_private *priv)
        if (priv->current_addr[0] == 0xff)
                memmove(priv->current_addr, cmd.permanentaddr, ETH_ALEN);
-        memcpy(priv->dev->dev_addr, priv->current_addr, ETH_ALEN);
+        if (!priv->copied_hwaddr) {
-        if (priv->mesh_dev)
+                memcpy(priv->dev->dev_addr, priv->current_addr, ETH_ALEN);
-                memcpy(priv->mesh_dev->dev_addr, priv->current_addr, ETH_ALEN);
+                if (priv->mesh_dev)
+                        memcpy(priv->mesh_dev->dev_addr,
+                                priv->current_addr, ETH_ALEN);
+                priv->copied_hwaddr = 1;
+        }
 out:
        lbs_deb_leave(LBS_DEB_CMD);
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 18dd9a02c459..bc461eb39660 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -90,6 +90,7 @@ struct lbs_private {
        void *card;
        u8 fw_ready;
        u8 surpriseremoved;
+        u8 setup_fw_on_resume;
        int (*hw_host_to_card) (struct lbs_private *priv, u8 type, u8 *payload, u16 nb);
        void (*reset_card) (struct lbs_private *priv);
        int (*enter_deep_sleep) (struct lbs_private *priv);
@@ -101,6 +102,7 @@ struct lbs_private {
        u32 fwcapinfo;
        u16 regioncode;
        u8 current_addr[ETH_ALEN];
+        u8 copied_hwaddr;
        /* Command download */
        u8 dnld_sent;
diff --git a/drivers/net/wireless/libertas/if_spi.c b/drivers/net/wireless/libertas/if_spi.c
index 00600239a053..f6c2cd665f49 100644
--- a/drivers/net/wireless/libertas/if_spi.c
+++ b/drivers/net/wireless/libertas/if_spi.c
@@ -20,10 +20,8 @@
 #include <linux/moduleparam.h>
 #include <linux/firmware.h>
 #include <linux/jiffies.h>
-#include <linux/kthread.h>
 #include <linux/list.h>
 #include <linux/netdevice.h>
-#include <linux/semaphore.h>
 #include <linux/slab.h>
 #include <linux/spi/libertas_spi.h>
 #include <linux/spi/spi.h>
@@ -34,6 +32,12 @@
 #include "dev.h"
 #include "if_spi.h"
+struct if_spi_packet {
+        struct list_head                list;
+        u16                             blen;
+        u8                              buffer[0] __attribute__((aligned(4)));
+};
 struct if_spi_card {
        struct spi_device               *spi;
        struct lbs_private              *priv;
@@ -51,18 +55,36 @@ struct if_spi_card {
        unsigned long                   spu_reg_delay;
        /* Handles all SPI communication (except for FW load) */
-        struct task_struct              *spi_thread;
+        struct workqueue_struct         *workqueue;
-        int                             run_thread;
+        struct work_struct              packet_work;
-        /* Used to wake up the spi_thread */
-        struct semaphore                spi_ready;
-        struct semaphore                spi_thread_terminated;
        u8                              cmd_buffer[IF_SPI_CMD_BUF_SIZE];
+        /* A buffer of incoming packets from libertas core.
+         * Since we can't sleep in hw_host_to_card, we have to buffer
+         * them. */
+        struct list_head                cmd_packet_list;
+        struct list_head                data_packet_list;
+        /* Protects cmd_packet_list and data_packet_list */
+        spinlock_t                      buffer_lock;
 };
 static void free_if_spi_card(struct if_spi_card *card)
 {
+        struct list_head *cursor, *next;
+        struct if_spi_packet *packet;
+        list_for_each_safe(cursor, next, &card->cmd_packet_list) {
+                packet = container_of(cursor, struct if_spi_packet, list);
+                list_del(&packet->list);
+                kfree(packet);
+        }
+        list_for_each_safe(cursor, next, &card->data_packet_list) {
+                packet = container_of(cursor, struct if_spi_packet, list);
+                list_del(&packet->list);
+                kfree(packet);
+        }
        spi_set_drvdata(card->spi, NULL);
        kfree(card);
 }
@@ -622,7 +644,7 @@ out:
 /*
 * SPI Transfer Thread
 *
- * The SPI thread handles all SPI transfers, so there is no need for a lock.
+ * The SPI worker handles all SPI transfers, so there is no need for a lock.
 */
 /* Move a command from the card to the host */
@@ -742,6 +764,40 @@ out:
        return err;
 }
+/* Move data or a command from the host to the card. */
+static void if_spi_h2c(struct if_spi_card *card,
+                        struct if_spi_packet *packet, int type)
+{
+        int err = 0;
+        u16 int_type, port_reg;
+        switch (type) {
+        case MVMS_DAT:
+                int_type = IF_SPI_CIC_TX_DOWNLOAD_OVER;
+                port_reg = IF_SPI_DATA_RDWRPORT_REG;
+                break;
+        case MVMS_CMD:
+                int_type = IF_SPI_CIC_CMD_DOWNLOAD_OVER;
+                port_reg = IF_SPI_CMD_RDWRPORT_REG;
+                break;
+        default:
+                lbs_pr_err("can't transfer buffer of type %d\n", type);
+                err = -EINVAL;
+                goto out;
+        }
+        /* Write the data to the card */
+        err = spu_write(card, port_reg, packet->buffer, packet->blen);
+        if (err)
+                goto out;
+out:
+        kfree(packet);
+        if (err)
+                lbs_pr_err("%s: error %d\n", __func__, err);
+}
 /* Inform the host about a card event */
 static void if_spi_e2h(struct if_spi_card *card)
 {
@@ -766,71 +822,88 @@ out:
                lbs_pr_err("%s: error %d\n", __func__, err);
 }
-static int lbs_spi_thread(void *data)
+static void if_spi_host_to_card_worker(struct work_struct *work)
 {
        int err;
-        struct if_spi_card *card = data;
+        struct if_spi_card *card;
        u16 hiStatus;
+        unsigned long flags;
+        struct if_spi_packet *packet;
-        while (1) {
+        card = container_of(work, struct if_spi_card, packet_work);
-                /* Wait to be woken up by one of two things.  First, our ISR
-                 * could tell us that something happened on the WLAN.
-                 * Secondly, libertas could call hw_host_to_card with more
-                 * data, which we might be able to send.
-                 */
-                do {
-                        err = down_interruptible(&card->spi_ready);
-                        if (!card->run_thread) {
-                                up(&card->spi_thread_terminated);
-                                do_exit(0);
-                        }
-                } while (err == -EINTR);
-                /* Read the host interrupt status register to see what we
+        lbs_deb_enter(LBS_DEB_SPI);
-                 * can do. */
-                err = spu_read_u16(card, IF_SPI_HOST_INT_STATUS_REG,
+        /* Read the host interrupt status register to see what we
-                                        &hiStatus);
+         * can do. */
-                if (err) {
+        err = spu_read_u16(card, IF_SPI_HOST_INT_STATUS_REG,
-                        lbs_pr_err("I/O error\n");
+                                &hiStatus);
+        if (err) {
+                lbs_pr_err("I/O error\n");
+                goto err;
+        }
+        if (hiStatus & IF_SPI_HIST_CMD_UPLOAD_RDY) {
+                err = if_spi_c2h_cmd(card);
+                if (err)
                        goto err;
-                }
+        }
+        if (hiStatus & IF_SPI_HIST_RX_UPLOAD_RDY) {
+                err = if_spi_c2h_data(card);
+                if (err)
+                        goto err;
+        }
-                if (hiStatus & IF_SPI_HIST_CMD_UPLOAD_RDY) {
+        /* workaround: in PS mode, the card does not set the Command
-                        err = if_spi_c2h_cmd(card);
+         * Download Ready bit, but it sets TX Download Ready. */
-                        if (err)
+        if (hiStatus & IF_SPI_HIST_CMD_DOWNLOAD_RDY ||
-                                goto err;
+           (card->priv->psstate != PS_STATE_FULL_POWER &&
-                }
+            (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY))) {
-                if (hiStatus & IF_SPI_HIST_RX_UPLOAD_RDY) {
+                /* This means two things. First of all,
-                        err = if_spi_c2h_data(card);
+                 * if there was a previous command sent, the card has
-                        if (err)
+                 * successfully received it.
-                                goto err;
+                 * Secondly, it is now ready to download another
+                 * command.
+                 */
+                lbs_host_to_card_done(card->priv);
+                /* Do we have any command packets from the host to
+                 * send? */
+                packet = NULL;
+                spin_lock_irqsave(&card->buffer_lock, flags);
+                if (!list_empty(&card->cmd_packet_list)) {
+                        packet = (struct if_spi_packet *)(card->
+                                        cmd_packet_list.next);
+                        list_del(&packet->list);
                }
+                spin_unlock_irqrestore(&card->buffer_lock, flags);
-                /* workaround: in PS mode, the card does not set the Command
+                if (packet)
-                 * Download Ready bit, but it sets TX Download Ready. */
+                        if_spi_h2c(card, packet, MVMS_CMD);
-                if (hiStatus & IF_SPI_HIST_CMD_DOWNLOAD_RDY ||
+        }
-                   (card->priv->psstate != PS_STATE_FULL_POWER &&
+        if (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY) {
-                    (hiStatus & IF_SPI_HIST_TX_DOWNLOAD_RDY))) {
+                /* Do we have any data packets from the host to
-                        lbs_host_to_card_done(card->priv);
+                 * send? */
+                packet = NULL;
+                spin_lock_irqsave(&card->buffer_lock, flags);
+                if (!list_empty(&card->data_packet_list)) {
+                        packet = (struct if_spi_packet *)(card->
+                                        data_packet_list.next);
+                        list_del(&packet->list);
                }
+                spin_unlock_irqrestore(&card->buffer_lock, flags);
-                if (hiStatus & IF_SPI_HIST_CARD_EVENT)
+                if (packet)
-                        if_spi_e2h(card);
+                        if_spi_h2c(card, packet, MVMS_DAT);
+        }
+        if (hiStatus & IF_SPI_HIST_CARD_EVENT)
+                if_spi_e2h(card);
 err:
-                if (err)
+        if (err)
-                        lbs_pr_err("%s: got error %d\n", __func__, err);
+                lbs_pr_err("%s: got error %d\n", __func__, err);
-        }
-}
-/* Block until lbs_spi_thread thread has terminated */
+        lbs_deb_leave(LBS_DEB_SPI);
-static void if_spi_terminate_spi_thread(struct if_spi_card *card)
-{
-        /* It would be nice to use kthread_stop here, but that function
-         * can't wake threads waiting for a semaphore. */
-        card->run_thread = 0;
-        up(&card->spi_ready);
-        down(&card->spi_thread_terminated);
 }
 /*
@@ -842,18 +915,40 @@ static int if_spi_host_to_card(struct lbs_private *priv,
                                u8 type, u8 *buf, u16 nb)
 {
        int err = 0;
+        unsigned long flags;
        struct if_spi_card *card = priv->card;
+        struct if_spi_packet *packet;
+        u16 blen;
        lbs_deb_enter_args(LBS_DEB_SPI, "type %d, bytes %d", type, nb);
-        nb = ALIGN(nb, 4);
+        if (nb == 0) {
+                lbs_pr_err("%s: invalid size requested: %d\n", __func__, nb);
+                err = -EINVAL;
+                goto out;
+        }
+        blen = ALIGN(nb, 4);
+        packet = kzalloc(sizeof(struct if_spi_packet) + blen, GFP_ATOMIC);
+        if (!packet) {
+                err = -ENOMEM;
+                goto out;
+        }
+        packet->blen = blen;
+        memcpy(packet->buffer, buf, nb);
+        memset(packet->buffer + nb, 0, blen - nb);
        switch (type) {
        case MVMS_CMD:
-                err = spu_write(card, IF_SPI_CMD_RDWRPORT_REG, buf, nb);
+                priv->dnld_sent = DNLD_CMD_SENT;
+                spin_lock_irqsave(&card->buffer_lock, flags);
+                list_add_tail(&packet->list, &card->cmd_packet_list);
+                spin_unlock_irqrestore(&card->buffer_lock, flags);
                break;
        case MVMS_DAT:
-                err = spu_write(card, IF_SPI_DATA_RDWRPORT_REG, buf, nb);
+                priv->dnld_sent = DNLD_DATA_SENT;
+                spin_lock_irqsave(&card->buffer_lock, flags);
+                list_add_tail(&packet->list, &card->data_packet_list);
+                spin_unlock_irqrestore(&card->buffer_lock, flags);
                break;
        default:
                lbs_pr_err("can't transfer buffer of type %d", type);
@@ -861,6 +956,9 @@ static int if_spi_host_to_card(struct lbs_private *priv,
                break;
        }
+        /* Queue spi xfer work */
+        queue_work(card->workqueue, &card->packet_work);
+out:
        lbs_deb_leave_args(LBS_DEB_SPI, "err=%d", err);
        return err;
 }
@@ -869,13 +967,14 @@ static int if_spi_host_to_card(struct lbs_private *priv,
 * Host Interrupts
 *
 * Service incoming interrupts from the WLAN device. We can't sleep here, so
- * don't try to talk on the SPI bus, just wake up the SPI thread.
+ * don't try to talk on the SPI bus, just queue the SPI xfer work.
 */
 static irqreturn_t if_spi_host_interrupt(int irq, void *dev_id)
 {
        struct if_spi_card *card = dev_id;
-        up(&card->spi_ready);
+        queue_work(card->workqueue, &card->packet_work);
        return IRQ_HANDLED;
 }
@@ -883,56 +982,26 @@ static irqreturn_t if_spi_host_interrupt(int irq, void *dev_id)
 * SPI callbacks
 */
-static int __devinit if_spi_probe(struct spi_device *spi)
+static int if_spi_init_card(struct if_spi_card *card)
 {
-        struct if_spi_card *card;
+        struct spi_device *spi = card->spi;
-        struct lbs_private *priv = NULL;
+        int err, i;
-        struct libertas_spi_platform_data *pdata = spi->dev.platform_data;
-        int err = 0, i;
        u32 scratch;
-        struct sched_param param = { .sched_priority = 1 };
        const struct firmware *helper = NULL;
        const struct firmware *mainfw = NULL;
        lbs_deb_enter(LBS_DEB_SPI);
-        if (!pdata) {
+        err = spu_init(card, card->pdata->use_dummy_writes);
-                err = -EINVAL;
-                goto out;
-        }
-        if (pdata->setup) {
-                err = pdata->setup(spi);
-                if (err)
-                        goto out;
-        }
-        /* Allocate card structure to represent this specific device */
-        card = kzalloc(sizeof(struct if_spi_card), GFP_KERNEL);
-        if (!card) {
-                err = -ENOMEM;
-                goto out;
-        }
-        spi_set_drvdata(spi, card);
-        card->pdata = pdata;
-        card->spi = spi;
-        card->prev_xfer_time = jiffies;
-        sema_init(&card->spi_ready, 0);
-        sema_init(&card->spi_thread_terminated, 0);
-        /* Initialize the SPI Interface Unit */
-        err = spu_init(card, pdata->use_dummy_writes);
        if (err)
-                goto free_card;
+                goto out;
        err = spu_get_chip_revision(card, &card->card_id, &card->card_rev);
        if (err)
-                goto free_card;
+                goto out;
-        /* Firmware load */
        err = spu_read_u32(card, IF_SPI_SCRATCH_4_REG, &scratch);
        if (err)
-                goto free_card;
+                goto out;
        if (scratch == SUCCESSFUL_FW_DOWNLOAD_MAGIC)
                lbs_deb_spi("Firmware is already loaded for "
                            "Marvell WLAN 802.11 adapter\n");
@@ -946,7 +1015,7 @@ static int __devinit if_spi_probe(struct spi_device *spi)
                        lbs_pr_err("Unsupported chip_id: 0x%02x\n",
                                        card->card_id);
                        err = -ENODEV;
-                        goto free_card;
+                        goto out;
                }
                err = lbs_get_firmware(&card->spi->dev, NULL, NULL,
@@ -954,7 +1023,7 @@ static int __devinit if_spi_probe(struct spi_device *spi)
                                        &mainfw);
                if (err) {
                        lbs_pr_err("failed to find firmware (%d)\n", err);
-                        goto free_card;
+                        goto out;
                }
                lbs_deb_spi("Initializing FW for Marvell WLAN 802.11 adapter "
@@ -966,15 +1035,68 @@ static int __devinit if_spi_probe(struct spi_device *spi)
                                spi->max_speed_hz);
                err = if_spi_prog_helper_firmware(card, helper);
                if (err)
-                        goto free_card;
+                        goto out;
                err = if_spi_prog_main_firmware(card, mainfw);
                if (err)
-                        goto free_card;
+                        goto out;
                lbs_deb_spi("loaded FW for Marvell WLAN 802.11 adapter\n");
        }
        err = spu_set_interrupt_mode(card, 0, 1);
        if (err)
+                goto out;
+out:
+        if (helper)
+                release_firmware(helper);
+        if (mainfw)
+                release_firmware(mainfw);
+        lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
+        return err;
+}
+static int __devinit if_spi_probe(struct spi_device *spi)
+{
+        struct if_spi_card *card;
+        struct lbs_private *priv = NULL;
+        struct libertas_spi_platform_data *pdata = spi->dev.platform_data;
+        int err = 0;
+        lbs_deb_enter(LBS_DEB_SPI);
+        if (!pdata) {
+                err = -EINVAL;
+                goto out;
+        }
+        if (pdata->setup) {
+                err = pdata->setup(spi);
+                if (err)
+                        goto out;
+        }
+        /* Allocate card structure to represent this specific device */
+        card = kzalloc(sizeof(struct if_spi_card), GFP_KERNEL);
+        if (!card) {
+                err = -ENOMEM;
+                goto teardown;
+        }
+        spi_set_drvdata(spi, card);
+        card->pdata = pdata;
+        card->spi = spi;
+        card->prev_xfer_time = jiffies;
+        INIT_LIST_HEAD(&card->cmd_packet_list);
+        INIT_LIST_HEAD(&card->data_packet_list);
+        spin_lock_init(&card->buffer_lock);
+        /* Initialize the SPI Interface Unit */
+        /* Firmware load */
+        err = if_spi_init_card(card);
+        if (err)
                goto free_card;
        /* Register our card with libertas.
@@ -993,27 +1115,16 @@ static int __devinit if_spi_probe(struct spi_device *spi)
        priv->fw_ready = 1;
        /* Initialize interrupt handling stuff. */
-        card->run_thread = 1;
+        card->workqueue = create_workqueue("libertas_spi");
-        card->spi_thread = kthread_run(lbs_spi_thread, card, "lbs_spi_thread");
+        INIT_WORK(&card->packet_work, if_spi_host_to_card_worker);
-        if (IS_ERR(card->spi_thread)) {
-                card->run_thread = 0;
-                err = PTR_ERR(card->spi_thread);
-                lbs_pr_err("error creating SPI thread: err=%d\n", err);
-                goto remove_card;
-        }
-        if (sched_setscheduler(card->spi_thread, SCHED_FIFO, &param))
-                lbs_pr_err("Error setting scheduler, using default.\n");
        err = request_irq(spi->irq, if_spi_host_interrupt,
                        IRQF_TRIGGER_FALLING, "libertas_spi", card);
        if (err) {
                lbs_pr_err("can't get host irq line-- request_irq failed\n");
-                goto terminate_thread;
+                goto terminate_workqueue;
        }
-        /* poke the IRQ handler so that we don't miss the first interrupt */
-        up(&card->spi_ready);
        /* Start the card.
         * This will call register_netdev, and we'll start
         * getting interrupts... */
@@ -1028,18 +1139,16 @@ static int __devinit if_spi_probe(struct spi_device *spi)
 release_irq:
        free_irq(spi->irq, card);
-terminate_thread:
+terminate_workqueue:
-        if_spi_terminate_spi_thread(card);
+        flush_workqueue(card->workqueue);
-remove_card:
+        destroy_workqueue(card->workqueue);
        lbs_remove_card(priv); /* will call free_netdev */
 free_card:
        free_if_spi_card(card);
+teardown:
+        if (pdata->teardown)
+                pdata->teardown(spi);
 out:
-        if (helper)
-                release_firmware(helper);
-        if (mainfw)
-                release_firmware(mainfw);
        lbs_deb_leave_args(LBS_DEB_SPI, "err %d\n", err);
        return err;
 }
@@ -1056,7 +1165,8 @@ static int __devexit libertas_spi_remove(struct spi_device *spi)
        lbs_remove_card(priv); /* will call free_netdev */
        free_irq(spi->irq, card);
-        if_spi_terminate_spi_thread(card);
+        flush_workqueue(card->workqueue);
+        destroy_workqueue(card->workqueue);
        if (card->pdata->teardown)
                card->pdata->teardown(spi);
        free_if_spi_card(card);
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 6836a6dd9853..ca8149cd5bd9 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -539,6 +539,43 @@ static int lbs_thread(void *data)
        return 0;
 }
+/**
+ * @brief This function gets the HW spec from the firmware and sets
+ *        some basic parameters.
+ *
+ *  @param priv    A pointer to struct lbs_private structure
+ *  @return        0 or -1
+ */
+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 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;
+        }
+        /* Send cmd to FW to enable 11D function */
+        ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_11D_ENABLE, 1);
+        lbs_set_mac_control(priv);
+done:
+        lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
+        return ret;
+}
 int lbs_suspend(struct lbs_private *priv)
 {
        int ret;
@@ -584,47 +621,13 @@ int lbs_resume(struct lbs_private *priv)
                        lbs_pr_err("deep sleep activation failed: %d\n", ret);
        }
-        lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
+        if (priv->setup_fw_on_resume)
-        return ret;
+                ret = lbs_setup_firmware(priv);
-}
-EXPORT_SYMBOL_GPL(lbs_resume);
-/**
- * @brief This function gets the HW spec from the firmware and sets
- *        some basic parameters.
- *
- *  @param priv    A pointer to struct lbs_private structure
- *  @return        0 or -1
- */
-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 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;
-        }
-        /* Send cmd to FW to enable 11D function */
-        ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_11D_ENABLE, 1);
-        lbs_set_mac_control(priv);
-done:
        lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
        return ret;
 }
+EXPORT_SYMBOL_GPL(lbs_resume);
 /**
 *  This function handles the timeout of command sending.
diff --git a/drivers/net/wireless/wl1251/acx.c b/drivers/net/wireless/wl1251/acx.c
index 64a0214cfb29..ef8370edace7 100644
--- a/drivers/net/wireless/wl1251/acx.c
+++ b/drivers/net/wireless/wl1251/acx.c
@@ -776,6 +776,31 @@ out:
        return ret;
 }
+int wl1251_acx_low_rssi(struct wl1251 *wl, s8 threshold, u8 weight,
+                        u8 depth, enum wl1251_acx_low_rssi_type type)
+{
+        struct acx_low_rssi *rssi;
+        int ret;
+        wl1251_debug(DEBUG_ACX, "acx low rssi");
+        rssi = kzalloc(sizeof(*rssi), GFP_KERNEL);
+        if (!rssi)
+                return -ENOMEM;
+        rssi->threshold = threshold;
+        rssi->weight = weight;
+        rssi->depth = depth;
+        rssi->type = type;
+        ret = wl1251_cmd_configure(wl, ACX_LOW_RSSI, rssi, sizeof(*rssi));
+        if (ret < 0)
+                wl1251_warning("failed to set low rssi threshold: %d", ret);
+        kfree(rssi);
+        return ret;
+}
 int wl1251_acx_set_preamble(struct wl1251 *wl, enum acx_preamble_type preamble)
 {
        struct acx_preamble *acx;
@@ -978,6 +1003,34 @@ out:
        return ret;
 }
+int wl1251_acx_bet_enable(struct wl1251 *wl, enum wl1251_acx_bet_mode mode,
+                          u8 max_consecutive)
+{
+        struct wl1251_acx_bet_enable *acx;
+        int ret;
+        wl1251_debug(DEBUG_ACX, "acx bet enable");
+        acx = kzalloc(sizeof(*acx), GFP_KERNEL);
+        if (!acx) {
+                ret = -ENOMEM;
+                goto out;
+        }
+        acx->enable = mode;
+        acx->max_consecutive = max_consecutive;
+        ret = wl1251_cmd_configure(wl, ACX_BET_ENABLE, acx, sizeof(*acx));
+        if (ret < 0) {
+                wl1251_warning("wl1251 acx bet enable failed: %d", ret);
+                goto out;
+        }
+out:
+        kfree(acx);
+        return ret;
+}
 int wl1251_acx_ac_cfg(struct wl1251 *wl, u8 ac, u8 cw_min, u16 cw_max,
                      u8 aifs, u16 txop)
 {
diff --git a/drivers/net/wireless/wl1251/acx.h b/drivers/net/wireless/wl1251/acx.h
index efcc3aaca14f..c2ba100f9b1a 100644
--- a/drivers/net/wireless/wl1251/acx.h
+++ b/drivers/net/wireless/wl1251/acx.h
@@ -399,6 +399,49 @@ struct acx_rts_threshold {
        u8 pad[2];
 } __packed;
+enum wl1251_acx_low_rssi_type {
+        /*
+         * The event is a "Level" indication which keeps triggering
+         * as long as the average RSSI is below the threshold.
+         */
+        WL1251_ACX_LOW_RSSI_TYPE_LEVEL = 0,
+        /*
+         * The event is an "Edge" indication which triggers
+         * only when the RSSI threshold is crossed from above.
+         */
+        WL1251_ACX_LOW_RSSI_TYPE_EDGE = 1,
+};
+struct acx_low_rssi {
+        struct acx_header header;
+        /*
+         * The threshold (in dBm) below (or above after low rssi
+         * indication) which the firmware generates an interrupt to the
+         * host. This parameter is signed.
+         */
+        s8 threshold;
+        /*
+         * The weight of the current RSSI sample, before adding the new
+         * sample, that is used to calculate the average RSSI.
+         */
+        u8 weight;
+        /*
+         * The number of Beacons/Probe response frames that will be
+         * received before issuing the Low or Regained RSSI event.
+         */
+        u8 depth;
+        /*
+         * Configures how the Low RSSI Event is triggered. Refer to
+         * enum wl1251_acx_low_rssi_type for more.
+         */
+        u8 type;
+} __packed;
 struct acx_beacon_filter_option {
        struct acx_header header;
@@ -1164,6 +1207,31 @@ struct wl1251_acx_wr_tbtt_and_dtim {
        u8  padding;
 } __packed;
+enum wl1251_acx_bet_mode {
+        WL1251_ACX_BET_DISABLE = 0,
+        WL1251_ACX_BET_ENABLE = 1,
+};
+struct wl1251_acx_bet_enable {
+        struct acx_header header;
+        /*
+         * Specifies if beacon early termination procedure is enabled or
+         * disabled, see enum wl1251_acx_bet_mode.
+         */
+        u8 enable;
+        /*
+         * Specifies the maximum number of consecutive beacons that may be
+         * early terminated. After this number is reached at least one full
+         * beacon must be correctly received in FW before beacon ET
+         * resumes. Range 0 - 255.
+         */
+        u8 max_consecutive;
+        u8 padding[2];
+} __packed;
 struct wl1251_acx_ac_cfg {
        struct acx_header header;
@@ -1393,6 +1461,8 @@ int wl1251_acx_cca_threshold(struct wl1251 *wl);
 int wl1251_acx_bcn_dtim_options(struct wl1251 *wl);
 int wl1251_acx_aid(struct wl1251 *wl, u16 aid);
 int wl1251_acx_event_mbox_mask(struct wl1251 *wl, u32 event_mask);
+int wl1251_acx_low_rssi(struct wl1251 *wl, s8 threshold, u8 weight,
+                        u8 depth, enum wl1251_acx_low_rssi_type type);
 int wl1251_acx_set_preamble(struct wl1251 *wl, enum acx_preamble_type preamble);
 int wl1251_acx_cts_protect(struct wl1251 *wl,
                            enum acx_ctsprotect_type ctsprotect);
@@ -1401,6 +1471,8 @@ int wl1251_acx_tsf_info(struct wl1251 *wl, u64 *mactime);
 int wl1251_acx_rate_policies(struct wl1251 *wl);
 int wl1251_acx_mem_cfg(struct wl1251 *wl);
 int wl1251_acx_wr_tbtt_and_dtim(struct wl1251 *wl, u16 tbtt, u8 dtim);
+int wl1251_acx_bet_enable(struct wl1251 *wl, enum wl1251_acx_bet_mode mode,
+                          u8 max_consecutive);
 int wl1251_acx_ac_cfg(struct wl1251 *wl, u8 ac, u8 cw_min, u16 cw_max,
                      u8 aifs, u16 txop);
 int wl1251_acx_tid_cfg(struct wl1251 *wl, u8 queue,
diff --git a/drivers/net/wireless/wl1251/event.c b/drivers/net/wireless/wl1251/event.c
index 712372e50a87..dfc4579acb06 100644
--- a/drivers/net/wireless/wl1251/event.c
+++ b/drivers/net/wireless/wl1251/event.c
@@ -90,6 +90,24 @@ static int wl1251_event_process(struct wl1251 *wl, struct event_mailbox *mbox)
                }
        }
+        if (wl->vif && wl->rssi_thold) {
+                if (vector & ROAMING_TRIGGER_LOW_RSSI_EVENT_ID) {
+                        wl1251_debug(DEBUG_EVENT,
+                                     "ROAMING_TRIGGER_LOW_RSSI_EVENT");
+                        ieee80211_cqm_rssi_notify(wl->vif,
+                                NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW,
+                                GFP_KERNEL);
+                }
+                if (vector & ROAMING_TRIGGER_REGAINED_RSSI_EVENT_ID) {
+                        wl1251_debug(DEBUG_EVENT,
+                                     "ROAMING_TRIGGER_REGAINED_RSSI_EVENT");
+                        ieee80211_cqm_rssi_notify(wl->vif,
+                                NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH,
+                                GFP_KERNEL);
+                }
+        }
        return 0;
 }
diff --git a/drivers/net/wireless/wl1251/main.c b/drivers/net/wireless/wl1251/main.c
index 012e1a4016fe..1c8b0775d849 100644
--- a/drivers/net/wireless/wl1251/main.c
+++ b/drivers/net/wireless/wl1251/main.c
@@ -502,6 +502,7 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
        wl->psm = 0;
        wl->tx_queue_stopped = false;
        wl->power_level = WL1251_DEFAULT_POWER_LEVEL;
+        wl->rssi_thold = 0;
        wl->channel = WL1251_DEFAULT_CHANNEL;
        wl1251_debugfs_reset(wl);
@@ -959,6 +960,16 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
        if (ret < 0)
                goto out;
+        if (changed & BSS_CHANGED_CQM) {
+                ret = wl1251_acx_low_rssi(wl, bss_conf->cqm_rssi_thold,
+                                          WL1251_DEFAULT_LOW_RSSI_WEIGHT,
+                                          WL1251_DEFAULT_LOW_RSSI_DEPTH,
+                                          WL1251_ACX_LOW_RSSI_TYPE_EDGE);
+                if (ret < 0)
+                        goto out;
+                wl->rssi_thold = bss_conf->cqm_rssi_thold;
+        }
        if (changed & BSS_CHANGED_BSSID) {
                memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
@@ -1310,9 +1321,11 @@ int wl1251_init_ieee80211(struct wl1251 *wl)
        wl->hw->flags = IEEE80211_HW_SIGNAL_DBM |
                IEEE80211_HW_SUPPORTS_PS |
                IEEE80211_HW_BEACON_FILTER |
-                IEEE80211_HW_SUPPORTS_UAPSD;
+                IEEE80211_HW_SUPPORTS_UAPSD |
+                IEEE80211_HW_SUPPORTS_CQM_RSSI;
-        wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
+        wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
+                                         BIT(NL80211_IFTYPE_ADHOC);
        wl->hw->wiphy->max_scan_ssids = 1;
        wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &wl1251_band_2ghz;
@@ -1374,6 +1387,7 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
        wl->psm_requested = false;
        wl->tx_queue_stopped = false;
        wl->power_level = WL1251_DEFAULT_POWER_LEVEL;
+        wl->rssi_thold = 0;
        wl->beacon_int = WL1251_DEFAULT_BEACON_INT;
        wl->dtim_period = WL1251_DEFAULT_DTIM_PERIOD;
        wl->vif = NULL;
diff --git a/drivers/net/wireless/wl1251/ps.c b/drivers/net/wireless/wl1251/ps.c
index 5ed47c8373d2..9ba23ede51bd 100644
--- a/drivers/net/wireless/wl1251/ps.c
+++ b/drivers/net/wireless/wl1251/ps.c
@@ -153,6 +153,11 @@ int wl1251_ps_set_mode(struct wl1251 *wl, enum wl1251_cmd_ps_mode mode)
                if (ret < 0)
                        return ret;
+                ret = wl1251_acx_bet_enable(wl, WL1251_ACX_BET_ENABLE,
+                                            WL1251_DEFAULT_BET_CONSECUTIVE);
+                if (ret < 0)
+                        return ret;
                ret = wl1251_cmd_ps_mode(wl, STATION_POWER_SAVE_MODE);
                if (ret < 0)
                        return ret;
@@ -170,6 +175,12 @@ int wl1251_ps_set_mode(struct wl1251 *wl, enum wl1251_cmd_ps_mode mode)
                if (ret < 0)
                        return ret;
+                /* disable BET */
+                ret = wl1251_acx_bet_enable(wl, WL1251_ACX_BET_DISABLE,
+                                            WL1251_DEFAULT_BET_CONSECUTIVE);
+                if (ret < 0)
+                        return ret;
                /* disable beacon filtering */
                ret = wl1251_acx_beacon_filter_opt(wl, false);
                if (ret < 0)
diff --git a/drivers/net/wireless/wl1251/rx.c b/drivers/net/wireless/wl1251/rx.c
index 86eef456d7b2..b659e15c78df 100644
--- a/drivers/net/wireless/wl1251/rx.c
+++ b/drivers/net/wireless/wl1251/rx.c
@@ -96,8 +96,52 @@ static void wl1251_rx_status(struct wl1251 *wl,
        if (unlikely(!(desc->flags & RX_DESC_VALID_FCS)))
                status->flag |= RX_FLAG_FAILED_FCS_CRC;
+        switch (desc->rate) {
+                /* skip 1 and 12 Mbps because they have same value 0x0a */
+        case RATE_2MBPS:
+                status->rate_idx = 1;
+                break;
+        case RATE_5_5MBPS:
+                status->rate_idx = 2;
+                break;
+        case RATE_11MBPS:
+                status->rate_idx = 3;
+                break;
+        case RATE_6MBPS:
+                status->rate_idx = 4;
+                break;
+        case RATE_9MBPS:
+                status->rate_idx = 5;
+                break;
+        case RATE_18MBPS:
+                status->rate_idx = 7;
+                break;
+        case RATE_24MBPS:
+                status->rate_idx = 8;
+                break;
+        case RATE_36MBPS:
+                status->rate_idx = 9;
+                break;
+        case RATE_48MBPS:
+                status->rate_idx = 10;
+                break;
+        case RATE_54MBPS:
+                status->rate_idx = 11;
+                break;
+        }
+        /* for 1 and 12 Mbps we have to check the modulation */
+        if (desc->rate == RATE_1MBPS) {
+                if (!(desc->mod_pre & OFDM_RATE_BIT))
+                        /* CCK -> RATE_1MBPS */
+                        status->rate_idx = 0;
+                else
+                        /* OFDM -> RATE_12MBPS */
+                        status->rate_idx = 6;
+        }
-        /* FIXME: set status->rate_idx */
+        if (desc->mod_pre & SHORT_PREAMBLE_BIT)
+                status->flag |= RX_FLAG_SHORTPRE;
 }
 static void wl1251_rx_body(struct wl1251 *wl,
diff --git a/drivers/net/wireless/wl1251/tx.c b/drivers/net/wireless/wl1251/tx.c
index 554b4f9a3d3e..28121c590a2b 100644
--- a/drivers/net/wireless/wl1251/tx.c
+++ b/drivers/net/wireless/wl1251/tx.c
@@ -213,16 +213,30 @@ static int wl1251_tx_send_packet(struct wl1251 *wl, struct sk_buff *skb,
                wl1251_debug(DEBUG_TX, "skb offset %d", offset);
                /* check whether the current skb can be used */
-                if (!skb_cloned(skb) && (skb_tailroom(skb) >= offset)) {
+                if (skb_cloned(skb) || (skb_tailroom(skb) < offset)) {
-                        unsigned char *src = skb->data;
+                        struct sk_buff *newskb = skb_copy_expand(skb, 0, 3,
+                                                                 GFP_KERNEL);
+                        if (unlikely(newskb == NULL)) {
+                                wl1251_error("Can't allocate skb!");
+                                return -EINVAL;
+                        }
-                        /* align the buffer on a 4-byte boundary */
+                        tx_hdr = (struct tx_double_buffer_desc *) newskb->data;
+                        dev_kfree_skb_any(skb);
+                        wl->tx_frames[tx_hdr->id] = skb = newskb;
+                        offset = (4 - (long)skb->data) & 0x03;
+                        wl1251_debug(DEBUG_TX, "new skb offset %d", offset);
+                }
+                /* align the buffer on a 4-byte boundary */
+                if (offset) {
+                        unsigned char *src = skb->data;
                        skb_reserve(skb, offset);
                        memmove(skb->data, src, skb->len);
                        tx_hdr = (struct tx_double_buffer_desc *) skb->data;
-                } else {
-                        wl1251_info("No handler, fixme!");
-                        return -EINVAL;
                }
        }
@@ -368,7 +382,7 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
 {
        struct ieee80211_tx_info *info;
        struct sk_buff *skb;
-        int hdrlen, ret;
+        int hdrlen;
        u8 *frame;
        skb = wl->tx_frames[result->id];
@@ -407,40 +421,12 @@ static void wl1251_tx_packet_cb(struct wl1251 *wl,
        ieee80211_tx_status(wl->hw, skb);
        wl->tx_frames[result->id] = NULL;
-        if (wl->tx_queue_stopped) {
-                wl1251_debug(DEBUG_TX, "cb: queue was stopped");
-                skb = skb_dequeue(&wl->tx_queue);
-                /* The skb can be NULL because tx_work might have been
-                   scheduled before the queue was stopped making the
-                   queue empty */
-                if (skb) {
-                        ret = wl1251_tx_frame(wl, skb);
-                        if (ret == -EBUSY) {
-                                /* firmware buffer is still full */
-                                wl1251_debug(DEBUG_TX, "cb: fw buffer "
-                                             "still full");
-                                skb_queue_head(&wl->tx_queue, skb);
-                                return;
-                        } else if (ret < 0) {
-                                dev_kfree_skb(skb);
-                                return;
-                        }
-                }
-                wl1251_debug(DEBUG_TX, "cb: waking queues");
-                ieee80211_wake_queues(wl->hw);
-                wl->tx_queue_stopped = false;
-        }
 }
 /* Called upon reception of a TX complete interrupt */
 void wl1251_tx_complete(struct wl1251 *wl)
 {
-        int i, result_index, num_complete = 0;
+        int i, result_index, num_complete = 0, queue_len;
        struct tx_result result[FW_TX_CMPLT_BLOCK_SIZE], *result_ptr;
        unsigned long flags;
@@ -471,18 +457,22 @@ void wl1251_tx_complete(struct wl1251 *wl)
                }
        }
-        if (wl->tx_queue_stopped
+        queue_len = skb_queue_len(&wl->tx_queue);
-            &&
-            skb_queue_len(&wl->tx_queue) <= WL1251_TX_QUEUE_LOW_WATERMARK){
-                /* firmware buffer has space, restart queues */
+        if ((num_complete > 0) && (queue_len > 0)) {
+                /* firmware buffer has space, reschedule tx_work */
+                wl1251_debug(DEBUG_TX, "tx_complete: reschedule tx_work");
+                ieee80211_queue_work(wl->hw, &wl->tx_work);
+        }
+        if (wl->tx_queue_stopped &&
+            queue_len <= WL1251_TX_QUEUE_LOW_WATERMARK) {
+                /* tx_queue has space, restart queues */
                wl1251_debug(DEBUG_TX, "tx_complete: waking queues");
                spin_lock_irqsave(&wl->wl_lock, flags);
                ieee80211_wake_queues(wl->hw);
                wl->tx_queue_stopped = false;
                spin_unlock_irqrestore(&wl->wl_lock, flags);
-                ieee80211_queue_work(wl->hw, &wl->tx_work);
        }
        /* Every completed frame needs to be acknowledged */
diff --git a/drivers/net/wireless/wl1251/wl1251.h b/drivers/net/wireless/wl1251/wl1251.h
index c0ce2c8b43b8..bb23cd522b22 100644
--- a/drivers/net/wireless/wl1251/wl1251.h
+++ b/drivers/net/wireless/wl1251/wl1251.h
@@ -370,6 +370,8 @@ struct wl1251 {
        /* in dBm */
        int power_level;
+        int rssi_thold;
        struct wl1251_stats stats;
        struct wl1251_debugfs debugfs;
@@ -410,6 +412,8 @@ void wl1251_disable_interrupts(struct wl1251 *wl);
 #define WL1251_DEFAULT_CHANNEL 0
+#define WL1251_DEFAULT_BET_CONSECUTIVE 10
 #define CHIP_ID_1251_PG10                  (0x7010101)
 #define CHIP_ID_1251_PG11                  (0x7020101)
 #define CHIP_ID_1251_PG12                  (0x7030101)
@@ -431,4 +435,7 @@ void wl1251_disable_interrupts(struct wl1251 *wl);
 #define WL1251_PART_WORK_REG_START      REGISTERS_BASE
 #define WL1251_PART_WORK_REG_SIZE       REGISTERS_WORK_SIZE
+#define WL1251_DEFAULT_LOW_RSSI_WEIGHT          10
+#define WL1251_DEFAULT_LOW_RSSI_DEPTH           10
 #endif
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zd1211rw/zd_chip.c
index 6a9b66051cf7..54f68f134ea7 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zd1211rw/zd_chip.c
@@ -108,25 +108,17 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 {
        int r;
        int i;
-        zd_addr_t *a16;
+        zd_addr_t a16[USB_MAX_IOREAD32_COUNT * 2];
-        u16 *v16;
+        u16 v16[USB_MAX_IOREAD32_COUNT * 2];
        unsigned int count16;
        if (count > USB_MAX_IOREAD32_COUNT)
                return -EINVAL;
-        /* Allocate a single memory block for values and addresses. */
+        /* Use stack for values and addresses. */
-        count16 = 2*count;
+        count16 = 2 * count;
-        /* zd_addr_t is __nocast, so the kmalloc needs an explicit cast */
+        BUG_ON(count16 * sizeof(zd_addr_t) > sizeof(a16));
-        a16 = (zd_addr_t *) kmalloc(count16 * (sizeof(zd_addr_t) + sizeof(u16)),
+        BUG_ON(count16 * sizeof(u16) > sizeof(v16));
-                                   GFP_KERNEL);
-        if (!a16) {
-                dev_dbg_f(zd_chip_dev(chip),
-                          "error ENOMEM in allocation of a16\n");
-                r = -ENOMEM;
-                goto out;
-        }
-        v16 = (u16 *)(a16 + count16);
        for (i = 0; i < count; i++) {
                int j = 2*i;
@@ -139,7 +131,7 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
        if (r) {
                dev_dbg_f(zd_chip_dev(chip),
                          "error: zd_ioread16v_locked. Error number %d\n", r);
-                goto out;
+                return r;
        }
        for (i = 0; i < count; i++) {
@@ -147,18 +139,18 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
                values[i] = (v16[j] << 16) | v16[j+1];
        }
-out:
+        return 0;
-        kfree((void *)a16);
-        return r;
 }
 int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
                   unsigned int count)
 {
        int i, j, r;
-        struct zd_ioreq16 *ioreqs16;
+        struct zd_ioreq16 ioreqs16[USB_MAX_IOWRITE32_COUNT * 2];
        unsigned int count16;
+        /* Use stack for values and addresses. */
        ZD_ASSERT(mutex_is_locked(&chip->mutex));
        if (count == 0)
@@ -166,15 +158,8 @@ int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
        if (count > USB_MAX_IOWRITE32_COUNT)
                return -EINVAL;
-        /* Allocate a single memory block for values and addresses. */
+        count16 = 2 * count;
-        count16 = 2*count;
+        BUG_ON(count16 * sizeof(struct zd_ioreq16) > sizeof(ioreqs16));
-        ioreqs16 = kmalloc(count16 * sizeof(struct zd_ioreq16), GFP_KERNEL);
-        if (!ioreqs16) {
-                r = -ENOMEM;
-                dev_dbg_f(zd_chip_dev(chip),
-                          "error %d in ioreqs16 allocation\n", r);
-                goto out;
-        }
        for (i = 0; i < count; i++) {
                j = 2*i;
@@ -192,8 +177,6 @@ int _zd_iowrite32v_locked(struct zd_chip *chip, const struct zd_ioreq32 *ioreqs,
                          "error %d in zd_usb_write16v\n", r);
        }
 #endif /* DEBUG */
-out:
-        kfree(ioreqs16);
        return r;
 }
@@ -370,16 +353,12 @@ error:
        return r;
 }
-/* MAC address: if custom mac addresses are to be used CR_MAC_ADDR_P1 and
+static int zd_write_mac_addr_common(struct zd_chip *chip, const u8 *mac_addr,
- *              CR_MAC_ADDR_P2 must be overwritten
+                                    const struct zd_ioreq32 *in_reqs,
- */
+                                    const char *type)
-int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
 {
        int r;
-        struct zd_ioreq32 reqs[2] = {
+        struct zd_ioreq32 reqs[2] = {in_reqs[0], in_reqs[1]};
-                [0] = { .addr = CR_MAC_ADDR_P1 },
-                [1] = { .addr = CR_MAC_ADDR_P2 },
-        };
        if (mac_addr) {
                reqs[0].value = (mac_addr[3] << 24)
@@ -388,9 +367,9 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
                              |  mac_addr[0];
                reqs[1].value = (mac_addr[5] <<  8)
                              |  mac_addr[4];
-                dev_dbg_f(zd_chip_dev(chip), "mac addr %pM\n", mac_addr);
+                dev_dbg_f(zd_chip_dev(chip), "%s addr %pM\n", type, mac_addr);
        } else {
-                dev_dbg_f(zd_chip_dev(chip), "set NULL mac\n");
+                dev_dbg_f(zd_chip_dev(chip), "set NULL %s\n", type);
        }
        mutex_lock(&chip->mutex);
@@ -399,6 +378,29 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
        return r;
 }
+/* MAC address: if custom mac addresses are to be used CR_MAC_ADDR_P1 and
+ *              CR_MAC_ADDR_P2 must be overwritten
+ */
+int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
+{
+        static const struct zd_ioreq32 reqs[2] = {
+                [0] = { .addr = CR_MAC_ADDR_P1 },
+                [1] = { .addr = CR_MAC_ADDR_P2 },
+        };
+        return zd_write_mac_addr_common(chip, mac_addr, reqs, "mac");
+}
+int zd_write_bssid(struct zd_chip *chip, const u8 *bssid)
+{
+        static const struct zd_ioreq32 reqs[2] = {
+                [0] = { .addr = CR_BSSID_P1 },
+                [1] = { .addr = CR_BSSID_P2 },
+        };
+        return zd_write_mac_addr_common(chip, bssid, reqs, "bssid");
+}
 int zd_read_regdomain(struct zd_chip *chip, u8 *regdomain)
 {
        int r;
@@ -849,11 +851,12 @@ static int get_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 static int set_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 {
        struct zd_ioreq32 reqs[3];
+        u16 b_interval = s->beacon_interval & 0xffff;
-        if (s->beacon_interval <= 5)
+        if (b_interval <= 5)
-                s->beacon_interval = 5;
+                b_interval = 5;
-        if (s->pre_tbtt < 4 || s->pre_tbtt >= s->beacon_interval)
+        if (s->pre_tbtt < 4 || s->pre_tbtt >= b_interval)
-                s->pre_tbtt = s->beacon_interval - 1;
+                s->pre_tbtt = b_interval - 1;
        if (s->atim_wnd_period >= s->pre_tbtt)
                s->atim_wnd_period = s->pre_tbtt - 1;
@@ -862,31 +865,57 @@ static int set_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
        reqs[1].addr = CR_PRE_TBTT;
        reqs[1].value = s->pre_tbtt;
        reqs[2].addr = CR_BCN_INTERVAL;
-        reqs[2].value = s->beacon_interval;
+        reqs[2].value = (s->beacon_interval & ~0xffff) | b_interval;
        return zd_iowrite32a_locked(chip, reqs, ARRAY_SIZE(reqs));
 }
-static int set_beacon_interval(struct zd_chip *chip, u32 interval)
+static int set_beacon_interval(struct zd_chip *chip, u16 interval,
+                               u8 dtim_period, int type)
 {
        int r;
        struct aw_pt_bi s;
+        u32 b_interval, mode_flag;
        ZD_ASSERT(mutex_is_locked(&chip->mutex));
+        if (interval > 0) {
+                switch (type) {
+                case NL80211_IFTYPE_ADHOC:
+                case NL80211_IFTYPE_MESH_POINT:
+                        mode_flag = BCN_MODE_IBSS;
+                        break;
+                case NL80211_IFTYPE_AP:
+                        mode_flag = BCN_MODE_AP;
+                        break;
+                default:
+                        mode_flag = 0;
+                        break;
+                }
+        } else {
+                dtim_period = 0;
+                mode_flag = 0;
+        }
+        b_interval = mode_flag | (dtim_period << 16) | interval;
+        r = zd_iowrite32_locked(chip, b_interval, CR_BCN_INTERVAL);
+        if (r)
+                return r;
        r = get_aw_pt_bi(chip, &s);
        if (r)
                return r;
-        s.beacon_interval = interval;
        return set_aw_pt_bi(chip, &s);
 }
-int zd_set_beacon_interval(struct zd_chip *chip, u32 interval)
+int zd_set_beacon_interval(struct zd_chip *chip, u16 interval, u8 dtim_period,
+                           int type)
 {
        int r;
        mutex_lock(&chip->mutex);
-        r = set_beacon_interval(chip, interval);
+        r = set_beacon_interval(chip, interval, dtim_period, type);
        mutex_unlock(&chip->mutex);
        return r;
 }
@@ -905,7 +934,7 @@ static int hw_init(struct zd_chip *chip)
        if (r)
                return r;
-        return set_beacon_interval(chip, 100);
+        return set_beacon_interval(chip, 100, 0, NL80211_IFTYPE_UNSPECIFIED);
 }
 static zd_addr_t fw_reg_addr(struct zd_chip *chip, u16 offset)
@@ -1407,6 +1436,9 @@ void zd_chip_disable_int(struct zd_chip *chip)
        mutex_lock(&chip->mutex);
        zd_usb_disable_int(&chip->usb);
        mutex_unlock(&chip->mutex);
+        /* cancel pending interrupt work */
+        cancel_work_sync(&zd_chip_to_mac(chip)->process_intr);
 }
 int zd_chip_enable_rxtx(struct zd_chip *chip)
@@ -1416,6 +1448,7 @@ int zd_chip_enable_rxtx(struct zd_chip *chip)
        mutex_lock(&chip->mutex);
        zd_usb_enable_tx(&chip->usb);
        r = zd_usb_enable_rx(&chip->usb);
+        zd_tx_watchdog_enable(&chip->usb);
        mutex_unlock(&chip->mutex);
        return r;
 }
@@ -1423,6 +1456,7 @@ int zd_chip_enable_rxtx(struct zd_chip *chip)
 void zd_chip_disable_rxtx(struct zd_chip *chip)
 {
        mutex_lock(&chip->mutex);
+        zd_tx_watchdog_disable(&chip->usb);
        zd_usb_disable_rx(&chip->usb);
        zd_usb_disable_tx(&chip->usb);
        mutex_unlock(&chip->mutex);
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zd1211rw/zd_chip.h
index f8bbf7d302ae..14e4402a6111 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zd1211rw/zd_chip.h
@@ -546,6 +546,7 @@ enum {
 #define RX_FILTER_CTRL (RX_FILTER_RTS | RX_FILTER_CTS | \
        RX_FILTER_CFEND | RX_FILTER_CFACK)
+#define BCN_MODE_AP                     0x1000000
 #define BCN_MODE_IBSS                   0x2000000
 /* Monitor mode sets filter to 0xfffff */
@@ -881,6 +882,7 @@ static inline u8 _zd_chip_get_channel(struct zd_chip *chip)
 u8  zd_chip_get_channel(struct zd_chip *chip);
 int zd_read_regdomain(struct zd_chip *chip, u8 *regdomain);
 int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr);
+int zd_write_bssid(struct zd_chip *chip, const u8 *bssid);
 int zd_chip_switch_radio_on(struct zd_chip *chip);
 int zd_chip_switch_radio_off(struct zd_chip *chip);
 int zd_chip_enable_int(struct zd_chip *chip);
@@ -920,7 +922,8 @@ enum led_status {
 int zd_chip_control_leds(struct zd_chip *chip, enum led_status status);
-int zd_set_beacon_interval(struct zd_chip *chip, u32 interval);
+int zd_set_beacon_interval(struct zd_chip *chip, u16 interval, u8 dtim_period,
+                           int type);
 static inline int zd_get_beacon_interval(struct zd_chip *chip, u32 *interval)
 {
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 6107304cb94c..74a269ebbeb9 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -138,6 +138,12 @@ static const struct ieee80211_channel zd_channels[] = {
 static void housekeeping_init(struct zd_mac *mac);
 static void housekeeping_enable(struct zd_mac *mac);
 static void housekeeping_disable(struct zd_mac *mac);
+static void beacon_init(struct zd_mac *mac);
+static void beacon_enable(struct zd_mac *mac);
+static void beacon_disable(struct zd_mac *mac);
+static void set_rts_cts(struct zd_mac *mac, unsigned int short_preamble);
+static int zd_mac_config_beacon(struct ieee80211_hw *hw,
+                                struct sk_buff *beacon);
 static int zd_reg2alpha2(u8 regdomain, char *alpha2)
 {
@@ -231,6 +237,26 @@ static int set_rx_filter(struct zd_mac *mac)
        return zd_iowrite32(&mac->chip, CR_RX_FILTER, filter);
 }
+static int set_mac_and_bssid(struct zd_mac *mac)
+{
+        int r;
+        if (!mac->vif)
+                return -1;
+        r = zd_write_mac_addr(&mac->chip, mac->vif->addr);
+        if (r)
+                return r;
+        /* Vendor driver after setting MAC either sets BSSID for AP or
+         * filter for other modes.
+         */
+        if (mac->type != NL80211_IFTYPE_AP)
+                return set_rx_filter(mac);
+        else
+                return zd_write_bssid(&mac->chip, mac->vif->addr);
+}
 static int set_mc_hash(struct zd_mac *mac)
 {
        struct zd_mc_hash hash;
@@ -238,7 +264,7 @@ static int set_mc_hash(struct zd_mac *mac)
        return zd_chip_set_multicast_hash(&mac->chip, &hash);
 }
-static int zd_op_start(struct ieee80211_hw *hw)
+int zd_op_start(struct ieee80211_hw *hw)
 {
        struct zd_mac *mac = zd_hw_mac(hw);
        struct zd_chip *chip = &mac->chip;
@@ -275,6 +301,8 @@ static int zd_op_start(struct ieee80211_hw *hw)
                goto disable_rxtx;
        housekeeping_enable(mac);
+        beacon_enable(mac);
+        set_bit(ZD_DEVICE_RUNNING, &mac->flags);
        return 0;
 disable_rxtx:
        zd_chip_disable_rxtx(chip);
@@ -286,19 +314,22 @@ out:
        return r;
 }
-static void zd_op_stop(struct ieee80211_hw *hw)
+void zd_op_stop(struct ieee80211_hw *hw)
 {
        struct zd_mac *mac = zd_hw_mac(hw);
        struct zd_chip *chip = &mac->chip;
        struct sk_buff *skb;
        struct sk_buff_head *ack_wait_queue = &mac->ack_wait_queue;
+        clear_bit(ZD_DEVICE_RUNNING, &mac->flags);
        /* The order here deliberately is a little different from the open()
         * method, since we need to make sure there is no opportunity for RX
         * frames to be processed by mac80211 after we have stopped it.
         */
        zd_chip_disable_rxtx(chip);
+        beacon_disable(mac);
        housekeeping_disable(mac);
        flush_workqueue(zd_workqueue);
@@ -311,6 +342,68 @@ static void zd_op_stop(struct ieee80211_hw *hw)
                dev_kfree_skb_any(skb);
 }
+int zd_restore_settings(struct zd_mac *mac)
+{
+        struct sk_buff *beacon;
+        struct zd_mc_hash multicast_hash;
+        unsigned int short_preamble;
+        int r, beacon_interval, beacon_period;
+        u8 channel;
+        dev_dbg_f(zd_mac_dev(mac), "\n");
+        spin_lock_irq(&mac->lock);
+        multicast_hash = mac->multicast_hash;
+        short_preamble = mac->short_preamble;
+        beacon_interval = mac->beacon.interval;
+        beacon_period = mac->beacon.period;
+        channel = mac->channel;
+        spin_unlock_irq(&mac->lock);
+        r = set_mac_and_bssid(mac);
+        if (r < 0) {
+                dev_dbg_f(zd_mac_dev(mac), "set_mac_and_bssid failed, %d\n", r);
+                return r;
+        }
+        r = zd_chip_set_channel(&mac->chip, channel);
+        if (r < 0) {
+                dev_dbg_f(zd_mac_dev(mac), "zd_chip_set_channel failed, %d\n",
+                          r);
+                return r;
+        }
+        set_rts_cts(mac, short_preamble);
+        r = zd_chip_set_multicast_hash(&mac->chip, &multicast_hash);
+        if (r < 0) {
+                dev_dbg_f(zd_mac_dev(mac),
+                          "zd_chip_set_multicast_hash failed, %d\n", r);
+                return r;
+        }
+        if (mac->type == NL80211_IFTYPE_MESH_POINT ||
+            mac->type == NL80211_IFTYPE_ADHOC ||
+            mac->type == NL80211_IFTYPE_AP) {
+                if (mac->vif != NULL) {
+                        beacon = ieee80211_beacon_get(mac->hw, mac->vif);
+                        if (beacon) {
+                                zd_mac_config_beacon(mac->hw, beacon);
+                                kfree_skb(beacon);
+                        }
+                }
+                zd_set_beacon_interval(&mac->chip, beacon_interval,
+                                        beacon_period, mac->type);
+                spin_lock_irq(&mac->lock);
+                mac->beacon.last_update = jiffies;
+                spin_unlock_irq(&mac->lock);
+        }
+        return 0;
+}
 /**
 * zd_mac_tx_status - reports tx status of a packet if required
 * @hw - a &struct ieee80211_hw pointer
@@ -574,64 +667,120 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 static int zd_mac_config_beacon(struct ieee80211_hw *hw, struct sk_buff *beacon)
 {
        struct zd_mac *mac = zd_hw_mac(hw);
-        int r;
+        int r, ret, num_cmds, req_pos = 0;
        u32 tmp, j = 0;
        /* 4 more bytes for tail CRC */
        u32 full_len = beacon->len + 4;
+        unsigned long end_jiffies, message_jiffies;
+        struct zd_ioreq32 *ioreqs;
+        /* Alloc memory for full beacon write at once. */
+        num_cmds = 1 + zd_chip_is_zd1211b(&mac->chip) + full_len;
+        ioreqs = kmalloc(num_cmds * sizeof(struct zd_ioreq32), GFP_KERNEL);
+        if (!ioreqs)
+                return -ENOMEM;
-        r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 0);
+        mutex_lock(&mac->chip.mutex);
+        r = zd_iowrite32_locked(&mac->chip, 0, CR_BCN_FIFO_SEMAPHORE);
        if (r < 0)
-                return r;
+                goto out;
-        r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+        r = zd_ioread32_locked(&mac->chip, &tmp, CR_BCN_FIFO_SEMAPHORE);
        if (r < 0)
-                return r;
+                goto release_sema;
+        end_jiffies = jiffies + HZ / 2; /*~500ms*/
+        message_jiffies = jiffies + HZ / 10; /*~100ms*/
        while (tmp & 0x2) {
-                r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+                r = zd_ioread32_locked(&mac->chip, &tmp, CR_BCN_FIFO_SEMAPHORE);
                if (r < 0)
-                        return r;
+                        goto release_sema;
-                if ((++j % 100) == 0) {
+                if (time_is_before_eq_jiffies(message_jiffies)) {
-                        printk(KERN_ERR "CR_BCN_FIFO_SEMAPHORE not ready\n");
+                        message_jiffies = jiffies + HZ / 10;
-                        if (j >= 500)  {
+                        dev_err(zd_mac_dev(mac),
-                                printk(KERN_ERR "Giving up beacon config.\n");
+                                        "CR_BCN_FIFO_SEMAPHORE not ready\n");
-                                return -ETIMEDOUT;
+                        if (time_is_before_eq_jiffies(end_jiffies))  {
+                                dev_err(zd_mac_dev(mac),
+                                                "Giving up beacon config.\n");
+                                r = -ETIMEDOUT;
+                                goto reset_device;
                        }
                }
-                msleep(1);
+                msleep(20);
        }
-        r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, full_len - 1);
+        ioreqs[req_pos].addr = CR_BCN_FIFO;
-        if (r < 0)
+        ioreqs[req_pos].value = full_len - 1;
-                return r;
+        req_pos++;
        if (zd_chip_is_zd1211b(&mac->chip)) {
-                r = zd_iowrite32(&mac->chip, CR_BCN_LENGTH, full_len - 1);
+                ioreqs[req_pos].addr = CR_BCN_LENGTH;
-                if (r < 0)
+                ioreqs[req_pos].value = full_len - 1;
-                        return r;
+                req_pos++;
        }
        for (j = 0 ; j < beacon->len; j++) {
-                r = zd_iowrite32(&mac->chip, CR_BCN_FIFO,
+                ioreqs[req_pos].addr = CR_BCN_FIFO;
-                                *((u8 *)(beacon->data + j)));
+                ioreqs[req_pos].value = *((u8 *)(beacon->data + j));
-                if (r < 0)
+                req_pos++;
-                        return r;
        }
        for (j = 0; j < 4; j++) {
-                r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, 0x0);
+                ioreqs[req_pos].addr = CR_BCN_FIFO;
-                if (r < 0)
+                ioreqs[req_pos].value = 0x0;
-                        return r;
+                req_pos++;
        }
-        r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 1);
+        BUG_ON(req_pos != num_cmds);
-        if (r < 0)
-                return r;
+        r = zd_iowrite32a_locked(&mac->chip, ioreqs, num_cmds);
+release_sema:
+        /*
+         * Try very hard to release device beacon semaphore, as otherwise
+         * device/driver can be left in unusable state.
+         */
+        end_jiffies = jiffies + HZ / 2; /*~500ms*/
+        ret = zd_iowrite32_locked(&mac->chip, 1, CR_BCN_FIFO_SEMAPHORE);
+        while (ret < 0) {
+                if (time_is_before_eq_jiffies(end_jiffies)) {
+                        ret = -ETIMEDOUT;
+                        break;
+                }
+                msleep(20);
+                ret = zd_iowrite32_locked(&mac->chip, 1, CR_BCN_FIFO_SEMAPHORE);
+        }
+        if (ret < 0)
+                dev_err(zd_mac_dev(mac), "Could not release "
+                                         "CR_BCN_FIFO_SEMAPHORE!\n");
+        if (r < 0 || ret < 0) {
+                if (r >= 0)
+                        r = ret;
+                goto out;
+        }
        /* 802.11b/g 2.4G CCK 1Mb
         * 802.11a, not yet implemented, uses different values (see GPL vendor
         * driver)
         */
-        return zd_iowrite32(&mac->chip, CR_BCN_PLCP_CFG, 0x00000400 |
+        r = zd_iowrite32_locked(&mac->chip, 0x00000400 | (full_len << 19),
-                        (full_len << 19));
+                                CR_BCN_PLCP_CFG);
+out:
+        mutex_unlock(&mac->chip.mutex);
+        kfree(ioreqs);
+        return r;
+reset_device:
+        mutex_unlock(&mac->chip.mutex);
+        kfree(ioreqs);
+        /* semaphore stuck, reset device to avoid fw freeze later */
+        dev_warn(zd_mac_dev(mac), "CR_BCN_FIFO_SEMAPHORE stuck, "
+                                  "reseting device...");
+        usb_queue_reset_device(mac->chip.usb.intf);
+        return r;
 }
 static int fill_ctrlset(struct zd_mac *mac,
@@ -779,6 +928,13 @@ static int filter_ack(struct ieee80211_hw *hw, struct ieee80211_hdr *rx_hdr,
                mac->ack_pending = 1;
                mac->ack_signal = stats->signal;
+                /* Prevent pending tx-packet on AP-mode */
+                if (mac->type == NL80211_IFTYPE_AP) {
+                        skb = __skb_dequeue(q);
+                        zd_mac_tx_status(hw, skb, mac->ack_signal, NULL);
+                        mac->ack_pending = 0;
+                }
        }
        spin_unlock_irqrestore(&q->lock, flags);
@@ -882,13 +1038,16 @@ static int zd_op_add_interface(struct ieee80211_hw *hw,
        case NL80211_IFTYPE_MESH_POINT:
        case NL80211_IFTYPE_STATION:
        case NL80211_IFTYPE_ADHOC:
+        case NL80211_IFTYPE_AP:
                mac->type = vif->type;
                break;
        default:
                return -EOPNOTSUPP;
        }
-        return zd_write_mac_addr(&mac->chip, vif->addr);
+        mac->vif = vif;
+        return set_mac_and_bssid(mac);
 }
 static void zd_op_remove_interface(struct ieee80211_hw *hw,
@@ -896,7 +1055,8 @@ static void zd_op_remove_interface(struct ieee80211_hw *hw,
 {
        struct zd_mac *mac = zd_hw_mac(hw);
        mac->type = NL80211_IFTYPE_UNSPECIFIED;
-        zd_set_beacon_interval(&mac->chip, 0);
+        mac->vif = NULL;
+        zd_set_beacon_interval(&mac->chip, 0, 0, NL80211_IFTYPE_UNSPECIFIED);
        zd_write_mac_addr(&mac->chip, NULL);
 }
@@ -905,49 +1065,67 @@ static int zd_op_config(struct ieee80211_hw *hw, u32 changed)
        struct zd_mac *mac = zd_hw_mac(hw);
        struct ieee80211_conf *conf = &hw->conf;
+        spin_lock_irq(&mac->lock);
+        mac->channel = conf->channel->hw_value;
+        spin_unlock_irq(&mac->lock);
        return zd_chip_set_channel(&mac->chip, conf->channel->hw_value);
 }
-static void zd_process_intr(struct work_struct *work)
+static void zd_beacon_done(struct zd_mac *mac)
 {
-        u16 int_status;
+        struct sk_buff *skb, *beacon;
-        struct zd_mac *mac = container_of(work, struct zd_mac, process_intr);
-        int_status = le16_to_cpu(*(__le16 *)(mac->intr_buffer+4));
+        if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
-        if (int_status & INT_CFG_NEXT_BCN)
+                return;
-                dev_dbg_f_limit(zd_mac_dev(mac), "INT_CFG_NEXT_BCN\n");
+        if (!mac->vif || mac->vif->type != NL80211_IFTYPE_AP)
-        else
+                return;
-                dev_dbg_f(zd_mac_dev(mac), "Unsupported interrupt\n");
-        zd_chip_enable_hwint(&mac->chip);
-}
+        /*
+         * Send out buffered broad- and multicast frames.
+         */
+        while (!ieee80211_queue_stopped(mac->hw, 0)) {
+                skb = ieee80211_get_buffered_bc(mac->hw, mac->vif);
+                if (!skb)
+                        break;
+                zd_op_tx(mac->hw, skb);
+        }
-static void set_multicast_hash_handler(struct work_struct *work)
+        /*
-{
+         * Fetch next beacon so that tim_count is updated.
-        struct zd_mac *mac =
+         */
-                container_of(work, struct zd_mac, set_multicast_hash_work);
+        beacon = ieee80211_beacon_get(mac->hw, mac->vif);
-        struct zd_mc_hash hash;
+        if (beacon) {
+                zd_mac_config_beacon(mac->hw, beacon);
+                kfree_skb(beacon);
+        }
        spin_lock_irq(&mac->lock);
-        hash = mac->multicast_hash;
+        mac->beacon.last_update = jiffies;
        spin_unlock_irq(&mac->lock);
-        zd_chip_set_multicast_hash(&mac->chip, &hash);
 }
-static void set_rx_filter_handler(struct work_struct *work)
+static void zd_process_intr(struct work_struct *work)
 {
-        struct zd_mac *mac =
+        u16 int_status;
-                container_of(work, struct zd_mac, set_rx_filter_work);
+        unsigned long flags;
-        int r;
+        struct zd_mac *mac = container_of(work, struct zd_mac, process_intr);
-        dev_dbg_f(zd_mac_dev(mac), "\n");
+        spin_lock_irqsave(&mac->lock, flags);
-        r = set_rx_filter(mac);
+        int_status = le16_to_cpu(*(__le16 *)(mac->intr_buffer + 4));
-        if (r)
+        spin_unlock_irqrestore(&mac->lock, flags);
-                dev_err(zd_mac_dev(mac), "set_rx_filter_handler error %d\n", r);
+        if (int_status & INT_CFG_NEXT_BCN) {
+                /*dev_dbg_f_limit(zd_mac_dev(mac), "INT_CFG_NEXT_BCN\n");*/
+                zd_beacon_done(mac);
+        } else {
+                dev_dbg_f(zd_mac_dev(mac), "Unsupported interrupt\n");
+        }
+        zd_chip_enable_hwint(&mac->chip);
 }
 static u64 zd_op_prepare_multicast(struct ieee80211_hw *hw,
                                   struct netdev_hw_addr_list *mc_list)
 {
@@ -979,6 +1157,7 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
        };
        struct zd_mac *mac = zd_hw_mac(hw);
        unsigned long flags;
+        int r;
        /* Only deal with supported flags */
        changed_flags &= SUPPORTED_FIF_FLAGS;
@@ -1000,11 +1179,13 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
        mac->multicast_hash = hash;
        spin_unlock_irqrestore(&mac->lock, flags);
-        /* XXX: these can be called here now, can sleep now! */
+        zd_chip_set_multicast_hash(&mac->chip, &hash);
-        queue_work(zd_workqueue, &mac->set_multicast_hash_work);
-        if (changed_flags & FIF_CONTROL)
+        if (changed_flags & FIF_CONTROL) {
-                queue_work(zd_workqueue, &mac->set_rx_filter_work);
+                r = set_rx_filter(mac);
+                if (r)
+                        dev_err(zd_mac_dev(mac), "set_rx_filter error %d\n", r);
+        }
        /* no handling required for FIF_OTHER_BSS as we don't currently
         * do BSSID filtering */
@@ -1016,20 +1197,9 @@ static void zd_op_configure_filter(struct ieee80211_hw *hw,
         * time. */
 }
-static void set_rts_cts_work(struct work_struct *work)
+static void set_rts_cts(struct zd_mac *mac, unsigned int short_preamble)
 {
-        struct zd_mac *mac =
-                container_of(work, struct zd_mac, set_rts_cts_work);
-        unsigned long flags;
-        unsigned int short_preamble;
        mutex_lock(&mac->chip.mutex);
-        spin_lock_irqsave(&mac->lock, flags);
-        mac->updating_rts_rate = 0;
-        short_preamble = mac->short_preamble;
-        spin_unlock_irqrestore(&mac->lock, flags);
        zd_chip_set_rts_cts_rate_locked(&mac->chip, short_preamble);
        mutex_unlock(&mac->chip.mutex);
 }
@@ -1040,33 +1210,42 @@ static void zd_op_bss_info_changed(struct ieee80211_hw *hw,
                                   u32 changes)
 {
        struct zd_mac *mac = zd_hw_mac(hw);
-        unsigned long flags;
        int associated;
        dev_dbg_f(zd_mac_dev(mac), "changes: %x\n", changes);
        if (mac->type == NL80211_IFTYPE_MESH_POINT ||
-            mac->type == NL80211_IFTYPE_ADHOC) {
+            mac->type == NL80211_IFTYPE_ADHOC ||
+            mac->type == NL80211_IFTYPE_AP) {
                associated = true;
                if (changes & BSS_CHANGED_BEACON) {
                        struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
                        if (beacon) {
+                                zd_chip_disable_hwint(&mac->chip);
                                zd_mac_config_beacon(hw, beacon);
+                                zd_chip_enable_hwint(&mac->chip);
                                kfree_skb(beacon);
                        }
                }
                if (changes & BSS_CHANGED_BEACON_ENABLED) {
-                        u32 interval;
+                        u16 interval = 0;
+                        u8 period = 0;
-                        if (bss_conf->enable_beacon)
+                        if (bss_conf->enable_beacon) {
-                                interval = BCN_MODE_IBSS |
+                                period = bss_conf->dtim_period;
-                                                bss_conf->beacon_int;
+                                interval = bss_conf->beacon_int;
-                        else
+                        }
-                                interval = 0;
-                        zd_set_beacon_interval(&mac->chip, interval);
+                        spin_lock_irq(&mac->lock);
+                        mac->beacon.period = period;
+                        mac->beacon.interval = interval;
+                        mac->beacon.last_update = jiffies;
+                        spin_unlock_irq(&mac->lock);
+                        zd_set_beacon_interval(&mac->chip, interval, period,
+                                               mac->type);
                }
        } else
                associated = is_valid_ether_addr(bss_conf->bssid);
@@ -1078,15 +1257,11 @@ static void zd_op_bss_info_changed(struct ieee80211_hw *hw,
        /* TODO: do hardware bssid filtering */
        if (changes & BSS_CHANGED_ERP_PREAMBLE) {
-                spin_lock_irqsave(&mac->lock, flags);
+                spin_lock_irq(&mac->lock);
                mac->short_preamble = bss_conf->use_short_preamble;
-                if (!mac->updating_rts_rate) {
+                spin_unlock_irq(&mac->lock);
-                        mac->updating_rts_rate = 1;
-                        /* FIXME: should disable TX here, until work has
+                set_rts_cts(mac, bss_conf->use_short_preamble);
-                         * completed and RTS_CTS reg is updated */
-                        queue_work(zd_workqueue, &mac->set_rts_cts_work);
-                }
-                spin_unlock_irqrestore(&mac->lock, flags);
        }
 }
@@ -1138,12 +1313,14 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
        hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &mac->band;
        hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-                    IEEE80211_HW_SIGNAL_UNSPEC;
+                    IEEE80211_HW_SIGNAL_UNSPEC |
+                    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
        hw->wiphy->interface_modes =
                BIT(NL80211_IFTYPE_MESH_POINT) |
                BIT(NL80211_IFTYPE_STATION) |
-                BIT(NL80211_IFTYPE_ADHOC);
+                BIT(NL80211_IFTYPE_ADHOC) |
+                BIT(NL80211_IFTYPE_AP);
        hw->max_signal = 100;
        hw->queues = 1;
@@ -1160,15 +1337,82 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
        zd_chip_init(&mac->chip, hw, intf);
        housekeeping_init(mac);
-        INIT_WORK(&mac->set_multicast_hash_work, set_multicast_hash_handler);
+        beacon_init(mac);
-        INIT_WORK(&mac->set_rts_cts_work, set_rts_cts_work);
-        INIT_WORK(&mac->set_rx_filter_work, set_rx_filter_handler);
        INIT_WORK(&mac->process_intr, zd_process_intr);
        SET_IEEE80211_DEV(hw, &intf->dev);
        return hw;
 }
+#define BEACON_WATCHDOG_DELAY round_jiffies_relative(HZ)
+static void beacon_watchdog_handler(struct work_struct *work)
+{
+        struct zd_mac *mac =
+                container_of(work, struct zd_mac, beacon.watchdog_work.work);
+        struct sk_buff *beacon;
+        unsigned long timeout;
+        int interval, period;
+        if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+                goto rearm;
+        if (mac->type != NL80211_IFTYPE_AP || !mac->vif)
+                goto rearm;
+        spin_lock_irq(&mac->lock);
+        interval = mac->beacon.interval;
+        period = mac->beacon.period;
+        timeout = mac->beacon.last_update + msecs_to_jiffies(interval) + HZ;
+        spin_unlock_irq(&mac->lock);
+        if (interval > 0 && time_is_before_jiffies(timeout)) {
+                dev_dbg_f(zd_mac_dev(mac), "beacon interrupt stalled, "
+                                           "restarting. "
+                                           "(interval: %d, dtim: %d)\n",
+                                           interval, period);
+                zd_chip_disable_hwint(&mac->chip);
+                beacon = ieee80211_beacon_get(mac->hw, mac->vif);
+                if (beacon) {
+                        zd_mac_config_beacon(mac->hw, beacon);
+                        kfree_skb(beacon);
+                }
+                zd_set_beacon_interval(&mac->chip, interval, period, mac->type);
+                zd_chip_enable_hwint(&mac->chip);
+                spin_lock_irq(&mac->lock);
+                mac->beacon.last_update = jiffies;
+                spin_unlock_irq(&mac->lock);
+        }
+rearm:
+        queue_delayed_work(zd_workqueue, &mac->beacon.watchdog_work,
+                           BEACON_WATCHDOG_DELAY);
+}
+static void beacon_init(struct zd_mac *mac)
+{
+        INIT_DELAYED_WORK(&mac->beacon.watchdog_work, beacon_watchdog_handler);
+}
+static void beacon_enable(struct zd_mac *mac)
+{
+        dev_dbg_f(zd_mac_dev(mac), "\n");
+        mac->beacon.last_update = jiffies;
+        queue_delayed_work(zd_workqueue, &mac->beacon.watchdog_work,
+                           BEACON_WATCHDOG_DELAY);
+}
+static void beacon_disable(struct zd_mac *mac)
+{
+        dev_dbg_f(zd_mac_dev(mac), "\n");
+        cancel_delayed_work_sync(&mac->beacon.watchdog_work);
+}
 #define LINK_LED_WORK_DELAY HZ
 static void link_led_handler(struct work_struct *work)
@@ -1179,6 +1423,9 @@ static void link_led_handler(struct work_struct *work)
        int is_associated;
        int r;
+        if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+                goto requeue;
        spin_lock_irq(&mac->lock);
        is_associated = mac->associated;
        spin_unlock_irq(&mac->lock);
@@ -1188,6 +1435,7 @@ static void link_led_handler(struct work_struct *work)
        if (r)
                dev_dbg_f(zd_mac_dev(mac), "zd_chip_control_leds error %d\n", r);
+requeue:
        queue_delayed_work(zd_workqueue, &mac->housekeeping.link_led_work,
                           LINK_LED_WORK_DELAY);
 }
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index a6d86b996c79..f8c93c3fe755 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -163,6 +163,17 @@ struct housekeeping {
        struct delayed_work link_led_work;
 };
+struct beacon {
+        struct delayed_work watchdog_work;
+        unsigned long last_update;
+        u16 interval;
+        u8 period;
+};
+enum zd_device_flags {
+        ZD_DEVICE_RUNNING,
+};
 #define ZD_MAC_STATS_BUFFER_SIZE 16
 #define ZD_MAC_MAX_ACK_WAITERS 50
@@ -172,17 +183,19 @@ struct zd_mac {
        spinlock_t lock;
        spinlock_t intr_lock;
        struct ieee80211_hw *hw;
+        struct ieee80211_vif *vif;
        struct housekeeping housekeeping;
-        struct work_struct set_multicast_hash_work;
+        struct beacon beacon;
        struct work_struct set_rts_cts_work;
-        struct work_struct set_rx_filter_work;
        struct work_struct process_intr;
        struct zd_mc_hash multicast_hash;
        u8 intr_buffer[USB_MAX_EP_INT_BUFFER];
        u8 regdomain;
        u8 default_regdomain;
+        u8 channel;
        int type;
        int associated;
+        unsigned long flags;
        struct sk_buff_head ack_wait_queue;
        struct ieee80211_channel channels[14];
        struct ieee80211_rate rates[12];
@@ -191,9 +204,6 @@ struct zd_mac {
        /* Short preamble (used for RTS/CTS) */
        unsigned int short_preamble:1;
-        /* flags to indicate update in progress */
-        unsigned int updating_rts_rate:1;
        /* whether to pass frames with CRC errors to stack */
        unsigned int pass_failed_fcs:1;
@@ -304,6 +314,10 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length);
 void zd_mac_tx_failed(struct urb *urb);
 void zd_mac_tx_to_dev(struct sk_buff *skb, int error);
+int zd_op_start(struct ieee80211_hw *hw);
+void zd_op_stop(struct ieee80211_hw *hw);
+int zd_restore_settings(struct zd_mac *mac);
 #ifdef DEBUG
 void zd_dump_rx_status(const struct rx_status *status);
 #else
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index 06041cb1c422..f6df3665fdb6 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -377,8 +377,10 @@ static inline void handle_regs_int(struct urb *urb)
        int_num = le16_to_cpu(*(__le16 *)(urb->transfer_buffer+2));
        if (int_num == CR_INTERRUPT) {
                struct zd_mac *mac = zd_hw_mac(zd_usb_to_hw(urb->context));
+                spin_lock(&mac->lock);
                memcpy(&mac->intr_buffer, urb->transfer_buffer,
                                USB_MAX_EP_INT_BUFFER);
+                spin_unlock(&mac->lock);
                schedule_work(&mac->process_intr);
        } else if (intr->read_regs_enabled) {
                intr->read_regs.length = len = urb->actual_length;
@@ -409,8 +411,10 @@ static void int_urb_complete(struct urb *urb)
        case -ENOENT:
        case -ECONNRESET:
        case -EPIPE:
-                goto kfree;
+                dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
+                return;
        default:
+                dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
                goto resubmit;
        }
@@ -441,12 +445,11 @@ static void int_urb_complete(struct urb *urb)
 resubmit:
        r = usb_submit_urb(urb, GFP_ATOMIC);
        if (r) {
-                dev_dbg_f(urb_dev(urb), "resubmit urb %p\n", urb);
+                dev_dbg_f(urb_dev(urb), "error: resubmit urb %p err code %d\n",
-                goto kfree;
+                          urb, r);
+                /* TODO: add worker to reset intr->urb */
        }
        return;
-kfree:
-        kfree(urb->transfer_buffer);
 }
 static inline int int_urb_interval(struct usb_device *udev)
@@ -477,9 +480,8 @@ static inline int usb_int_enabled(struct zd_usb *usb)
 int zd_usb_enable_int(struct zd_usb *usb)
 {
        int r;
-        struct usb_device *udev;
+        struct usb_device *udev = zd_usb_to_usbdev(usb);
        struct zd_usb_interrupt *intr = &usb->intr;
-        void *transfer_buffer = NULL;
        struct urb *urb;
        dev_dbg_f(zd_usb_dev(usb), "\n");
@@ -500,20 +502,21 @@ int zd_usb_enable_int(struct zd_usb *usb)
        intr->urb = urb;
        spin_unlock_irq(&intr->lock);
-        /* TODO: make it a DMA buffer */
        r = -ENOMEM;
-        transfer_buffer = kmalloc(USB_MAX_EP_INT_BUFFER, GFP_KERNEL);
+        intr->buffer = usb_alloc_coherent(udev, USB_MAX_EP_INT_BUFFER,
-        if (!transfer_buffer) {
+                                          GFP_KERNEL, &intr->buffer_dma);
+        if (!intr->buffer) {
                dev_dbg_f(zd_usb_dev(usb),
                        "couldn't allocate transfer_buffer\n");
                goto error_set_urb_null;
        }
-        udev = zd_usb_to_usbdev(usb);
        usb_fill_int_urb(urb, udev, usb_rcvintpipe(udev, EP_INT_IN),
-                         transfer_buffer, USB_MAX_EP_INT_BUFFER,
+                         intr->buffer, USB_MAX_EP_INT_BUFFER,
                         int_urb_complete, usb,
                         intr->interval);
+        urb->transfer_dma = intr->buffer_dma;
+        urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
        dev_dbg_f(zd_usb_dev(usb), "submit urb %p\n", intr->urb);
        r = usb_submit_urb(urb, GFP_KERNEL);
@@ -525,7 +528,8 @@ int zd_usb_enable_int(struct zd_usb *usb)
        return 0;
 error:
-        kfree(transfer_buffer);
+        usb_free_coherent(udev, USB_MAX_EP_INT_BUFFER,
+                          intr->buffer, intr->buffer_dma);
 error_set_urb_null:
        spin_lock_irq(&intr->lock);
        intr->urb = NULL;
@@ -539,8 +543,11 @@ out:
 void zd_usb_disable_int(struct zd_usb *usb)
 {
        unsigned long flags;
+        struct usb_device *udev = zd_usb_to_usbdev(usb);
        struct zd_usb_interrupt *intr = &usb->intr;
        struct urb *urb;
+        void *buffer;
+        dma_addr_t buffer_dma;
        spin_lock_irqsave(&intr->lock, flags);
        urb = intr->urb;
@@ -549,11 +556,18 @@ void zd_usb_disable_int(struct zd_usb *usb)
                return;
        }
        intr->urb = NULL;
+        buffer = intr->buffer;
+        buffer_dma = intr->buffer_dma;
+        intr->buffer = NULL;
        spin_unlock_irqrestore(&intr->lock, flags);
        usb_kill_urb(urb);
        dev_dbg_f(zd_usb_dev(usb), "urb %p killed\n", urb);
        usb_free_urb(urb);
+        if (buffer)
+                usb_free_coherent(udev, USB_MAX_EP_INT_BUFFER,
+                                  buffer, buffer_dma);
 }
 static void handle_rx_packet(struct zd_usb *usb, const u8 *buffer,
@@ -601,6 +615,7 @@ static void handle_rx_packet(struct zd_usb *usb, const u8 *buffer,
 static void rx_urb_complete(struct urb *urb)
 {
+        int r;
        struct zd_usb *usb;
        struct zd_usb_rx *rx;
        const u8 *buffer;
@@ -615,6 +630,7 @@ static void rx_urb_complete(struct urb *urb)
        case -ENOENT:
        case -ECONNRESET:
        case -EPIPE:
+                dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
                return;
        default:
                dev_dbg_f(urb_dev(urb), "urb %p error %d\n", urb, urb->status);
@@ -626,6 +642,8 @@ static void rx_urb_complete(struct urb *urb)
        usb = urb->context;
        rx = &usb->rx;
+        zd_usb_reset_rx_idle_timer(usb);
        if (length%rx->usb_packet_size > rx->usb_packet_size-4) {
                /* If there is an old first fragment, we don't care. */
                dev_dbg_f(urb_dev(urb), "*** first fragment ***\n");
@@ -654,7 +672,9 @@ static void rx_urb_complete(struct urb *urb)
        }
 resubmit:
-        usb_submit_urb(urb, GFP_ATOMIC);
+        r = usb_submit_urb(urb, GFP_ATOMIC);
+        if (r)
+                dev_dbg_f(urb_dev(urb), "urb %p resubmit error %d\n", urb, r);
 }
 static struct urb *alloc_rx_urb(struct zd_usb *usb)
@@ -690,7 +710,7 @@ static void free_rx_urb(struct urb *urb)
        usb_free_urb(urb);
 }
-int zd_usb_enable_rx(struct zd_usb *usb)
+static int __zd_usb_enable_rx(struct zd_usb *usb)
 {
        int i, r;
        struct zd_usb_rx *rx = &usb->rx;
@@ -742,7 +762,21 @@ error:
        return r;
 }
-void zd_usb_disable_rx(struct zd_usb *usb)
+int zd_usb_enable_rx(struct zd_usb *usb)
+{
+        int r;
+        struct zd_usb_rx *rx = &usb->rx;
+        mutex_lock(&rx->setup_mutex);
+        r = __zd_usb_enable_rx(usb);
+        mutex_unlock(&rx->setup_mutex);
+        zd_usb_reset_rx_idle_timer(usb);
+        return r;
+}
+static void __zd_usb_disable_rx(struct zd_usb *usb)
 {
        int i;
        unsigned long flags;
@@ -769,6 +803,40 @@ void zd_usb_disable_rx(struct zd_usb *usb)
        spin_unlock_irqrestore(&rx->lock, flags);
 }
+void zd_usb_disable_rx(struct zd_usb *usb)
+{
+        struct zd_usb_rx *rx = &usb->rx;
+        mutex_lock(&rx->setup_mutex);
+        __zd_usb_disable_rx(usb);
+        mutex_unlock(&rx->setup_mutex);
+        cancel_delayed_work_sync(&rx->idle_work);
+}
+static void zd_usb_reset_rx(struct zd_usb *usb)
+{
+        bool do_reset;
+        struct zd_usb_rx *rx = &usb->rx;
+        unsigned long flags;
+        mutex_lock(&rx->setup_mutex);
+        spin_lock_irqsave(&rx->lock, flags);
+        do_reset = rx->urbs != NULL;
+        spin_unlock_irqrestore(&rx->lock, flags);
+        if (do_reset) {
+                __zd_usb_disable_rx(usb);
+                __zd_usb_enable_rx(usb);
+        }
+        mutex_unlock(&rx->setup_mutex);
+        if (do_reset)
+                zd_usb_reset_rx_idle_timer(usb);
+}
 /**
 * zd_usb_disable_tx - disable transmission
 * @usb: the zd1211rw-private USB structure
@@ -779,19 +847,21 @@ void zd_usb_disable_tx(struct zd_usb *usb)
 {
        struct zd_usb_tx *tx = &usb->tx;
        unsigned long flags;
-        struct list_head *pos, *n;
+        atomic_set(&tx->enabled, 0);
+        /* kill all submitted tx-urbs */
+        usb_kill_anchored_urbs(&tx->submitted);
        spin_lock_irqsave(&tx->lock, flags);
-        list_for_each_safe(pos, n, &tx->free_urb_list) {
+        WARN_ON(!skb_queue_empty(&tx->submitted_skbs));
-                list_del(pos);
+        WARN_ON(tx->submitted_urbs != 0);
-                usb_free_urb(list_entry(pos, struct urb, urb_list));
-        }
-        tx->enabled = 0;
        tx->submitted_urbs = 0;
+        spin_unlock_irqrestore(&tx->lock, flags);
        /* The stopped state is ignored, relying on ieee80211_wake_queues()
         * in a potentionally following zd_usb_enable_tx().
         */
-        spin_unlock_irqrestore(&tx->lock, flags);
 }
 /**
@@ -807,63 +877,13 @@ void zd_usb_enable_tx(struct zd_usb *usb)
        struct zd_usb_tx *tx = &usb->tx;
        spin_lock_irqsave(&tx->lock, flags);
-        tx->enabled = 1;
+        atomic_set(&tx->enabled, 1);
        tx->submitted_urbs = 0;
        ieee80211_wake_queues(zd_usb_to_hw(usb));
        tx->stopped = 0;
        spin_unlock_irqrestore(&tx->lock, flags);
 }
-/**
- * alloc_tx_urb - provides an tx URB
- * @usb: a &struct zd_usb pointer
- *
- * Allocates a new URB. If possible takes the urb from the free list in
- * usb->tx.
- */
-static struct urb *alloc_tx_urb(struct zd_usb *usb)
-{
-        struct zd_usb_tx *tx = &usb->tx;
-        unsigned long flags;
-        struct list_head *entry;
-        struct urb *urb;
-        spin_lock_irqsave(&tx->lock, flags);
-        if (list_empty(&tx->free_urb_list)) {
-                urb = usb_alloc_urb(0, GFP_ATOMIC);
-                goto out;
-        }
-        entry = tx->free_urb_list.next;
-        list_del(entry);
-        urb = list_entry(entry, struct urb, urb_list);
-out:
-        spin_unlock_irqrestore(&tx->lock, flags);
-        return urb;
-}
-/**
- * free_tx_urb - frees a used tx URB
- * @usb: a &struct zd_usb pointer
- * @urb: URB to be freed
- *
- * Frees the transmission URB, which means to put it on the free URB
- * list.
- */
-static void free_tx_urb(struct zd_usb *usb, struct urb *urb)
-{
-        struct zd_usb_tx *tx = &usb->tx;
-        unsigned long flags;
-        spin_lock_irqsave(&tx->lock, flags);
-        if (!tx->enabled) {
-                usb_free_urb(urb);
-                goto out;
-        }
-        list_add(&urb->urb_list, &tx->free_urb_list);
-out:
-        spin_unlock_irqrestore(&tx->lock, flags);
-}
 static void tx_dec_submitted_urbs(struct zd_usb *usb)
 {
        struct zd_usb_tx *tx = &usb->tx;
@@ -905,6 +925,16 @@ static void tx_urb_complete(struct urb *urb)
        struct sk_buff *skb;
        struct ieee80211_tx_info *info;
        struct zd_usb *usb;
+        struct zd_usb_tx *tx;
+        skb = (struct sk_buff *)urb->context;
+        info = IEEE80211_SKB_CB(skb);
+        /*
+         * grab 'usb' pointer before handing off the skb (since
+         * it might be freed by zd_mac_tx_to_dev or mac80211)
+         */
+        usb = &zd_hw_mac(info->rate_driver_data[0])->chip.usb;
+        tx = &usb->tx;
        switch (urb->status) {
        case 0:
@@ -922,20 +952,16 @@ static void tx_urb_complete(struct urb *urb)
                goto resubmit;
        }
 free_urb:
-        skb = (struct sk_buff *)urb->context;
+        skb_unlink(skb, &usb->tx.submitted_skbs);
-        /*
-         * grab 'usb' pointer before handing off the skb (since
-         * it might be freed by zd_mac_tx_to_dev or mac80211)
-         */
-        info = IEEE80211_SKB_CB(skb);
-        usb = &zd_hw_mac(info->rate_driver_data[0])->chip.usb;
        zd_mac_tx_to_dev(skb, urb->status);
-        free_tx_urb(usb, urb);
+        usb_free_urb(urb);
        tx_dec_submitted_urbs(usb);
        return;
 resubmit:
+        usb_anchor_urb(urb, &tx->submitted);
        r = usb_submit_urb(urb, GFP_ATOMIC);
        if (r) {
+                usb_unanchor_urb(urb);
                dev_dbg_f(urb_dev(urb), "error resubmit urb %p %d\n", urb, r);
                goto free_urb;
        }
@@ -956,10 +982,17 @@ resubmit:
 int zd_usb_tx(struct zd_usb *usb, struct sk_buff *skb)
 {
        int r;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        struct usb_device *udev = zd_usb_to_usbdev(usb);
        struct urb *urb;
+        struct zd_usb_tx *tx = &usb->tx;
-        urb = alloc_tx_urb(usb);
+        if (!atomic_read(&tx->enabled)) {
+                r = -ENOENT;
+                goto out;
+        }
+        urb = usb_alloc_urb(0, GFP_ATOMIC);
        if (!urb) {
                r = -ENOMEM;
                goto out;
@@ -968,17 +1001,118 @@ int zd_usb_tx(struct zd_usb *usb, struct sk_buff *skb)
        usb_fill_bulk_urb(urb, udev, usb_sndbulkpipe(udev, EP_DATA_OUT),
                          skb->data, skb->len, tx_urb_complete, skb);
+        info->rate_driver_data[1] = (void *)jiffies;
+        skb_queue_tail(&tx->submitted_skbs, skb);
+        usb_anchor_urb(urb, &tx->submitted);
        r = usb_submit_urb(urb, GFP_ATOMIC);
-        if (r)
+        if (r) {
+                dev_dbg_f(zd_usb_dev(usb), "error submit urb %p %d\n", urb, r);
+                usb_unanchor_urb(urb);
+                skb_unlink(skb, &tx->submitted_skbs);
                goto error;
+        }
        tx_inc_submitted_urbs(usb);
        return 0;
 error:
-        free_tx_urb(usb, urb);
+        usb_free_urb(urb);
 out:
        return r;
 }
+static bool zd_tx_timeout(struct zd_usb *usb)
+{
+        struct zd_usb_tx *tx = &usb->tx;
+        struct sk_buff_head *q = &tx->submitted_skbs;
+        struct sk_buff *skb, *skbnext;
+        struct ieee80211_tx_info *info;
+        unsigned long flags, trans_start;
+        bool have_timedout = false;
+        spin_lock_irqsave(&q->lock, flags);
+        skb_queue_walk_safe(q, skb, skbnext) {
+                info = IEEE80211_SKB_CB(skb);
+                trans_start = (unsigned long)info->rate_driver_data[1];
+                if (time_is_before_jiffies(trans_start + ZD_TX_TIMEOUT)) {
+                        have_timedout = true;
+                        break;
+                }
+        }
+        spin_unlock_irqrestore(&q->lock, flags);
+        return have_timedout;
+}
+static void zd_tx_watchdog_handler(struct work_struct *work)
+{
+        struct zd_usb *usb =
+                container_of(work, struct zd_usb, tx.watchdog_work.work);
+        struct zd_usb_tx *tx = &usb->tx;
+        if (!atomic_read(&tx->enabled) || !tx->watchdog_enabled)
+                goto out;
+        if (!zd_tx_timeout(usb))
+                goto out;
+        /* TX halted, try reset */
+        dev_warn(zd_usb_dev(usb), "TX-stall detected, reseting device...");
+        usb_queue_reset_device(usb->intf);
+        /* reset will stop this worker, don't rearm */
+        return;
+out:
+        queue_delayed_work(zd_workqueue, &tx->watchdog_work,
+                           ZD_TX_WATCHDOG_INTERVAL);
+}
+void zd_tx_watchdog_enable(struct zd_usb *usb)
+{
+        struct zd_usb_tx *tx = &usb->tx;
+        if (!tx->watchdog_enabled) {
+                dev_dbg_f(zd_usb_dev(usb), "\n");
+                queue_delayed_work(zd_workqueue, &tx->watchdog_work,
+                                   ZD_TX_WATCHDOG_INTERVAL);
+                tx->watchdog_enabled = 1;
+        }
+}
+void zd_tx_watchdog_disable(struct zd_usb *usb)
+{
+        struct zd_usb_tx *tx = &usb->tx;
+        if (tx->watchdog_enabled) {
+                dev_dbg_f(zd_usb_dev(usb), "\n");
+                tx->watchdog_enabled = 0;
+                cancel_delayed_work_sync(&tx->watchdog_work);
+        }
+}
+static void zd_rx_idle_timer_handler(struct work_struct *work)
+{
+        struct zd_usb *usb =
+                container_of(work, struct zd_usb, rx.idle_work.work);
+        struct zd_mac *mac = zd_usb_to_mac(usb);
+        if (!test_bit(ZD_DEVICE_RUNNING, &mac->flags))
+                return;
+        dev_dbg_f(zd_usb_dev(usb), "\n");
+        /* 30 seconds since last rx, reset rx */
+        zd_usb_reset_rx(usb);
+}
+void zd_usb_reset_rx_idle_timer(struct zd_usb *usb)
+{
+        struct zd_usb_rx *rx = &usb->rx;
+        cancel_delayed_work(&rx->idle_work);
+        queue_delayed_work(zd_workqueue, &rx->idle_work, ZD_RX_IDLE_INTERVAL);
+}
 static inline void init_usb_interrupt(struct zd_usb *usb)
 {
        struct zd_usb_interrupt *intr = &usb->intr;
@@ -993,22 +1127,27 @@ static inline void init_usb_rx(struct zd_usb *usb)
 {
        struct zd_usb_rx *rx = &usb->rx;
        spin_lock_init(&rx->lock);
+        mutex_init(&rx->setup_mutex);
        if (interface_to_usbdev(usb->intf)->speed == USB_SPEED_HIGH) {
                rx->usb_packet_size = 512;
        } else {
                rx->usb_packet_size = 64;
        }
        ZD_ASSERT(rx->fragment_length == 0);
+        INIT_DELAYED_WORK(&rx->idle_work, zd_rx_idle_timer_handler);
 }
 static inline void init_usb_tx(struct zd_usb *usb)
 {
        struct zd_usb_tx *tx = &usb->tx;
        spin_lock_init(&tx->lock);
-        tx->enabled = 0;
+        atomic_set(&tx->enabled, 0);
        tx->stopped = 0;
-        INIT_LIST_HEAD(&tx->free_urb_list);
+        skb_queue_head_init(&tx->submitted_skbs);
+        init_usb_anchor(&tx->submitted);
        tx->submitted_urbs = 0;
+        tx->watchdog_enabled = 0;
+        INIT_DELAYED_WORK(&tx->watchdog_work, zd_tx_watchdog_handler);
 }
 void zd_usb_init(struct zd_usb *usb, struct ieee80211_hw *hw,
@@ -1240,6 +1379,7 @@ static void disconnect(struct usb_interface *intf)
        ieee80211_unregister_hw(hw);
        /* Just in case something has gone wrong! */
+        zd_usb_disable_tx(usb);
        zd_usb_disable_rx(usb);
        zd_usb_disable_int(usb);
@@ -1255,11 +1395,92 @@ static void disconnect(struct usb_interface *intf)
        dev_dbg(&intf->dev, "disconnected\n");
 }
+static void zd_usb_resume(struct zd_usb *usb)
+{
+        struct zd_mac *mac = zd_usb_to_mac(usb);
+        int r;
+        dev_dbg_f(zd_usb_dev(usb), "\n");
+        r = zd_op_start(zd_usb_to_hw(usb));
+        if (r < 0) {
+                dev_warn(zd_usb_dev(usb), "Device resume failed "
+                         "with error code %d. Retrying...\n", r);
+                if (usb->was_running)
+                        set_bit(ZD_DEVICE_RUNNING, &mac->flags);
+                usb_queue_reset_device(usb->intf);
+                return;
+        }
+        if (mac->type != NL80211_IFTYPE_UNSPECIFIED) {
+                r = zd_restore_settings(mac);
+                if (r < 0) {
+                        dev_dbg(zd_usb_dev(usb),
+                                "failed to restore settings, %d\n", r);
+                        return;
+                }
+        }
+}
+static void zd_usb_stop(struct zd_usb *usb)
+{
+        dev_dbg_f(zd_usb_dev(usb), "\n");
+        zd_op_stop(zd_usb_to_hw(usb));
+        zd_usb_disable_tx(usb);
+        zd_usb_disable_rx(usb);
+        zd_usb_disable_int(usb);
+        usb->initialized = 0;
+}
+static int pre_reset(struct usb_interface *intf)
+{
+        struct ieee80211_hw *hw = usb_get_intfdata(intf);
+        struct zd_mac *mac;
+        struct zd_usb *usb;
+        if (!hw || intf->condition != USB_INTERFACE_BOUND)
+                return 0;
+        mac = zd_hw_mac(hw);
+        usb = &mac->chip.usb;
+        usb->was_running = test_bit(ZD_DEVICE_RUNNING, &mac->flags);
+        zd_usb_stop(usb);
+        mutex_lock(&mac->chip.mutex);
+        return 0;
+}
+static int post_reset(struct usb_interface *intf)
+{
+        struct ieee80211_hw *hw = usb_get_intfdata(intf);
+        struct zd_mac *mac;
+        struct zd_usb *usb;
+        if (!hw || intf->condition != USB_INTERFACE_BOUND)
+                return 0;
+        mac = zd_hw_mac(hw);
+        usb = &mac->chip.usb;
+        mutex_unlock(&mac->chip.mutex);
+        if (usb->was_running)
+                zd_usb_resume(usb);
+        return 0;
+}
 static struct usb_driver driver = {
        .name           = KBUILD_MODNAME,
        .id_table       = usb_ids,
        .probe          = probe,
        .disconnect     = disconnect,
+        .pre_reset      = pre_reset,
+        .post_reset     = post_reset,
 };
 struct workqueue_struct *zd_workqueue;
@@ -1393,15 +1614,20 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
                return -EWOULDBLOCK;
        }
        if (!usb_int_enabled(usb)) {
-                 dev_dbg_f(zd_usb_dev(usb),
+                dev_dbg_f(zd_usb_dev(usb),
                          "error: usb interrupt not enabled\n");
                return -EWOULDBLOCK;
        }
+        ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+        BUILD_BUG_ON(sizeof(struct usb_req_read_regs) + USB_MAX_IOREAD16_COUNT *
+                     sizeof(__le16) > sizeof(usb->req_buf));
+        BUG_ON(sizeof(struct usb_req_read_regs) + count * sizeof(__le16) >
+               sizeof(usb->req_buf));
        req_len = sizeof(struct usb_req_read_regs) + count * sizeof(__le16);
-        req = kmalloc(req_len, GFP_KERNEL);
+        req = (void *)usb->req_buf;
-        if (!req)
-                return -ENOMEM;
        req->id = cpu_to_le16(USB_REQ_READ_REGS);
        for (i = 0; i < count; i++)
                req->addr[i] = cpu_to_le16((u16)addresses[i]);
@@ -1409,7 +1635,7 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
        udev = zd_usb_to_usbdev(usb);
        prepare_read_regs_int(usb);
        r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-                         req, req_len, &actual_req_len, 1000 /* ms */);
+                         req, req_len, &actual_req_len, 50 /* ms */);
        if (r) {
                dev_dbg_f(zd_usb_dev(usb),
                        "error in usb_bulk_msg(). Error number %d\n", r);
@@ -1424,7 +1650,7 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
        }
        timeout = wait_for_completion_timeout(&usb->intr.read_regs.completion,
-                                              msecs_to_jiffies(1000));
+                                              msecs_to_jiffies(50));
        if (!timeout) {
                disable_read_regs_int(usb);
                dev_dbg_f(zd_usb_dev(usb), "read timed out\n");
@@ -1434,7 +1660,6 @@ int zd_usb_ioread16v(struct zd_usb *usb, u16 *values,
        r = get_results(usb, values, req, count);
 error:
-        kfree(req);
        return r;
 }
@@ -1460,11 +1685,17 @@ int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
                return -EWOULDBLOCK;
        }
+        ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+        BUILD_BUG_ON(sizeof(struct usb_req_write_regs) +
+                     USB_MAX_IOWRITE16_COUNT * sizeof(struct reg_data) >
+                     sizeof(usb->req_buf));
+        BUG_ON(sizeof(struct usb_req_write_regs) +
+               count * sizeof(struct reg_data) >
+               sizeof(usb->req_buf));
        req_len = sizeof(struct usb_req_write_regs) +
                  count * sizeof(struct reg_data);
-        req = kmalloc(req_len, GFP_KERNEL);
+        req = (void *)usb->req_buf;
-        if (!req)
-                return -ENOMEM;
        req->id = cpu_to_le16(USB_REQ_WRITE_REGS);
        for (i = 0; i < count; i++) {
@@ -1475,7 +1706,7 @@ int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
        udev = zd_usb_to_usbdev(usb);
        r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-                         req, req_len, &actual_req_len, 1000 /* ms */);
+                         req, req_len, &actual_req_len, 50 /* ms */);
        if (r) {
                dev_dbg_f(zd_usb_dev(usb),
                        "error in usb_bulk_msg(). Error number %d\n", r);
@@ -1492,7 +1723,6 @@ int zd_usb_iowrite16v(struct zd_usb *usb, const struct zd_ioreq16 *ioreqs,
        /* FALL-THROUGH with r == 0 */
 error:
-        kfree(req);
        return r;
 }
@@ -1537,14 +1767,19 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
        if (r) {
                dev_dbg_f(zd_usb_dev(usb),
                        "error %d: Couldn't read CR203\n", r);
-                goto out;
+                return r;
        }
        bit_value_template &= ~(RF_IF_LE|RF_CLK|RF_DATA);
+        ZD_ASSERT(mutex_is_locked(&zd_usb_to_chip(usb)->mutex));
+        BUILD_BUG_ON(sizeof(struct usb_req_rfwrite) +
+                     USB_MAX_RFWRITE_BIT_COUNT * sizeof(__le16) >
+                     sizeof(usb->req_buf));
+        BUG_ON(sizeof(struct usb_req_rfwrite) + bits * sizeof(__le16) >
+               sizeof(usb->req_buf));
        req_len = sizeof(struct usb_req_rfwrite) + bits * sizeof(__le16);
-        req = kmalloc(req_len, GFP_KERNEL);
+        req = (void *)usb->req_buf;
-        if (!req)
-                return -ENOMEM;
        req->id = cpu_to_le16(USB_REQ_WRITE_RF);
        /* 1: 3683a, but not used in ZYDAS driver */
@@ -1560,7 +1795,7 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
        udev = zd_usb_to_usbdev(usb);
        r = usb_bulk_msg(udev, usb_sndbulkpipe(udev, EP_REGS_OUT),
-                         req, req_len, &actual_req_len, 1000 /* ms */);
+                         req, req_len, &actual_req_len, 50 /* ms */);
        if (r) {
                dev_dbg_f(zd_usb_dev(usb),
                        "error in usb_bulk_msg(). Error number %d\n", r);
@@ -1576,6 +1811,5 @@ int zd_usb_rfwrite(struct zd_usb *usb, u32 value, u8 bits)
        /* FALL-THROUGH with r == 0 */
 out:
-        kfree(req);
        return r;
 }
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.h b/drivers/net/wireless/zd1211rw/zd_usb.h
index 1b1655cb7cb4..2d688f48a34c 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.h
+++ b/drivers/net/wireless/zd1211rw/zd_usb.h
@@ -32,6 +32,10 @@
 #define ZD_USB_TX_HIGH  5
 #define ZD_USB_TX_LOW   2
+#define ZD_TX_TIMEOUT           (HZ * 5)
+#define ZD_TX_WATCHDOG_INTERVAL round_jiffies_relative(HZ)
+#define ZD_RX_IDLE_INTERVAL     round_jiffies_relative(30 * HZ)
 enum devicetype {
        DEVICE_ZD1211  = 0,
        DEVICE_ZD1211B = 1,
@@ -162,6 +166,8 @@ struct zd_usb_interrupt {
        struct read_regs_int read_regs;
        spinlock_t lock;
        struct urb *urb;
+        void *buffer;
+        dma_addr_t buffer_dma;
        int interval;
        u8 read_regs_enabled:1;
 };
@@ -175,7 +181,9 @@ static inline struct usb_int_regs *get_read_regs(struct zd_usb_interrupt *intr)
 struct zd_usb_rx {
        spinlock_t lock;
-        u8 fragment[2*USB_MAX_RX_SIZE];
+        struct mutex setup_mutex;
+        struct delayed_work idle_work;
+        u8 fragment[2 * USB_MAX_RX_SIZE];
        unsigned int fragment_length;
        unsigned int usb_packet_size;
        struct urb **urbs;
@@ -184,19 +192,21 @@ struct zd_usb_rx {
 /**
 * struct zd_usb_tx - structure used for transmitting frames
+ * @enabled: atomic enabled flag, indicates whether tx is enabled
 * @lock: lock for transmission
- * @free_urb_list: list of free URBs, contains all the URBs, which can be used
+ * @submitted: anchor for URBs sent to device
 * @submitted_urbs: atomic integer that counts the URBs having sent to the
 *      device, which haven't been completed
- * @enabled: enabled flag, indicates whether tx is enabled
 * @stopped: indicates whether higher level tx queues are stopped
 */
 struct zd_usb_tx {
+        atomic_t enabled;
        spinlock_t lock;
-        struct list_head free_urb_list;
+        struct delayed_work watchdog_work;
+        struct sk_buff_head submitted_skbs;
+        struct usb_anchor submitted;
        int submitted_urbs;
-        int enabled;
+        u8 stopped:1, watchdog_enabled:1;
-        int stopped;
 };
 /* Contains the usb parts. The structure doesn't require a lock because intf
@@ -207,7 +217,8 @@ struct zd_usb {
        struct zd_usb_rx rx;
        struct zd_usb_tx tx;
        struct usb_interface *intf;
-        u8 is_zd1211b:1, initialized:1;
+        u8 req_buf[64]; /* zd_usb_iowrite16v needs 62 bytes */
+        u8 is_zd1211b:1, initialized:1, was_running:1;
 };
 #define zd_usb_dev(usb) (&usb->intf->dev)
@@ -234,12 +245,17 @@ void zd_usb_clear(struct zd_usb *usb);
 int zd_usb_scnprint_id(struct zd_usb *usb, char *buffer, size_t size);
+void zd_tx_watchdog_enable(struct zd_usb *usb);
+void zd_tx_watchdog_disable(struct zd_usb *usb);
 int zd_usb_enable_int(struct zd_usb *usb);
 void zd_usb_disable_int(struct zd_usb *usb);
 int zd_usb_enable_rx(struct zd_usb *usb);
 void zd_usb_disable_rx(struct zd_usb *usb);
+void zd_usb_reset_rx_idle_timer(struct zd_usb *usb);
 void zd_usb_enable_tx(struct zd_usb *usb);
 void zd_usb_disable_tx(struct zd_usb *usb);