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

Conflicts:

	drivers/net/ps3_gelic_wireless.c
	drivers/net/wireless/libertas/main.c

99 files changed, 6415 insertions, 4160 deletions

diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index d415d81b5444..654a78c31087 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -2284,6 +2284,19 @@ config GELIC_WIRELESS
           the driver automatically distinguishes the models, you can
           safely enable this option even if you have a wireless-less model.
 
+config GELIC_WIRELESS_OLD_PSK_INTERFACE
+       bool "PS3 Wireless private PSK interface (OBSOLETE)"
+       depends on GELIC_WIRELESS
+       help
+          This option retains the obsolete private interface to pass
+          the PSK from user space programs to the driver.  The PSK
+          stands for 'Pre Shared Key' and is used for WPA[2]-PSK
+          (WPA-Personal) environment.
+          If WPA[2]-PSK is used and you need to use old programs that
+          support only this old interface, say Y.  Otherwise N.
+
+          If unsure, say N.
+
 config GIANFAR
         tristate "Gianfar Ethernet"
         depends on FSL_SOC
diff --git a/drivers/net/ps3_gelic_wireless.c b/drivers/net/ps3_gelic_wireless.c
index e5e02339a5cc..aa963ac1e37b 100644
--- a/drivers/net/ps3_gelic_wireless.c
+++ b/drivers/net/ps3_gelic_wireless.c
@@ -45,7 +45,8 @@
 #include "ps3_gelic_wireless.h"
 
 
-static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan);
+static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan,
+                               u8 *essid, size_t essid_len);
 static int gelic_wl_try_associate(struct net_device *netdev);
 
 /*
@@ -105,6 +106,7 @@ static const struct eurus_cmd_arg_info cmd_info[GELIC_EURUS_CMD_MAX_INDEX] = {
         [GELIC_EURUS_CMD_GET_WEP_CFG]    = { .post_arg = 1},
         [GELIC_EURUS_CMD_GET_WPA_CFG]    = { .post_arg = 1},
         [GELIC_EURUS_CMD_GET_RSSI_CFG]   = { .post_arg = 1},
+        [GELIC_EURUS_CMD_START_SCAN]     = { .pre_arg = 1},
         [GELIC_EURUS_CMD_GET_SCAN]       = { .post_arg = 1},
 };
 
@@ -163,7 +165,9 @@ static void gelic_eurus_sync_cmd_worker(struct work_struct *work)
         card = port_to_card(wl_port(wl));
 
         if (cmd_info[cmd->cmd].pre_arg) {
-                arg1 = ps3_mm_phys_to_lpar(__pa(cmd->buffer));
+                arg1 = (cmd->buffer) ?
+                        ps3_mm_phys_to_lpar(__pa(cmd->buffer)) :
+                        0;
                 arg2 = cmd->buf_size;
         } else {
                 arg1 = 0;
@@ -350,7 +354,8 @@ static int gelic_wl_get_range(struct net_device *netdev,
 
         /* encryption capability */
         range->enc_capa = IW_ENC_CAPA_WPA |
-                IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP;
+                IW_ENC_CAPA_CIPHER_TKIP | IW_ENC_CAPA_CIPHER_CCMP |
+                IW_ENC_CAPA_4WAY_HANDSHAKE;
         if (wpa2_capable())
                 range->enc_capa |= IW_ENC_CAPA_WPA2;
         range->encoding_size[0] = 5;    /* 40bit WEP */
@@ -359,6 +364,9 @@ static int gelic_wl_get_range(struct net_device *netdev,
         range->num_encoding_sizes = 3;
         range->max_encoding_tokens = GELIC_WEP_KEYS;
 
+        /* scan capability */
+        range->scan_capa = IW_SCAN_CAPA_ESSID;
+
         pr_debug("%s: ->\n", __func__);
         return 0;
 
@@ -370,8 +378,18 @@ static int gelic_wl_set_scan(struct net_device *netdev,
                            union iwreq_data *wrqu, char *extra)
 {
         struct gelic_wl_info *wl = port_wl(netdev_priv(netdev));
-
-        return gelic_wl_start_scan(wl, 1);
+        struct iw_scan_req *req;
+        u8 *essid = NULL;
+        size_t essid_len = 0;
+
+        if (wrqu->data.length == sizeof(struct iw_scan_req) &&
+            wrqu->data.flags & IW_SCAN_THIS_ESSID) {
+                req = (struct iw_scan_req*)extra;
+                essid = req->essid;
+                essid_len = req->essid_len;
+                pr_debug("%s: ESSID scan =%s\n", __func__, essid);
+        }
+        return gelic_wl_start_scan(wl, 1, essid, essid_len);
 }
 
 #define OUI_LEN 3
@@ -1256,42 +1274,19 @@ static int gelic_wl_set_encodeext(struct net_device *netdev,
                 set_bit(key_index, &wl->key_enabled);
                 /* remember wep info changed */
                 set_bit(GELIC_WL_STAT_CONFIGURED, &wl->stat);
-        } else if ((alg == IW_ENCODE_ALG_TKIP) || (alg == IW_ENCODE_ALG_CCMP)) {
-                pr_debug("%s: TKIP/CCMP requested alg=%d\n", __func__, alg);
-                /* check key length */
-                if (IW_ENCODING_TOKEN_MAX < ext->key_len) {
-                        pr_info("%s: key is too long %d\n", __func__,
-                                ext->key_len);
+        } else if (alg == IW_ENCODE_ALG_PMK) {
+                if (ext->key_len != WPA_PSK_LEN) {
+                        pr_err("%s: PSK length wrong %d\n", __func__,
+                               ext->key_len);
                         ret = -EINVAL;
                         goto done;
                 }
-                if (alg == IW_ENCODE_ALG_CCMP) {
-                        pr_debug("%s: AES selected\n", __func__);
-                        wl->group_cipher_method = GELIC_WL_CIPHER_AES;
-                        wl->pairwise_cipher_method = GELIC_WL_CIPHER_AES;
-                        wl->wpa_level = GELIC_WL_WPA_LEVEL_WPA2;
-                } else {
-                        pr_debug("%s: TKIP selected, WPA forced\n", __func__);
-                        wl->group_cipher_method = GELIC_WL_CIPHER_TKIP;
-                        wl->pairwise_cipher_method = GELIC_WL_CIPHER_TKIP;
-                        /* FIXME: how do we do if WPA2 + TKIP? */
-                        wl->wpa_level = GELIC_WL_WPA_LEVEL_WPA;
-                }
-                if (flags & IW_ENCODE_RESTRICTED)
-                        BUG();
-                wl->auth_method = GELIC_EURUS_AUTH_OPEN;
-                /* We should use same key for both and unicast */
-                if (ext->ext_flags & IW_ENCODE_EXT_GROUP_KEY)
-                        pr_debug("%s: group key \n", __func__);
-                else
-                        pr_debug("%s: unicast key \n", __func__);
-                /* OK, update the key */
-                wl->key_len[key_index] = ext->key_len;
-                memset(wl->key[key_index], 0, IW_ENCODING_TOKEN_MAX);
-                memcpy(wl->key[key_index], ext->key, ext->key_len);
-                set_bit(key_index, &wl->key_enabled);
-                /* remember info changed */
-                set_bit(GELIC_WL_STAT_CONFIGURED, &wl->stat);
+                memset(wl->psk, 0, sizeof(wl->psk));
+                memcpy(wl->psk, ext->key, ext->key_len);
+                wl->psk_len = ext->key_len;
+                wl->psk_type = GELIC_EURUS_WPA_PSK_BIN;
+                /* remember PSK configured */
+                set_bit(GELIC_WL_STAT_WPA_PSK_SET, &wl->stat);
         }
 done:
         spin_unlock_irqrestore(&wl->lock, irqflag);
@@ -1397,6 +1392,7 @@ static int gelic_wl_get_mode(struct net_device *netdev,
         return 0;
 }
 
+#ifdef CONFIG_GELIC_WIRELESS_OLD_PSK_INTERFACE
 /* SIOCIWFIRSTPRIV */
 static int hex2bin(u8 *str, u8 *bin, unsigned int len)
 {
@@ -1501,6 +1497,7 @@ static int gelic_wl_priv_get_psk(struct net_device *net_dev,
         pr_debug("%s:-> %d\n", __func__, data->data.length);
         return 0;
 }
+#endif
 
 /* SIOCGIWNICKN */
 static int gelic_wl_get_nick(struct net_device *net_dev,
@@ -1524,15 +1521,20 @@ static struct iw_statistics *gelic_wl_get_wireless_stats(
         struct gelic_eurus_cmd *cmd;
         struct iw_statistics *is;
         struct gelic_eurus_rssi_info *rssi;
+        void *buf;
 
         pr_debug("%s: <-\n", __func__);
 
+        buf = (void *)__get_free_page(GFP_KERNEL);
+        if (!buf)
+                return NULL;
+
         is = &wl->iwstat;
         memset(is, 0, sizeof(*is));
         cmd = gelic_eurus_sync_cmd(wl, GELIC_EURUS_CMD_GET_RSSI_CFG,
-                                   wl->buf, sizeof(*rssi));
+                                   buf, sizeof(*rssi));
         if (cmd && !cmd->status && !cmd->cmd_status) {
-                rssi = wl->buf;
+                rssi = buf;
                 is->qual.level = be16_to_cpu(rssi->rssi);
                 is->qual.updated = IW_QUAL_LEVEL_UPDATED |
                         IW_QUAL_QUAL_INVALID | IW_QUAL_NOISE_INVALID;
@@ -1541,6 +1543,7 @@ static struct iw_statistics *gelic_wl_get_wireless_stats(
                 is->qual.updated = IW_QUAL_ALL_INVALID;
 
         kfree(cmd);
+        free_page((unsigned long)buf);
         pr_debug("%s: ->\n", __func__);
         return is;
 }
@@ -1548,10 +1551,13 @@ static struct iw_statistics *gelic_wl_get_wireless_stats(
 /*
  *  scanning helpers
  */
-static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan)
+static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan,
+                               u8 *essid, size_t essid_len)
 {
         struct gelic_eurus_cmd *cmd;
         int ret = 0;
+        void *buf = NULL;
+        size_t len;
 
         pr_debug("%s: <- always=%d\n", __func__, always_scan);
         if (mutex_lock_interruptible(&wl->scan_lock))
@@ -1574,12 +1580,27 @@ static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan)
                 complete(&wl->scan_done);
                 goto out;
         }
+
+        /* ESSID scan ? */
+        if (essid_len && essid) {
+                buf = (void *)__get_free_page(GFP_KERNEL);
+                if (!buf) {
+                        ret = -ENOMEM;
+                        goto out;
+                }
+                len = IW_ESSID_MAX_SIZE; /* hypervisor always requires 32 */
+                memset(buf, 0, len);
+                memcpy(buf, essid, essid_len);
+                pr_debug("%s: essid scan='%s'\n", __func__, (char *)buf);
+        } else
+                len = 0;
+
         /*
          * issue start scan request
          */
         wl->scan_stat = GELIC_WL_SCAN_STAT_SCANNING;
         cmd = gelic_eurus_sync_cmd(wl, GELIC_EURUS_CMD_START_SCAN,
-                                   NULL, 0);
+                                   buf, len);
         if (!cmd || cmd->status || cmd->cmd_status) {
                 wl->scan_stat = GELIC_WL_SCAN_STAT_INIT;
                 complete(&wl->scan_done);
@@ -1588,6 +1609,7 @@ static int gelic_wl_start_scan(struct gelic_wl_info *wl, int always_scan)
         }
         kfree(cmd);
 out:
+        free_page((unsigned long)buf);
         mutex_unlock(&wl->scan_lock);
         pr_debug("%s: ->\n", __func__);
         return ret;
@@ -1607,11 +1629,18 @@ static void gelic_wl_scan_complete_event(struct gelic_wl_info *wl)
         union iwreq_data data;
         unsigned long this_time = jiffies;
         unsigned int data_len, i, found, r;
+        void *buf;
         DECLARE_MAC_BUF(mac);
 
         pr_debug("%s:start\n", __func__);
         mutex_lock(&wl->scan_lock);
 
+        buf = (void *)__get_free_page(GFP_KERNEL);
+        if (!buf) {
+                pr_info("%s: scan buffer alloc failed\n", __func__);
+                goto out;
+        }
+
         if (wl->scan_stat != GELIC_WL_SCAN_STAT_SCANNING) {
                 /*
                  * stop() may be called while scanning, ignore result
@@ -1622,7 +1651,7 @@ static void gelic_wl_scan_complete_event(struct gelic_wl_info *wl)
         }
 
         cmd = gelic_eurus_sync_cmd(wl, GELIC_EURUS_CMD_GET_SCAN,
-                                   wl->buf, PAGE_SIZE);
+                                   buf, PAGE_SIZE);
         if (!cmd || cmd->status || cmd->cmd_status) {
                 wl->scan_stat = GELIC_WL_SCAN_STAT_INIT;
                 pr_info("%s:cmd failed\n", __func__);
@@ -1649,7 +1678,7 @@ static void gelic_wl_scan_complete_event(struct gelic_wl_info *wl)
         }
 
         /* put them in the newtork_list */
-        for (i = 0, scan_info_size = 0, scan_info = wl->buf;
+        for (i = 0, scan_info_size = 0, scan_info = buf;
              scan_info_size < data_len;
              i++, scan_info_size += be16_to_cpu(scan_info->size),
              scan_info = (void *)scan_info + be16_to_cpu(scan_info->size)) {
@@ -1726,6 +1755,7 @@ static void gelic_wl_scan_complete_event(struct gelic_wl_info *wl)
         wireless_send_event(port_to_netdev(wl_port(wl)), SIOCGIWSCAN, &data,
                             NULL);
 out:
+        free_page((unsigned long)buf);
         complete(&wl->scan_done);
         mutex_unlock(&wl->scan_lock);
         pr_debug("%s:end\n", __func__);
@@ -1848,7 +1878,10 @@ static int gelic_wl_do_wep_setup(struct gelic_wl_info *wl)
 
         pr_debug("%s: <-\n", __func__);
         /* we can assume no one should uses the buffer */
-        wep = wl->buf;
+        wep = (struct gelic_eurus_wep_cfg *)__get_free_page(GFP_KERNEL);
+        if (!wep)
+                return -ENOMEM;
+
         memset(wep, 0, sizeof(*wep));
 
         if (wl->group_cipher_method == GELIC_WL_CIPHER_WEP) {
@@ -1898,6 +1931,7 @@ static int gelic_wl_do_wep_setup(struct gelic_wl_info *wl)
 
         kfree(cmd);
 out:
+        free_page((unsigned long)wep);
         pr_debug("%s: ->\n", __func__);
         return ret;
 }
@@ -1941,7 +1975,10 @@ static int gelic_wl_do_wpa_setup(struct gelic_wl_info *wl)
 
         pr_debug("%s: <-\n", __func__);
         /* we can assume no one should uses the buffer */
-        wpa = wl->buf;
+        wpa = (struct gelic_eurus_wpa_cfg *)__get_free_page(GFP_KERNEL);
+        if (!wpa)
+                return -ENOMEM;
+
         memset(wpa, 0, sizeof(*wpa));
 
         if (!test_bit(GELIC_WL_STAT_WPA_PSK_SET, &wl->stat))
@@ -2000,6 +2037,7 @@ static int gelic_wl_do_wpa_setup(struct gelic_wl_info *wl)
         else if (cmd->status || cmd->cmd_status)
                 ret = -ENXIO;
         kfree(cmd);
+        free_page((unsigned long)wpa);
         pr_debug("%s: --> %d\n", __func__, ret);
         return ret;
 }
@@ -2018,7 +2056,10 @@ static int gelic_wl_associate_bss(struct gelic_wl_info *wl,
         pr_debug("%s: <-\n", __func__);
 
         /* do common config */
-        common = wl->buf;
+        common = (struct gelic_eurus_common_cfg *)__get_free_page(GFP_KERNEL);
+        if (!common)
+                return -ENOMEM;
+
         memset(common, 0, sizeof(*common));
         common->bss_type = cpu_to_be16(GELIC_EURUS_BSS_INFRA);
         common->op_mode = cpu_to_be16(GELIC_EURUS_OPMODE_11BG);
@@ -2104,6 +2145,7 @@ static int gelic_wl_associate_bss(struct gelic_wl_info *wl,
                 pr_info("%s: connected\n", __func__);
         }
 out:
+        free_page((unsigned long)common);
         pr_debug("%s: ->\n", __func__);
         return ret;
 }
@@ -2255,6 +2297,9 @@ static void gelic_wl_assoc_worker(struct work_struct *work)
 
         struct gelic_wl_scan_info *best_bss;
         int ret;
+        unsigned long irqflag;
+        u8 *essid;
+        size_t essid_len;
 
         wl = container_of(work, struct gelic_wl_info, assoc_work.work);
 
@@ -2263,7 +2308,19 @@ static void gelic_wl_assoc_worker(struct work_struct *work)
         if (wl->assoc_stat != GELIC_WL_ASSOC_STAT_DISCONN)
                 goto out;
 
-        ret = gelic_wl_start_scan(wl, 0);
+        spin_lock_irqsave(&wl->lock, irqflag);
+        if (test_bit(GELIC_WL_STAT_ESSID_SET, &wl->stat)) {
+                pr_debug("%s: assoc ESSID configured %s\n", __func__,
+                         wl->essid);
+                essid = wl->essid;
+                essid_len = wl->essid_len;
+        } else {
+                essid = NULL;
+                essid_len = 0;
+        }
+        spin_unlock_irqrestore(&wl->lock, irqflag);
+
+        ret = gelic_wl_start_scan(wl, 0, essid, essid_len);
         if (ret == -ERESTARTSYS) {
                 pr_debug("%s: scan start failed association\n", __func__);
                 schedule_delayed_work(&wl->assoc_work, HZ/10); /*FIXME*/
@@ -2351,6 +2408,7 @@ static const iw_handler gelic_wl_wext_handler[] =
         IW_IOCTL(SIOCGIWNICKN)          = gelic_wl_get_nick,
 };
 
+#ifdef CONFIG_GELIC_WIRELESS_OLD_PSK_INTERFACE
 static struct iw_priv_args gelic_wl_private_args[] =
 {
         {
@@ -2372,15 +2430,18 @@ static const iw_handler gelic_wl_private_handler[] =
         gelic_wl_priv_set_psk,
         gelic_wl_priv_get_psk,
 };
+#endif
 
 static const struct iw_handler_def gelic_wl_wext_handler_def = {
         .num_standard           = ARRAY_SIZE(gelic_wl_wext_handler),
         .standard               = gelic_wl_wext_handler,
         .get_wireless_stats     = gelic_wl_get_wireless_stats,
+#ifdef CONFIG_GELIC_WIRELESS_OLD_PSK_INTERFACE
         .num_private            = ARRAY_SIZE(gelic_wl_private_handler),
         .num_private_args       = ARRAY_SIZE(gelic_wl_private_args),
         .private                = gelic_wl_private_handler,
         .private_args           = gelic_wl_private_args,
+#endif
 };
 
 static struct net_device *gelic_wl_alloc(struct gelic_card *card)
@@ -2446,16 +2507,9 @@ static struct net_device *gelic_wl_alloc(struct gelic_card *card)
         BUILD_BUG_ON(PAGE_SIZE <
                      sizeof(struct gelic_eurus_scan_info) *
                      GELIC_EURUS_MAX_SCAN);
-        wl->buf = (void *)get_zeroed_page(GFP_KERNEL);
-        if (!wl->buf) {
-                pr_info("%s:buffer allocation failed\n", __func__);
-                goto fail_getpage;
-        }
         pr_debug("%s:end\n", __func__);
         return netdev;
 
-fail_getpage:
-        destroy_workqueue(wl->event_queue);
 fail_event_workqueue:
         destroy_workqueue(wl->eurus_cmd_queue);
 fail_cmd_workqueue:
@@ -2474,8 +2528,6 @@ static void gelic_wl_free(struct gelic_wl_info *wl)
 
         pr_debug("%s: <-\n", __func__);
 
-        free_page((unsigned long)wl->buf);
-
         pr_debug("%s: destroy queues\n", __func__);
         destroy_workqueue(wl->eurus_cmd_queue);
         destroy_workqueue(wl->event_queue);
diff --git a/drivers/net/ps3_gelic_wireless.h b/drivers/net/ps3_gelic_wireless.h
index bc730632da5b..5339e0078d18 100644
--- a/drivers/net/ps3_gelic_wireless.h
+++ b/drivers/net/ps3_gelic_wireless.h
@@ -288,9 +288,6 @@ struct gelic_wl_info {
         u8 active_bssid[ETH_ALEN]; /* associated bssid */
         unsigned int essid_len;
 
-        /* buffer for hypervisor IO */
-        void *buf;
-
         struct iw_public_data wireless_data;
         struct iw_statistics iwstat;
 };
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index 7af5d8851f67..0ba55ba93958 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -324,7 +324,7 @@ static void adm8211_interrupt_tci(struct ieee80211_hw *dev)
         for (dirty_tx = priv->dirty_tx; priv->cur_tx - dirty_tx; dirty_tx++) {
                 unsigned int entry = dirty_tx % priv->tx_ring_size;
                 u32 status = le32_to_cpu(priv->tx_ring[entry].status);
-                struct ieee80211_tx_status tx_status;
+                struct ieee80211_tx_info *txi;
                 struct adm8211_tx_ring_info *info;
                 struct sk_buff *skb;
 
@@ -334,24 +334,23 @@ static void adm8211_interrupt_tci(struct ieee80211_hw *dev)
 
                 info = &priv->tx_buffers[entry];
                 skb = info->skb;
+                txi = IEEE80211_SKB_CB(skb);
 
                 /* TODO: check TDES0_STATUS_TUF and TDES0_STATUS_TRO */
 
                 pci_unmap_single(priv->pdev, info->mapping,
                                  info->skb->len, PCI_DMA_TODEVICE);
 
-                memset(&tx_status, 0, sizeof(tx_status));
+                memset(&txi->status, 0, sizeof(txi->status));
                 skb_pull(skb, sizeof(struct adm8211_tx_hdr));
                 memcpy(skb_push(skb, info->hdrlen), skb->cb, info->hdrlen);
-                memcpy(&tx_status.control, &info->tx_control,
-                       sizeof(tx_status.control));
-                if (!(tx_status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
+                if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK)) {
                         if (status & TDES0_STATUS_ES)
-                                tx_status.excessive_retries = 1;
+                                txi->status.excessive_retries = 1;
                         else
-                                tx_status.flags |= IEEE80211_TX_STATUS_ACK;
+                                txi->flags |= IEEE80211_TX_STAT_ACK;
                 }
-                ieee80211_tx_status_irqsafe(dev, skb, &tx_status);
+                ieee80211_tx_status_irqsafe(dev, skb);
 
                 info->skb = NULL;
         }
@@ -1638,7 +1637,6 @@ static void adm8211_calc_durations(int *dur, int *plcp, size_t payload_len, int
 /* Transmit skb w/adm8211_tx_hdr (802.11 header created by hardware) */
 static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
                            u16 plcp_signal,
-                           struct ieee80211_tx_control *control,
                            size_t hdrlen)
 {
         struct adm8211_priv *priv = dev->priv;
@@ -1664,7 +1662,6 @@ static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
 
         priv->tx_buffers[entry].skb = skb;
         priv->tx_buffers[entry].mapping = mapping;
-        memcpy(&priv->tx_buffers[entry].tx_control, control, sizeof(*control));
         priv->tx_buffers[entry].hdrlen = hdrlen;
         priv->tx_ring[entry].buffer1 = cpu_to_le32(mapping);
 
@@ -1685,18 +1682,18 @@ static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
 }
 
 /* Put adm8211_tx_hdr on skb and transmit */
-static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-                      struct ieee80211_tx_control *control)
+static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
         struct adm8211_tx_hdr *txhdr;
         u16 fc;
         size_t payload_len, hdrlen;
         int plcp, dur, len, plcp_signal, short_preamble;
         struct ieee80211_hdr *hdr;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ieee80211_rate *txrate = ieee80211_get_tx_rate(dev, info);
 
-        short_preamble = !!(control->tx_rate->flags &
-                                        IEEE80211_TXCTL_SHORT_PREAMBLE);
-        plcp_signal = control->tx_rate->bitrate;
+        short_preamble = !!(txrate->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE);
+        plcp_signal = txrate->bitrate;
 
         hdr = (struct ieee80211_hdr *)skb->data;
         fc = le16_to_cpu(hdr->frame_control) & ~IEEE80211_FCTL_PROTECTED;
@@ -1730,15 +1727,15 @@ static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
         if (short_preamble)
                 txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_SHORT_PREAMBLE);
 
-        if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
                 txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_ENABLE_RTS);
 
         if (fc & IEEE80211_FCTL_PROTECTED)
                 txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_ENABLE_WEP_ENGINE);
 
-        txhdr->retry_limit = control->retry_limit;
+        txhdr->retry_limit = info->control.retry_limit;
 
-        adm8211_tx_raw(dev, skb, plcp_signal, control, hdrlen);
+        adm8211_tx_raw(dev, skb, plcp_signal, hdrlen);
 
         return NETDEV_TX_OK;
 }
@@ -2015,7 +2012,7 @@ static int adm8211_resume(struct pci_dev *pdev)
 
         if (priv->mode != IEEE80211_IF_TYPE_INVALID) {
                 adm8211_start(dev);
-                ieee80211_start_queues(dev);
+                ieee80211_wake_queues(dev);
         }
 
         return 0;
diff --git a/drivers/net/wireless/adm8211.h b/drivers/net/wireless/adm8211.h
index 8d7c564b3b04..9b190ee26e90 100644
--- a/drivers/net/wireless/adm8211.h
+++ b/drivers/net/wireless/adm8211.h
@@ -443,7 +443,6 @@ struct adm8211_rx_ring_info {
 struct adm8211_tx_ring_info {
         struct sk_buff *skb;
         dma_addr_t mapping;
-        struct ieee80211_tx_control tx_control;
         size_t hdrlen;
 };
 
diff --git a/drivers/net/wireless/ath5k/base.c b/drivers/net/wireless/ath5k/base.c
index c76ada178781..85045afc1ba7 100644
--- a/drivers/net/wireless/ath5k/base.c
+++ b/drivers/net/wireless/ath5k/base.c
@@ -167,8 +167,7 @@ static struct pci_driver ath5k_pci_driver = {
 /*
  * Prototypes - MAC 802.11 stack related functions
  */
-static int ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                struct ieee80211_tx_control *ctl);
+static int ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 static int ath5k_reset(struct ieee80211_hw *hw);
 static int ath5k_start(struct ieee80211_hw *hw);
 static void ath5k_stop(struct ieee80211_hw *hw);
@@ -196,8 +195,7 @@ static int ath5k_get_tx_stats(struct ieee80211_hw *hw,
 static u64 ath5k_get_tsf(struct ieee80211_hw *hw);
 static void ath5k_reset_tsf(struct ieee80211_hw *hw);
 static int ath5k_beacon_update(struct ieee80211_hw *hw,
-                struct sk_buff *skb,
-                struct ieee80211_tx_control *ctl);
+                struct sk_buff *skb);
 
 static struct ieee80211_ops ath5k_hw_ops = {
         .tx             = ath5k_tx,
@@ -251,9 +249,7 @@ static void	ath5k_desc_free(struct ath5k_softc *sc,
 static int      ath5k_rxbuf_setup(struct ath5k_softc *sc,
                                 struct ath5k_buf *bf);
 static int      ath5k_txbuf_setup(struct ath5k_softc *sc,
-                                struct ath5k_buf *bf,
-                                struct ieee80211_tx_control *ctl);
-
+                                struct ath5k_buf *bf);
 static inline void ath5k_txbuf_free(struct ath5k_softc *sc,
                                 struct ath5k_buf *bf)
 {
@@ -289,8 +285,7 @@ static void 	ath5k_tx_processq(struct ath5k_softc *sc,
 static void     ath5k_tasklet_tx(unsigned long data);
 /* Beacon handling */
 static int      ath5k_beacon_setup(struct ath5k_softc *sc,
-                                struct ath5k_buf *bf,
-                                struct ieee80211_tx_control *ctl);
+                                        struct ath5k_buf *bf);
 static void     ath5k_beacon_send(struct ath5k_softc *sc);
 static void     ath5k_beacon_config(struct ath5k_softc *sc);
 static void     ath5k_beacon_update_timers(struct ath5k_softc *sc, u64 bc_tsf);
@@ -1295,36 +1290,36 @@ ath5k_rxbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 }
 
 static int
-ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
-                struct ieee80211_tx_control *ctl)
+ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 {
         struct ath5k_hw *ah = sc->ah;
         struct ath5k_txq *txq = sc->txq;
         struct ath5k_desc *ds = bf->desc;
         struct sk_buff *skb = bf->skb;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         unsigned int pktlen, flags, keyidx = AR5K_TXKEYIX_INVALID;
         int ret;
 
         flags = AR5K_TXDESC_INTREQ | AR5K_TXDESC_CLRDMASK;
-        bf->ctl = *ctl;
+
         /* XXX endianness */
         bf->skbaddr = pci_map_single(sc->pdev, skb->data, skb->len,
                         PCI_DMA_TODEVICE);
 
-        if (ctl->flags & IEEE80211_TXCTL_NO_ACK)
+        if (info->flags & IEEE80211_TX_CTL_NO_ACK)
                 flags |= AR5K_TXDESC_NOACK;
 
         pktlen = skb->len;
 
-        if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT)) {
-                keyidx = ctl->hw_key->hw_key_idx;
-                pktlen += ctl->icv_len;
+        if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT)) {
+                keyidx = info->control.hw_key->hw_key_idx;
+                pktlen += info->control.icv_len;
         }
-
         ret = ah->ah_setup_tx_desc(ah, ds, pktlen,
                 ieee80211_get_hdrlen_from_skb(skb), AR5K_PKT_TYPE_NORMAL,
-                (sc->power_level * 2), ctl->tx_rate->hw_value,
-                ctl->retry_limit, keyidx, 0, flags, 0, 0);
+                (sc->power_level * 2),
+                ieee80211_get_tx_rate(sc->hw, info)->hw_value,
+                info->control.retry_limit, keyidx, 0, flags, 0, 0);
         if (ret)
                 goto err_unmap;
 
@@ -1599,7 +1594,7 @@ ath5k_txq_cleanup(struct ath5k_softc *sc)
                                         sc->txqs[i].link);
                         }
         }
-        ieee80211_start_queues(sc->hw); /* XXX move to callers */
+        ieee80211_wake_queues(sc->hw); /* XXX move to callers */
 
         for (i = 0; i < ARRAY_SIZE(sc->txqs); i++)
                 if (sc->txqs[i].setup)
@@ -1926,11 +1921,11 @@ next:
 static void
 ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 {
-        struct ieee80211_tx_status txs = {};
         struct ath5k_tx_status ts = {};
         struct ath5k_buf *bf, *bf0;
         struct ath5k_desc *ds;
         struct sk_buff *skb;
+        struct ieee80211_tx_info *info;
         int ret;
 
         spin_lock(&txq->lock);
@@ -1950,24 +1945,25 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
                 }
 
                 skb = bf->skb;
+                info = IEEE80211_SKB_CB(skb);
                 bf->skb = NULL;
+
                 pci_unmap_single(sc->pdev, bf->skbaddr, skb->len,
                                 PCI_DMA_TODEVICE);
 
-                txs.control = bf->ctl;
-                txs.retry_count = ts.ts_shortretry + ts.ts_longretry / 6;
+                info->status.retry_count = ts.ts_shortretry + ts.ts_longretry / 6;
                 if (unlikely(ts.ts_status)) {
                         sc->ll_stats.dot11ACKFailureCount++;
                         if (ts.ts_status & AR5K_TXERR_XRETRY)
-                                txs.excessive_retries = 1;
+                                info->status.excessive_retries = 1;
                         else if (ts.ts_status & AR5K_TXERR_FILT)
-                                txs.flags |= IEEE80211_TX_STATUS_TX_FILTERED;
+                                info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
                 } else {
-                        txs.flags |= IEEE80211_TX_STATUS_ACK;
-                        txs.ack_signal = ts.ts_rssi;
+                        info->flags |= IEEE80211_TX_STAT_ACK;
+                        info->status.ack_signal = ts.ts_rssi;
                 }
 
-                ieee80211_tx_status(sc->hw, skb, &txs);
+                ieee80211_tx_status(sc->hw, skb);
                 sc->tx_stats[txq->qnum].count++;
 
                 spin_lock(&sc->txbuflock);
@@ -2004,10 +2000,10 @@ ath5k_tasklet_tx(unsigned long data)
  * Setup the beacon frame for transmit.
  */
 static int
-ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
-                struct ieee80211_tx_control *ctl)
+ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 {
         struct sk_buff *skb = bf->skb;
+        struct  ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct ath5k_hw *ah = sc->ah;
         struct ath5k_desc *ds;
         int ret, antenna = 0;
@@ -2046,7 +2042,8 @@ ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
         ret = ah->ah_setup_tx_desc(ah, ds, skb->len,
                         ieee80211_get_hdrlen_from_skb(skb),
                         AR5K_PKT_TYPE_BEACON, (sc->power_level * 2),
-                        ctl->tx_rate->hw_value, 1, AR5K_TXKEYIX_INVALID,
+                        ieee80211_get_tx_rate(sc->hw, info)->hw_value,
+                        1, AR5K_TXKEYIX_INVALID,
                         antenna, flags, 0, 0);
         if (ret)
                 goto err_unmap;
@@ -2624,11 +2621,11 @@ ath5k_led_event(struct ath5k_softc *sc, int event)
 \********************/
 
 static int
-ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                        struct ieee80211_tx_control *ctl)
+ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct ath5k_softc *sc = hw->priv;
         struct ath5k_buf *bf;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         unsigned long flags;
         int hdrlen;
         int pad;
@@ -2654,13 +2651,13 @@ ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
                 memmove(skb->data, skb->data+pad, hdrlen);
         }
 
-        sc->led_txrate = ctl->tx_rate->hw_value;
+        sc->led_txrate = ieee80211_get_tx_rate(hw, info)->hw_value;
 
         spin_lock_irqsave(&sc->txbuflock, flags);
         if (list_empty(&sc->txbuf)) {
                 ATH5K_ERR(sc, "no further txbuf available, dropping packet\n");
                 spin_unlock_irqrestore(&sc->txbuflock, flags);
-                ieee80211_stop_queue(hw, ctl->queue);
+                ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
                 return -1;
         }
         bf = list_first_entry(&sc->txbuf, struct ath5k_buf, list);
@@ -2672,7 +2669,7 @@ ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 
         bf->skb = skb;
 
-        if (ath5k_txbuf_setup(sc, bf, ctl)) {
+        if (ath5k_txbuf_setup(sc, bf)) {
                 bf->skb = NULL;
                 spin_lock_irqsave(&sc->txbuflock, flags);
                 list_add_tail(&bf->list, &sc->txbuf);
@@ -3050,8 +3047,7 @@ ath5k_reset_tsf(struct ieee80211_hw *hw)
 }
 
 static int
-ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                        struct ieee80211_tx_control *ctl)
+ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct ath5k_softc *sc = hw->priv;
         int ret;
@@ -3067,7 +3063,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
 
         ath5k_txbuf_free(sc, sc->bbuf);
         sc->bbuf->skb = skb;
-        ret = ath5k_beacon_setup(sc, sc->bbuf, ctl);
+        ret = ath5k_beacon_setup(sc, sc->bbuf);
         if (ret)
                 sc->bbuf->skb = NULL;
         else
diff --git a/drivers/net/wireless/ath5k/base.h b/drivers/net/wireless/ath5k/base.h
index ecb17495488c..bb4b26d523ab 100644
--- a/drivers/net/wireless/ath5k/base.h
+++ b/drivers/net/wireless/ath5k/base.h
@@ -60,7 +60,6 @@ struct ath5k_buf {
         dma_addr_t              daddr;  /* physical addr of desc */
         struct sk_buff          *skb;   /* skbuff for buf */
         dma_addr_t              skbaddr;/* physical addr of skb data */
-        struct ieee80211_tx_control ctl;
 };
 
 /*
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 0c2bc061e8f3..e919189919bb 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -422,6 +422,26 @@ enum {
                                          B43_IRQ_RFKILL | \
                                          B43_IRQ_TX_OK)
 
+/* The firmware register to fetch the debug-IRQ reason from. */
+#define B43_DEBUGIRQ_REASON_REG         63
+/* Debug-IRQ reasons. */
+#define B43_DEBUGIRQ_PANIC              0       /* The firmware panic'ed */
+#define B43_DEBUGIRQ_DUMP_SHM           1       /* Dump shared SHM */
+#define B43_DEBUGIRQ_DUMP_REGS          2       /* Dump the microcode registers */
+#define B43_DEBUGIRQ_MARKER             3       /* A "marker" was thrown by the firmware. */
+#define B43_DEBUGIRQ_ACK                0xFFFF  /* The host writes that to ACK the IRQ */
+
+/* The firmware register that contains the "marker" line. */
+#define B43_MARKER_ID_REG               2
+#define B43_MARKER_LINE_REG             3
+
+/* The firmware register to fetch the panic reason from. */
+#define B43_FWPANIC_REASON_REG          3
+/* Firmware panic reason codes */
+#define B43_FWPANIC_DIE                 0 /* Firmware died. Don't auto-restart it. */
+#define B43_FWPANIC_RESTART             1 /* Firmware died. Schedule a controller reset. */
+
+
 /* Device specific rate values.
  * The actual values defined here are (rate_in_mbps * 2).
  * Some code depends on this. Don't change it. */
@@ -733,7 +753,6 @@ struct b43_wl {
         /* The beacon we are currently using (AP or IBSS mode).
          * This beacon stuff is protected by the irq_lock. */
         struct sk_buff *current_beacon;
-        struct ieee80211_tx_control beacon_txctl;
         bool beacon0_uploaded;
         bool beacon1_uploaded;
         struct work_struct beacon_update_trigger;
@@ -766,6 +785,13 @@ struct b43_firmware {
         u16 rev;
         /* Firmware patchlevel */
         u16 patch;
+
+        /* Set to true, if we are using an opensource firmware. */
+        bool opensource;
+        /* Set to true, if the core needs a PCM firmware, but
+         * we failed to load one. This is always false for
+         * core rev > 10, as these don't need PCM firmware. */
+        bool pcm_request_failed;
 };
 
 /* Device (802.11 core) initialization status. */
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index f50e2014ffbe..b4eadd908bea 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -1131,10 +1131,10 @@ struct b43_dmaring *parse_cookie(struct b43_wldev *dev, u16 cookie, int *slot)
 }
 
 static int dma_tx_fragment(struct b43_dmaring *ring,
-                           struct sk_buff *skb,
-                           struct ieee80211_tx_control *ctl)
+                           struct sk_buff *skb)
 {
         const struct b43_dma_ops *ops = ring->ops;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         u8 *header;
         int slot, old_top_slot, old_used_slots;
         int err;
@@ -1158,7 +1158,7 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
         header = &(ring->txhdr_cache[slot * hdrsize]);
         cookie = generate_cookie(ring, slot);
         err = b43_generate_txhdr(ring->dev, header,
-                                 skb->data, skb->len, ctl, cookie);
+                                 skb->data, skb->len, info, cookie);
         if (unlikely(err)) {
                 ring->current_slot = old_top_slot;
                 ring->used_slots = old_used_slots;
@@ -1180,7 +1180,6 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
         desc = ops->idx2desc(ring, slot, &meta);
         memset(meta, 0, sizeof(*meta));
 
-        memcpy(&meta->txstat.control, ctl, sizeof(*ctl));
         meta->skb = skb;
         meta->is_last_fragment = 1;
 
@@ -1210,7 +1209,7 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 
         ops->fill_descriptor(ring, desc, meta->dmaaddr, skb->len, 0, 1, 1);
 
-        if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+        if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
                 /* Tell the firmware about the cookie of the last
                  * mcast frame, so it can clear the more-data bit in it. */
                 b43_shm_write16(ring->dev, B43_SHM_SHARED,
@@ -1281,16 +1280,16 @@ static struct b43_dmaring * select_ring_by_priority(struct b43_wldev *dev,
         return ring;
 }
 
-int b43_dma_tx(struct b43_wldev *dev,
-               struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+int b43_dma_tx(struct b43_wldev *dev, struct sk_buff *skb)
 {
         struct b43_dmaring *ring;
         struct ieee80211_hdr *hdr;
         int err = 0;
         unsigned long flags;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
         hdr = (struct ieee80211_hdr *)skb->data;
-        if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+        if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
                 /* The multicast ring will be sent after the DTIM */
                 ring = dev->dma.tx_ring_mcast;
                 /* Set the more-data bit. Ucode will clear it on
@@ -1298,7 +1297,8 @@ int b43_dma_tx(struct b43_wldev *dev,
                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
         } else {
                 /* Decide by priority where to put this frame. */
-                ring = select_ring_by_priority(dev, ctl->queue);
+                ring = select_ring_by_priority(
+                        dev, skb_get_queue_mapping(skb));
         }
 
         spin_lock_irqsave(&ring->lock, flags);
@@ -1316,9 +1316,9 @@ int b43_dma_tx(struct b43_wldev *dev,
         /* Assign the queue number to the ring (if not already done before)
          * so TX status handling can use it. The queue to ring mapping is
          * static, so we don't need to store it per frame. */
-        ring->queue_prio = ctl->queue;
+        ring->queue_prio = skb_get_queue_mapping(skb);
 
-        err = dma_tx_fragment(ring, skb, ctl);
+        err = dma_tx_fragment(ring, skb);
         if (unlikely(err == -ENOKEY)) {
                 /* Drop this packet, as we don't have the encryption key
                  * anymore and must not transmit it unencrypted. */
@@ -1334,7 +1334,7 @@ int b43_dma_tx(struct b43_wldev *dev,
         if ((free_slots(ring) < SLOTS_PER_PACKET) ||
             should_inject_overflow(ring)) {
                 /* This TX ring is full. */
-                ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+                ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
                 ring->stopped = 1;
                 if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
                         b43dbg(dev->wl, "Stopped TX ring %d\n", ring->index);
@@ -1377,13 +1377,19 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
                                          b43_txhdr_size(dev), 1);
 
                 if (meta->is_last_fragment) {
-                        B43_WARN_ON(!meta->skb);
-                        /* Call back to inform the ieee80211 subsystem about the
-                         * status of the transmission.
-                         * Some fields of txstat are already filled in dma_tx().
+                        struct ieee80211_tx_info *info;
+
+                        BUG_ON(!meta->skb);
+
+                        info = IEEE80211_SKB_CB(meta->skb);
+
+                        memset(&info->status, 0, sizeof(info->status));
+
+                        /*
+                         * Call back to inform the ieee80211 subsystem about
+                         * the status of the transmission.
                          */
-                        frame_succeed = b43_fill_txstatus_report(
-                                                &(meta->txstat), status);
+                        frame_succeed = b43_fill_txstatus_report(info, status);
 #ifdef CONFIG_B43_DEBUG
                         if (frame_succeed)
                                 ring->nr_succeed_tx_packets++;
@@ -1391,8 +1397,8 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
                                 ring->nr_failed_tx_packets++;
                         ring->nr_total_packet_tries += status->frame_count;
 #endif /* DEBUG */
-                        ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
-                                                    &(meta->txstat));
+                        ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb);
+
                         /* skb is freed by ieee80211_tx_status_irqsafe() */
                         meta->skb = NULL;
                 } else {
diff --git a/drivers/net/wireless/b43/dma.h b/drivers/net/wireless/b43/dma.h
index 20acf885dba5..d1eb5c0848a5 100644
--- a/drivers/net/wireless/b43/dma.h
+++ b/drivers/net/wireless/b43/dma.h
@@ -181,7 +181,6 @@ struct b43_dmadesc_meta {
         dma_addr_t dmaaddr;
         /* ieee80211 TX status. Only used once per 802.11 frag. */
         bool is_last_fragment;
-        struct ieee80211_tx_status txstat;
 };
 
 struct b43_dmaring;
@@ -285,7 +284,7 @@ void b43_dma_get_tx_stats(struct b43_wldev *dev,
                           struct ieee80211_tx_queue_stats *stats);
 
 int b43_dma_tx(struct b43_wldev *dev,
-               struct sk_buff *skb, struct ieee80211_tx_control *ctl);
+               struct sk_buff *skb);
 void b43_dma_handle_txstatus(struct b43_wldev *dev,
                              const struct b43_txstatus *status);
 
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index fc23ba5309bd..f9c14c66434e 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1368,18 +1368,18 @@ static void b43_write_beacon_template(struct b43_wldev *dev,
         unsigned int rate;
         u16 ctl;
         int antenna;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
 
         bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
         len = min((size_t) dev->wl->current_beacon->len,
                   0x200 - sizeof(struct b43_plcp_hdr6));
-        rate = dev->wl->beacon_txctl.tx_rate->hw_value;
+        rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
 
         b43_write_template_common(dev, (const u8 *)bcn,
                                   len, ram_offset, shm_size_offset, rate);
 
         /* Write the PHY TX control parameters. */
-        antenna = b43_antenna_from_ieee80211(dev,
-                        dev->wl->beacon_txctl.antenna_sel_tx);
+        antenna = b43_antenna_from_ieee80211(dev, info->antenna_sel_tx);
         antenna = b43_antenna_to_phyctl(antenna);
         ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
         /* We can't send beacons with short preamble. Would get PHY errors. */
@@ -1430,11 +1430,17 @@ static void b43_write_beacon_template(struct b43_wldev *dev,
                 i += ie_len + 2;
         }
         if (!tim_found) {
-                b43warn(dev->wl, "Did not find a valid TIM IE in "
-                        "the beacon template packet. AP or IBSS operation "
-                        "may be broken.\n");
-        } else
-                b43dbg(dev->wl, "Updated beacon template\n");
+                /*
+                 * If ucode wants to modify TIM do it behind the beacon, this
+                 * will happen, for example, when doing mesh networking.
+                 */
+                b43_shm_write16(dev, B43_SHM_SHARED,
+                                B43_SHM_SH_TIMBPOS,
+                                len + sizeof(struct b43_plcp_hdr6));
+                b43_shm_write16(dev, B43_SHM_SHARED,
+                                B43_SHM_SH_DTIMPER, 0);
+        }
+        b43dbg(dev->wl, "Updated beacon template at 0x%x\n", ram_offset);
 }
 
 static void b43_write_probe_resp_plcp(struct b43_wldev *dev,
@@ -1549,7 +1555,8 @@ static void handle_irq_beacon(struct b43_wldev *dev)
         struct b43_wl *wl = dev->wl;
         u32 cmd, beacon0_valid, beacon1_valid;
 
-        if (!b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+        if (!b43_is_mode(wl, IEEE80211_IF_TYPE_AP) &&
+            !b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
                 return;
 
         /* This is the bottom half of the asynchronous beacon update. */
@@ -1613,8 +1620,7 @@ static void b43_beacon_update_trigger_work(struct work_struct *work)
 
 /* Asynchronously update the packet templates in template RAM.
  * Locking: Requires wl->irq_lock to be locked. */
-static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon,
-                                 const struct ieee80211_tx_control *txctl)
+static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon)
 {
         /* This is the top half of the ansynchronous beacon update.
          * The bottom half is the beacon IRQ.
@@ -1625,7 +1631,6 @@ static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon,
         if (wl->current_beacon)
                 dev_kfree_skb_any(wl->current_beacon);
         wl->current_beacon = beacon;
-        memcpy(&wl->beacon_txctl, txctl, sizeof(wl->beacon_txctl));
         wl->beacon0_uploaded = 0;
         wl->beacon1_uploaded = 0;
         queue_work(wl->hw->workqueue, &wl->beacon_update_trigger);
@@ -1664,9 +1669,100 @@ static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
         b43dbg(dev->wl, "Set beacon interval to %u\n", beacon_int);
 }
 
+static void b43_handle_firmware_panic(struct b43_wldev *dev)
+{
+        u16 reason;
+
+        /* Read the register that contains the reason code for the panic. */
+        reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_FWPANIC_REASON_REG);
+        b43err(dev->wl, "Whoopsy, firmware panic! Reason: %u\n", reason);
+
+        switch (reason) {
+        default:
+                b43dbg(dev->wl, "The panic reason is unknown.\n");
+                /* fallthrough */
+        case B43_FWPANIC_DIE:
+                /* Do not restart the controller or firmware.
+                 * The device is nonfunctional from now on.
+                 * Restarting would result in this panic to trigger again,
+                 * so we avoid that recursion. */
+                break;
+        case B43_FWPANIC_RESTART:
+                b43_controller_restart(dev, "Microcode panic");
+                break;
+        }
+}
+
 static void handle_irq_ucode_debug(struct b43_wldev *dev)
 {
-        //TODO
+        unsigned int i, cnt;
+        u16 reason, marker_id, marker_line;
+        __le16 *buf;
+
+        /* The proprietary firmware doesn't have this IRQ. */
+        if (!dev->fw.opensource)
+                return;
+
+        /* Read the register that contains the reason code for this IRQ. */
+        reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_DEBUGIRQ_REASON_REG);
+
+        switch (reason) {
+        case B43_DEBUGIRQ_PANIC:
+                b43_handle_firmware_panic(dev);
+                break;
+        case B43_DEBUGIRQ_DUMP_SHM:
+                if (!B43_DEBUG)
+                        break; /* Only with driver debugging enabled. */
+                buf = kmalloc(4096, GFP_ATOMIC);
+                if (!buf) {
+                        b43dbg(dev->wl, "SHM-dump: Failed to allocate memory\n");
+                        goto out;
+                }
+                for (i = 0; i < 4096; i += 2) {
+                        u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, i);
+                        buf[i / 2] = cpu_to_le16(tmp);
+                }
+                b43info(dev->wl, "Shared memory dump:\n");
+                print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET,
+                               16, 2, buf, 4096, 1);
+                kfree(buf);
+                break;
+        case B43_DEBUGIRQ_DUMP_REGS:
+                if (!B43_DEBUG)
+                        break; /* Only with driver debugging enabled. */
+                b43info(dev->wl, "Microcode register dump:\n");
+                for (i = 0, cnt = 0; i < 64; i++) {
+                        u16 tmp = b43_shm_read16(dev, B43_SHM_SCRATCH, i);
+                        if (cnt == 0)
+                                printk(KERN_INFO);
+                        printk("r%02u: 0x%04X  ", i, tmp);
+                        cnt++;
+                        if (cnt == 6) {
+                                printk("\n");
+                                cnt = 0;
+                        }
+                }
+                printk("\n");
+                break;
+        case B43_DEBUGIRQ_MARKER:
+                if (!B43_DEBUG)
+                        break; /* Only with driver debugging enabled. */
+                marker_id = b43_shm_read16(dev, B43_SHM_SCRATCH,
+                                           B43_MARKER_ID_REG);
+                marker_line = b43_shm_read16(dev, B43_SHM_SCRATCH,
+                                             B43_MARKER_LINE_REG);
+                b43info(dev->wl, "The firmware just executed the MARKER(%u) "
+                        "at line number %u\n",
+                        marker_id, marker_line);
+                break;
+        default:
+                b43dbg(dev->wl, "Debug-IRQ triggered for unknown reason: %u\n",
+                       reason);
+        }
+out:
+        /* Acknowledge the debug-IRQ, so the firmware can continue. */
+        b43_shm_write16(dev, B43_SHM_SCRATCH,
+                        B43_DEBUGIRQ_REASON_REG, B43_DEBUGIRQ_ACK);
 }
 
 /* Interrupt handler bottom-half */
@@ -1853,7 +1949,8 @@ static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
 
 static int do_request_fw(struct b43_wldev *dev,
                          const char *name,
-                         struct b43_firmware_file *fw)
+                         struct b43_firmware_file *fw,
+                         bool silent)
 {
         char path[sizeof(modparam_fwpostfix) + 32];
         const struct firmware *blob;
@@ -1877,9 +1974,15 @@ static int do_request_fw(struct b43_wldev *dev,
                  "b43%s/%s.fw",
                  modparam_fwpostfix, name);
         err = request_firmware(&blob, path, dev->dev->dev);
-        if (err) {
-                b43err(dev->wl, "Firmware file \"%s\" not found "
-                       "or load failed.\n", path);
+        if (err == -ENOENT) {
+                if (!silent) {
+                        b43err(dev->wl, "Firmware file \"%s\" not found\n",
+                               path);
+                }
+                return err;
+        } else if (err) {
+                b43err(dev->wl, "Firmware file \"%s\" request failed (err=%d)\n",
+                       path, err);
                 return err;
         }
         if (blob->size < sizeof(struct b43_fw_header))
@@ -1930,7 +2033,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
                 filename = "ucode13";
         else
                 goto err_no_ucode;
-        err = do_request_fw(dev, filename, &fw->ucode);
+        err = do_request_fw(dev, filename, &fw->ucode, 0);
         if (err)
                 goto err_load;
 
@@ -1941,8 +2044,13 @@ static int b43_request_firmware(struct b43_wldev *dev)
                 filename = NULL;
         else
                 goto err_no_pcm;
-        err = do_request_fw(dev, filename, &fw->pcm);
-        if (err)
+        fw->pcm_request_failed = 0;
+        err = do_request_fw(dev, filename, &fw->pcm, 1);
+        if (err == -ENOENT) {
+                /* We did not find a PCM file? Not fatal, but
+                 * core rev <= 10 must do without hwcrypto then. */
+                fw->pcm_request_failed = 1;
+        } else if (err)
                 goto err_load;
 
         /* Get initvals */
@@ -1960,7 +2068,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
                 if ((rev >= 5) && (rev <= 10))
                         filename = "b0g0initvals5";
                 else if (rev >= 13)
-                        filename = "lp0initvals13";
+                        filename = "b0g0initvals13";
                 else
                         goto err_no_initvals;
                 break;
@@ -1973,7 +2081,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
         default:
                 goto err_no_initvals;
         }
-        err = do_request_fw(dev, filename, &fw->initvals);
+        err = do_request_fw(dev, filename, &fw->initvals, 0);
         if (err)
                 goto err_load;
 
@@ -2007,7 +2115,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
         default:
                 goto err_no_initvals;
         }
-        err = do_request_fw(dev, filename, &fw->initvals_band);
+        err = do_request_fw(dev, filename, &fw->initvals_band, 0);
         if (err)
                 goto err_load;
 
@@ -2124,14 +2232,28 @@ static int b43_upload_microcode(struct b43_wldev *dev)
                 err = -EOPNOTSUPP;
                 goto error;
         }
-        b43info(dev->wl, "Loading firmware version %u.%u "
-                "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
-                fwrev, fwpatch,
-                (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
-                (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
-
         dev->fw.rev = fwrev;
         dev->fw.patch = fwpatch;
+        dev->fw.opensource = (fwdate == 0xFFFF);
+
+        if (dev->fw.opensource) {
+                /* Patchlevel info is encoded in the "time" field. */
+                dev->fw.patch = fwtime;
+                b43info(dev->wl, "Loading OpenSource firmware version %u.%u%s\n",
+                        dev->fw.rev, dev->fw.patch,
+                        dev->fw.pcm_request_failed ? " (Hardware crypto not supported)" : "");
+        } else {
+                b43info(dev->wl, "Loading firmware version %u.%u "
+                        "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
+                        fwrev, fwpatch,
+                        (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
+                        (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
+                if (dev->fw.pcm_request_failed) {
+                        b43warn(dev->wl, "No \"pcm5.fw\" firmware file found. "
+                                "Hardware accelerated cryptography is disabled.\n");
+                        b43_print_fw_helptext(dev->wl, 0);
+                }
+        }
 
         if (b43_is_old_txhdr_format(dev)) {
                 b43warn(dev->wl, "You are using an old firmware image. "
@@ -2376,7 +2498,8 @@ static void b43_adjust_opmode(struct b43_wldev *dev)
         ctl &= ~B43_MACCTL_BEACPROMISC;
         ctl |= B43_MACCTL_INFRA;
 
-        if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+        if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+            b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
                 ctl |= B43_MACCTL_AP;
         else if (b43_is_mode(wl, IEEE80211_IF_TYPE_IBSS))
                 ctl &= ~B43_MACCTL_INFRA;
@@ -2813,8 +2936,7 @@ static int b43_rng_init(struct b43_wl *wl)
 }
 
 static int b43_op_tx(struct ieee80211_hw *hw,
-                     struct sk_buff *skb,
-                     struct ieee80211_tx_control *ctl)
+                     struct sk_buff *skb)
 {
         struct b43_wl *wl = hw_to_b43_wl(hw);
         struct b43_wldev *dev = wl->current_dev;
@@ -2836,9 +2958,9 @@ static int b43_op_tx(struct ieee80211_hw *hw,
         err = -ENODEV;
         if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
                 if (b43_using_pio_transfers(dev))
-                        err = b43_pio_tx(dev, skb, ctl);
+                        err = b43_pio_tx(dev, skb);
                 else
-                        err = b43_dma_tx(dev, skb, ctl);
+                        err = b43_dma_tx(dev, skb);
         }
 
         read_unlock_irqrestore(&wl->tx_lock, flags);
@@ -3244,8 +3366,9 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
         antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_rx);
         b43_set_rx_antenna(dev, antenna);
 
-        /* Update templates for AP mode. */
-        if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+        /* Update templates for AP/mesh mode. */
+        if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+            b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
                 b43_set_beacon_int(dev, conf->beacon_int);
 
         if (!!conf->radio_enabled != phy->radio_on) {
@@ -3296,6 +3419,13 @@ static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
         if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
                 goto out_unlock;
 
+        if (dev->fw.pcm_request_failed) {
+                /* We don't have firmware for the crypto engine.
+                 * Must use software-crypto. */
+                err = -EOPNOTSUPP;
+                goto out_unlock;
+        }
+
         err = -EINVAL;
         switch (key->alg) {
         case ALG_WEP:
@@ -3426,13 +3556,12 @@ static int b43_op_config_interface(struct ieee80211_hw *hw,
         else
                 memset(wl->bssid, 0, ETH_ALEN);
         if (b43_status(dev) >= B43_STAT_INITIALIZED) {
-                if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP)) {
-                        B43_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
+                if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+                    b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT)) {
+                        B43_WARN_ON(conf->type != wl->if_type);
                         b43_set_ssid(dev, conf->ssid, conf->ssid_len);
-                        if (conf->beacon) {
-                                b43_update_templates(wl, conf->beacon,
-                                                     conf->beacon_control);
-                        }
+                        if (conf->beacon)
+                                b43_update_templates(wl, conf->beacon);
                 }
                 b43_write_mac_bssid_templates(dev);
         }
@@ -3497,7 +3626,6 @@ static int b43_wireless_core_start(struct b43_wldev *dev)
         /* Start data flow (TX/RX). */
         b43_mac_enable(dev);
         b43_interrupt_enable(dev, dev->irq_savedstate);
-        ieee80211_start_queues(dev->wl->hw);
 
         /* Start maintainance work */
         b43_periodic_tasks_setup(dev);
@@ -3970,6 +4098,7 @@ static int b43_op_add_interface(struct ieee80211_hw *hw,
         /* TODO: allow WDS/AP devices to coexist */
 
         if (conf->type != IEEE80211_IF_TYPE_AP &&
+            conf->type != IEEE80211_IF_TYPE_MESH_POINT &&
             conf->type != IEEE80211_IF_TYPE_STA &&
             conf->type != IEEE80211_IF_TYPE_WDS &&
             conf->type != IEEE80211_IF_TYPE_IBSS)
@@ -4119,31 +4248,29 @@ static int b43_op_beacon_set_tim(struct ieee80211_hw *hw, int aid, int set)
         struct b43_wl *wl = hw_to_b43_wl(hw);
         struct sk_buff *beacon;
         unsigned long flags;
-        struct ieee80211_tx_control txctl;
 
         /* We could modify the existing beacon and set the aid bit in
          * the TIM field, but that would probably require resizing and
          * moving of data within the beacon template.
          * Simply request a new beacon and let mac80211 do the hard work. */
-        beacon = ieee80211_beacon_get(hw, wl->vif, &txctl);
+        beacon = ieee80211_beacon_get(hw, wl->vif);
         if (unlikely(!beacon))
                 return -ENOMEM;
         spin_lock_irqsave(&wl->irq_lock, flags);
-        b43_update_templates(wl, beacon, &txctl);
+        b43_update_templates(wl, beacon);
         spin_unlock_irqrestore(&wl->irq_lock, flags);
 
         return 0;
 }
 
 static int b43_op_ibss_beacon_update(struct ieee80211_hw *hw,
-                                     struct sk_buff *beacon,
-                                     struct ieee80211_tx_control *ctl)
+                                     struct sk_buff *beacon)
 {
         struct b43_wl *wl = hw_to_b43_wl(hw);
         unsigned long flags;
 
         spin_lock_irqsave(&wl->irq_lock, flags);
-        b43_update_templates(wl, beacon, ctl);
+        b43_update_templates(wl, beacon);
         spin_unlock_irqrestore(&wl->irq_lock, flags);
 
         return 0;
diff --git a/drivers/net/wireless/b43/pio.c b/drivers/net/wireless/b43/pio.c
index 08c8a087f30e..8b1555d95f1c 100644
--- a/drivers/net/wireless/b43/pio.c
+++ b/drivers/net/wireless/b43/pio.c
@@ -446,29 +446,27 @@ static void pio_tx_frame_4byte_queue(struct b43_pio_txpacket *pack,
 }
 
 static int pio_tx_frame(struct b43_pio_txqueue *q,
-                        struct sk_buff *skb,
-                        struct ieee80211_tx_control *ctl)
+                        struct sk_buff *skb)
 {
         struct b43_pio_txpacket *pack;
         struct b43_txhdr txhdr;
         u16 cookie;
         int err;
         unsigned int hdrlen;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
         B43_WARN_ON(list_empty(&q->packets_list));
         pack = list_entry(q->packets_list.next,
                           struct b43_pio_txpacket, list);
-        memset(&pack->txstat, 0, sizeof(pack->txstat));
-        memcpy(&pack->txstat.control, ctl, sizeof(*ctl));
 
         cookie = generate_cookie(q, pack);
         hdrlen = b43_txhdr_size(q->dev);
         err = b43_generate_txhdr(q->dev, (u8 *)&txhdr, skb->data,
-                                 skb->len, ctl, cookie);
+                                 skb->len, info, cookie);
         if (err)
                 return err;
 
-        if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+        if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
                 /* Tell the firmware about the cookie of the last
                  * mcast frame, so it can clear the more-data bit in it. */
                 b43_shm_write16(q->dev, B43_SHM_SHARED,
@@ -492,17 +490,18 @@ static int pio_tx_frame(struct b43_pio_txqueue *q,
         return 0;
 }
 
-int b43_pio_tx(struct b43_wldev *dev,
-               struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb)
 {
         struct b43_pio_txqueue *q;
         struct ieee80211_hdr *hdr;
         unsigned long flags;
         unsigned int hdrlen, total_len;
         int err = 0;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
         hdr = (struct ieee80211_hdr *)skb->data;
-        if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+
+        if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
                 /* The multicast queue will be sent after the DTIM. */
                 q = dev->pio.tx_queue_mcast;
                 /* Set the frame More-Data bit. Ucode will clear it
@@ -510,7 +509,7 @@ int b43_pio_tx(struct b43_wldev *dev,
                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
         } else {
                 /* Decide by priority where to put this frame. */
-                q = select_queue_by_priority(dev, ctl->queue);
+                q = select_queue_by_priority(dev, skb_get_queue_mapping(skb));
         }
 
         spin_lock_irqsave(&q->lock, flags);
@@ -533,7 +532,7 @@ int b43_pio_tx(struct b43_wldev *dev,
         if (total_len > (q->buffer_size - q->buffer_used)) {
                 /* Not enough memory on the queue. */
                 err = -EBUSY;
-                ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+                ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
                 q->stopped = 1;
                 goto out_unlock;
         }
@@ -541,9 +540,9 @@ int b43_pio_tx(struct b43_wldev *dev,
         /* Assign the queue number to the ring (if not already done before)
          * so TX status handling can use it. The mac80211-queue to b43-queue
          * mapping is static, so we don't need to store it per frame. */
-        q->queue_prio = ctl->queue;
+        q->queue_prio = skb_get_queue_mapping(skb);
 
-        err = pio_tx_frame(q, skb, ctl);
+        err = pio_tx_frame(q, skb);
         if (unlikely(err == -ENOKEY)) {
                 /* Drop this packet, as we don't have the encryption key
                  * anymore and must not transmit it unencrypted. */
@@ -561,7 +560,7 @@ int b43_pio_tx(struct b43_wldev *dev,
         if (((q->buffer_size - q->buffer_used) < roundup(2 + 2 + 6, 4)) ||
             (q->free_packet_slots == 0)) {
                 /* The queue is full. */
-                ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+                ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
                 q->stopped = 1;
         }
 
@@ -578,6 +577,7 @@ void b43_pio_handle_txstatus(struct b43_wldev *dev,
         struct b43_pio_txqueue *q;
         struct b43_pio_txpacket *pack = NULL;
         unsigned int total_len;
+        struct ieee80211_tx_info *info;
 
         q = parse_cookie(dev, status->cookie, &pack);
         if (unlikely(!q))
@@ -586,15 +586,17 @@ void b43_pio_handle_txstatus(struct b43_wldev *dev,
 
         spin_lock(&q->lock); /* IRQs are already disabled. */
 
-        b43_fill_txstatus_report(&(pack->txstat), status);
+        info = (void *)pack->skb;
+        memset(&info->status, 0, sizeof(info->status));
+
+        b43_fill_txstatus_report(info, status);
 
         total_len = pack->skb->len + b43_txhdr_size(dev);
         total_len = roundup(total_len, 4);
         q->buffer_used -= total_len;
         q->free_packet_slots += 1;
 
-        ieee80211_tx_status_irqsafe(dev->wl->hw, pack->skb,
-                                    &(pack->txstat));
+        ieee80211_tx_status_irqsafe(dev->wl->hw, pack->skb);
         pack->skb = NULL;
         list_add(&pack->list, &q->packets_list);
 
diff --git a/drivers/net/wireless/b43/pio.h b/drivers/net/wireless/b43/pio.h
index e2ec676cc9e4..6c174c91ca20 100644
--- a/drivers/net/wireless/b43/pio.h
+++ b/drivers/net/wireless/b43/pio.h
@@ -62,8 +62,6 @@ struct b43_pio_txpacket {
         struct b43_pio_txqueue *queue;
         /* The TX data packet. */
         struct sk_buff *skb;
-        /* The status meta data. */
-        struct ieee80211_tx_status txstat;
         /* Index in the (struct b43_pio_txqueue)->packets array. */
         u8 index;
 
@@ -167,8 +165,7 @@ int b43_pio_init(struct b43_wldev *dev);
 void b43_pio_stop(struct b43_wldev *dev);
 void b43_pio_free(struct b43_wldev *dev);
 
-int b43_pio_tx(struct b43_wldev *dev,
-               struct sk_buff *skb, struct ieee80211_tx_control *ctl);
+int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb);
 void b43_pio_handle_txstatus(struct b43_wldev *dev,
                              const struct b43_txstatus *status);
 void b43_pio_get_tx_stats(struct b43_wldev *dev,
@@ -193,8 +190,7 @@ static inline void b43_pio_stop(struct b43_wldev *dev)
 {
 }
 static inline int b43_pio_tx(struct b43_wldev *dev,
-                             struct sk_buff *skb,
-                             struct ieee80211_tx_control *ctl)
+                             struct sk_buff *skb)
 {
         return 0;
 }
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index afce9338d83a..f9e1cff2aecb 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -185,14 +185,14 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                        u8 *_txhdr,
                        const unsigned char *fragment_data,
                        unsigned int fragment_len,
-                       const struct ieee80211_tx_control *txctl,
+                       const struct ieee80211_tx_info *info,
                        u16 cookie)
 {
         struct b43_txhdr *txhdr = (struct b43_txhdr *)_txhdr;
         const struct b43_phy *phy = &dev->phy;
         const struct ieee80211_hdr *wlhdr =
             (const struct ieee80211_hdr *)fragment_data;
-        int use_encryption = (!(txctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT));
+        int use_encryption = (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT));
         u16 fctl = le16_to_cpu(wlhdr->frame_control);
         struct ieee80211_rate *fbrate;
         u8 rate, rate_fb;
@@ -201,13 +201,14 @@ int b43_generate_txhdr(struct b43_wldev *dev,
         u32 mac_ctl = 0;
         u16 phy_ctl = 0;
         u8 extra_ft = 0;
+        struct ieee80211_rate *txrate;
 
         memset(txhdr, 0, sizeof(*txhdr));
 
-        WARN_ON(!txctl->tx_rate);
-        rate = txctl->tx_rate ? txctl->tx_rate->hw_value : B43_CCK_RATE_1MB;
+        txrate = ieee80211_get_tx_rate(dev->wl->hw, info);
+        rate = txrate ? txrate->hw_value : B43_CCK_RATE_1MB;
         rate_ofdm = b43_is_ofdm_rate(rate);
-        fbrate = txctl->alt_retry_rate ? : txctl->tx_rate;
+        fbrate = ieee80211_get_alt_retry_rate(dev->wl->hw, info) ? : txrate;
         rate_fb = fbrate->hw_value;
         rate_fb_ofdm = b43_is_ofdm_rate(rate_fb);
 
@@ -227,15 +228,13 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                  * use the original dur_id field. */
                 txhdr->dur_fb = wlhdr->duration_id;
         } else {
-                txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
-                                                                 txctl->vif,
-                                                                 fragment_len,
-                                                                 fbrate);
+                txhdr->dur_fb = ieee80211_generic_frame_duration(
+                        dev->wl->hw, info->control.vif, fragment_len, fbrate);
         }
 
         plcp_fragment_len = fragment_len + FCS_LEN;
         if (use_encryption) {
-                u8 key_idx = txctl->hw_key->hw_key_idx;
+                u8 key_idx = info->control.hw_key->hw_key_idx;
                 struct b43_key *key;
                 int wlhdr_len;
                 size_t iv_len;
@@ -253,7 +252,7 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                 }
 
                 /* Hardware appends ICV. */
-                plcp_fragment_len += txctl->icv_len;
+                plcp_fragment_len += info->control.icv_len;
 
                 key_idx = b43_kidx_to_fw(dev, key_idx);
                 mac_ctl |= (key_idx << B43_TXH_MAC_KEYIDX_SHIFT) &
@@ -261,7 +260,7 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                 mac_ctl |= (key->algorithm << B43_TXH_MAC_KEYALG_SHIFT) &
                            B43_TXH_MAC_KEYALG;
                 wlhdr_len = ieee80211_get_hdrlen(fctl);
-                iv_len = min((size_t) txctl->iv_len,
+                iv_len = min((size_t) info->control.iv_len,
                              ARRAY_SIZE(txhdr->iv));
                 memcpy(txhdr->iv, ((u8 *) wlhdr) + wlhdr_len, iv_len);
         }
@@ -292,10 +291,10 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                 phy_ctl |= B43_TXH_PHY_ENC_OFDM;
         else
                 phy_ctl |= B43_TXH_PHY_ENC_CCK;
-        if (txctl->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
+        if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
                 phy_ctl |= B43_TXH_PHY_SHORTPRMBL;
 
-        switch (b43_ieee80211_antenna_sanitize(dev, txctl->antenna_sel_tx)) {
+        switch (b43_ieee80211_antenna_sanitize(dev, info->antenna_sel_tx)) {
         case 0: /* Default */
                 phy_ctl |= B43_TXH_PHY_ANT01AUTO;
                 break;
@@ -316,34 +315,36 @@ int b43_generate_txhdr(struct b43_wldev *dev,
         }
 
         /* MAC control */
-        if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
                 mac_ctl |= B43_TXH_MAC_ACK;
         if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
               ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
                 mac_ctl |= B43_TXH_MAC_HWSEQ;
-        if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+        if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
                 mac_ctl |= B43_TXH_MAC_STMSDU;
         if (phy->type == B43_PHYTYPE_A)
                 mac_ctl |= B43_TXH_MAC_5GHZ;
-        if (txctl->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
+        if (info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
                 mac_ctl |= B43_TXH_MAC_LONGFRAME;
 
         /* Generate the RTS or CTS-to-self frame */
-        if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
-            (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+        if ((info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) ||
+            (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)) {
                 unsigned int len;
                 struct ieee80211_hdr *hdr;
                 int rts_rate, rts_rate_fb;
                 int rts_rate_ofdm, rts_rate_fb_ofdm;
                 struct b43_plcp_hdr6 *plcp;
+                struct ieee80211_rate *rts_cts_rate;
 
-                WARN_ON(!txctl->rts_cts_rate);
-                rts_rate = txctl->rts_cts_rate ? txctl->rts_cts_rate->hw_value : B43_CCK_RATE_1MB;
+                rts_cts_rate = ieee80211_get_rts_cts_rate(dev->wl->hw, info);
+
+                rts_rate = rts_cts_rate ? rts_cts_rate->hw_value : B43_CCK_RATE_1MB;
                 rts_rate_ofdm = b43_is_ofdm_rate(rts_rate);
                 rts_rate_fb = b43_calc_fallback_rate(rts_rate);
                 rts_rate_fb_ofdm = b43_is_ofdm_rate(rts_rate_fb);
 
-                if (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+                if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                         struct ieee80211_cts *cts;
 
                         if (b43_is_old_txhdr_format(dev)) {
@@ -353,9 +354,9 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                                 cts = (struct ieee80211_cts *)
                                         (txhdr->new_format.rts_frame);
                         }
-                        ieee80211_ctstoself_get(dev->wl->hw, txctl->vif,
+                        ieee80211_ctstoself_get(dev->wl->hw, info->control.vif,
                                                 fragment_data, fragment_len,
-                                                txctl, cts);
+                                                info, cts);
                         mac_ctl |= B43_TXH_MAC_SENDCTS;
                         len = sizeof(struct ieee80211_cts);
                 } else {
@@ -368,9 +369,9 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                                 rts = (struct ieee80211_rts *)
                                         (txhdr->new_format.rts_frame);
                         }
-                        ieee80211_rts_get(dev->wl->hw, txctl->vif,
+                        ieee80211_rts_get(dev->wl->hw, info->control.vif,
                                           fragment_data, fragment_len,
-                                          txctl, rts);
+                                          info, rts);
                         mac_ctl |= B43_TXH_MAC_SENDRTS;
                         len = sizeof(struct ieee80211_rts);
                 }
@@ -684,27 +685,27 @@ void b43_handle_txstatus(struct b43_wldev *dev,
 /* Fill out the mac80211 TXstatus report based on the b43-specific
  * txstatus report data. This returns a boolean whether the frame was
  * successfully transmitted. */
-bool b43_fill_txstatus_report(struct ieee80211_tx_status *report,
+bool b43_fill_txstatus_report(struct ieee80211_tx_info *report,
                               const struct b43_txstatus *status)
 {
         bool frame_success = 1;
 
         if (status->acked) {
                 /* The frame was ACKed. */
-                report->flags |= IEEE80211_TX_STATUS_ACK;
+                report->flags |= IEEE80211_TX_STAT_ACK;
         } else {
                 /* The frame was not ACKed... */
-                if (!(report->control.flags & IEEE80211_TXCTL_NO_ACK)) {
+                if (!(report->flags & IEEE80211_TX_CTL_NO_ACK)) {
                         /* ...but we expected an ACK. */
                         frame_success = 0;
-                        report->excessive_retries = 1;
+                        report->status.excessive_retries = 1;
                 }
         }
         if (status->frame_count == 0) {
                 /* The frame was not transmitted at all. */
-                report->retry_count = 0;
+                report->status.retry_count = 0;
         } else
-                report->retry_count = status->frame_count - 1;
+                report->status.retry_count = status->frame_count - 1;
 
         return frame_success;
 }
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
index b05f44e0d626..0215faf47541 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/b43/xmit.h
@@ -178,7 +178,7 @@ int b43_generate_txhdr(struct b43_wldev *dev,
                        u8 * txhdr,
                        const unsigned char *fragment_data,
                        unsigned int fragment_len,
-                       const struct ieee80211_tx_control *txctl, u16 cookie);
+                       const struct ieee80211_tx_info *txctl, u16 cookie);
 
 /* Transmit Status */
 struct b43_txstatus {
@@ -294,7 +294,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr);
 
 void b43_handle_txstatus(struct b43_wldev *dev,
                          const struct b43_txstatus *status);
-bool b43_fill_txstatus_report(struct ieee80211_tx_status *report,
+bool b43_fill_txstatus_report(struct ieee80211_tx_info *report,
                               const struct b43_txstatus *status);
 
 void b43_tx_suspend(struct b43_wldev *dev);
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
index d6686f713b6d..33cc256c5baf 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -1205,10 +1205,10 @@ struct b43legacy_dmaring *parse_cookie(struct b43legacy_wldev *dev,
 }
 
 static int dma_tx_fragment(struct b43legacy_dmaring *ring,
-                            struct sk_buff *skb,
-                            struct ieee80211_tx_control *ctl)
+                            struct sk_buff *skb)
 {
         const struct b43legacy_dma_ops *ops = ring->ops;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         u8 *header;
         int slot, old_top_slot, old_used_slots;
         int err;
@@ -1231,7 +1231,7 @@ static int dma_tx_fragment(struct b43legacy_dmaring *ring,
         header = &(ring->txhdr_cache[slot * sizeof(
                                struct b43legacy_txhdr_fw3)]);
         err = b43legacy_generate_txhdr(ring->dev, header,
-                                 skb->data, skb->len, ctl,
+                                 skb->data, skb->len, info,
                                  generate_cookie(ring, slot));
         if (unlikely(err)) {
                 ring->current_slot = old_top_slot;
@@ -1255,7 +1255,6 @@ static int dma_tx_fragment(struct b43legacy_dmaring *ring,
         desc = ops->idx2desc(ring, slot, &meta);
         memset(meta, 0, sizeof(*meta));
 
-        memcpy(&meta->txstat.control, ctl, sizeof(*ctl));
         meta->skb = skb;
         meta->is_last_fragment = 1;
 
@@ -1323,14 +1322,13 @@ int should_inject_overflow(struct b43legacy_dmaring *ring)
 }
 
 int b43legacy_dma_tx(struct b43legacy_wldev *dev,
-                     struct sk_buff *skb,
-                     struct ieee80211_tx_control *ctl)
+                     struct sk_buff *skb)
 {
         struct b43legacy_dmaring *ring;
         int err = 0;
         unsigned long flags;
 
-        ring = priority_to_txring(dev, ctl->queue);
+        ring = priority_to_txring(dev, skb_get_queue_mapping(skb));
         spin_lock_irqsave(&ring->lock, flags);
         B43legacy_WARN_ON(!ring->tx);
         if (unlikely(free_slots(ring) < SLOTS_PER_PACKET)) {
@@ -1343,7 +1341,7 @@ int b43legacy_dma_tx(struct b43legacy_wldev *dev,
          * That would be a mac80211 bug. */
         B43legacy_BUG_ON(ring->stopped);
 
-        err = dma_tx_fragment(ring, skb, ctl);
+        err = dma_tx_fragment(ring, skb);
         if (unlikely(err == -ENOKEY)) {
                 /* Drop this packet, as we don't have the encryption key
                  * anymore and must not transmit it unencrypted. */
@@ -1401,26 +1399,29 @@ void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
                                          1);
 
                 if (meta->is_last_fragment) {
-                        B43legacy_WARN_ON(!meta->skb);
+                        struct ieee80211_tx_info *info;
+                        BUG_ON(!meta->skb);
+                        info = IEEE80211_SKB_CB(meta->skb);
                         /* Call back to inform the ieee80211 subsystem about the
                          * status of the transmission.
                          * Some fields of txstat are already filled in dma_tx().
                          */
+
+                        memset(&info->status, 0, sizeof(info->status));
+
                         if (status->acked) {
-                                meta->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+                                info->flags |= IEEE80211_TX_STAT_ACK;
                         } else {
-                                if (!(meta->txstat.control.flags
-                                      & IEEE80211_TXCTL_NO_ACK))
-                                         meta->txstat.excessive_retries = 1;
+                                if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+                                         info->status.excessive_retries = 1;
                         }
                         if (status->frame_count == 0) {
                                 /* The frame was not transmitted at all. */
-                                meta->txstat.retry_count = 0;
+                                info->status.retry_count = 0;
                         } else
-                                meta->txstat.retry_count = status->frame_count
+                                info->status.retry_count = status->frame_count
                                                            - 1;
-                        ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
-                                                    &(meta->txstat));
+                        ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb);
                         /* skb is freed by ieee80211_tx_status_irqsafe() */
                         meta->skb = NULL;
                 } else {
diff --git a/drivers/net/wireless/b43legacy/dma.h b/drivers/net/wireless/b43legacy/dma.h
index 2dd488c5be2d..2f186003c31e 100644
--- a/drivers/net/wireless/b43legacy/dma.h
+++ b/drivers/net/wireless/b43legacy/dma.h
@@ -195,7 +195,6 @@ struct b43legacy_dmadesc_meta {
         dma_addr_t dmaaddr;
         /* ieee80211 TX status. Only used once per 802.11 frag. */
         bool is_last_fragment;
-        struct ieee80211_tx_status txstat;
 };
 
 struct b43legacy_dmaring;
@@ -297,8 +296,7 @@ void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
                                 struct ieee80211_tx_queue_stats *stats);
 
 int b43legacy_dma_tx(struct b43legacy_wldev *dev,
-                     struct sk_buff *skb,
-                     struct ieee80211_tx_control *ctl);
+                     struct sk_buff *skb);
 void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
                                    const struct b43legacy_txstatus *status);
 
@@ -323,8 +321,7 @@ void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
 }
 static inline
 int b43legacy_dma_tx(struct b43legacy_wldev *dev,
-                     struct sk_buff *skb,
-                     struct ieee80211_tx_control *ctl)
+                     struct sk_buff *skb)
 {
         return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 7755c59e0803..f706ca65f159 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -2358,8 +2358,7 @@ static int b43legacy_rng_init(struct b43legacy_wl *wl)
 }
 
 static int b43legacy_op_tx(struct ieee80211_hw *hw,
-                           struct sk_buff *skb,
-                           struct ieee80211_tx_control *ctl)
+                           struct sk_buff *skb)
 {
         struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
         struct b43legacy_wldev *dev = wl->current_dev;
@@ -2373,10 +2372,10 @@ static int b43legacy_op_tx(struct ieee80211_hw *hw,
         /* DMA-TX is done without a global lock. */
         if (b43legacy_using_pio(dev)) {
                 spin_lock_irqsave(&wl->irq_lock, flags);
-                err = b43legacy_pio_tx(dev, skb, ctl);
+                err = b43legacy_pio_tx(dev, skb);
                 spin_unlock_irqrestore(&wl->irq_lock, flags);
         } else
-                err = b43legacy_dma_tx(dev, skb, ctl);
+                err = b43legacy_dma_tx(dev, skb);
 out:
         if (unlikely(err))
                 return NETDEV_TX_BUSY;
@@ -2794,7 +2793,6 @@ static int b43legacy_wireless_core_start(struct b43legacy_wldev *dev)
         /* Start data flow (TX/RX) */
         b43legacy_mac_enable(dev);
         b43legacy_interrupt_enable(dev, dev->irq_savedstate);
-        ieee80211_start_queues(dev->wl->hw);
 
         /* Start maintenance work */
         b43legacy_periodic_tasks_setup(dev);
@@ -3410,7 +3408,7 @@ static int b43legacy_op_beacon_set_tim(struct ieee80211_hw *hw,
          * field, but that would probably require resizing and moving of data
          * within the beacon template. Simply request a new beacon and let
          * mac80211 do the hard work. */
-        beacon = ieee80211_beacon_get(hw, wl->vif, NULL);
+        beacon = ieee80211_beacon_get(hw, wl->vif);
         if (unlikely(!beacon))
                 return -ENOMEM;
         spin_lock_irqsave(&wl->irq_lock, flags);
@@ -3421,8 +3419,7 @@ static int b43legacy_op_beacon_set_tim(struct ieee80211_hw *hw,
 }
 
 static int b43legacy_op_ibss_beacon_update(struct ieee80211_hw *hw,
-                                           struct sk_buff *beacon,
-                                           struct ieee80211_tx_control *ctl)
+                                           struct sk_buff *beacon)
 {
         struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
         unsigned long flags;
diff --git a/drivers/net/wireless/b43legacy/pio.c b/drivers/net/wireless/b43legacy/pio.c
index 8d3d27d3cd67..a86c7647fa2d 100644
--- a/drivers/net/wireless/b43legacy/pio.c
+++ b/drivers/net/wireless/b43legacy/pio.c
@@ -196,7 +196,7 @@ static int pio_tx_write_fragment(struct b43legacy_pioqueue *queue,
         B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags != 0);
         err = b43legacy_generate_txhdr(queue->dev,
                                  txhdr, skb->data, skb->len,
-                                 &packet->txstat.control,
+                                 IEEE80211_SKB_CB(skb),
                                  generate_cookie(queue, packet));
         if (err)
                 return err;
@@ -463,8 +463,7 @@ err_destroy0:
 }
 
 int b43legacy_pio_tx(struct b43legacy_wldev *dev,
-                     struct sk_buff *skb,
-                     struct ieee80211_tx_control *ctl)
+                     struct sk_buff *skb)
 {
         struct b43legacy_pioqueue *queue = dev->pio.queue1;
         struct b43legacy_pio_txpacket *packet;
@@ -476,9 +475,6 @@ int b43legacy_pio_tx(struct b43legacy_wldev *dev,
                             list);
         packet->skb = skb;
 
-        memset(&packet->txstat, 0, sizeof(packet->txstat));
-        memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
-
         list_move_tail(&packet->list, &queue->txqueue);
         queue->nr_txfree--;
         queue->nr_tx_packets++;
@@ -494,6 +490,7 @@ void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
 {
         struct b43legacy_pioqueue *queue;
         struct b43legacy_pio_txpacket *packet;
+        struct ieee80211_tx_info *info;
 
         queue = parse_cookie(dev, status->cookie, &packet);
         B43legacy_WARN_ON(!queue);
@@ -505,11 +502,13 @@ void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
         queue->tx_devq_used -= (packet->skb->len +
                                 sizeof(struct b43legacy_txhdr_fw3));
 
+        info = IEEE80211_SKB_CB(packet->skb);
+        memset(&info->status, 0, sizeof(info->status));
+
         if (status->acked)
-                packet->txstat.flags |= IEEE80211_TX_STATUS_ACK;
-        packet->txstat.retry_count = status->frame_count - 1;
-        ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb,
-                                    &(packet->txstat));
+                info->flags |= IEEE80211_TX_STAT_ACK;
+        info->status.retry_count = status->frame_count - 1;
+        ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb);
         packet->skb = NULL;
 
         free_txpacket(packet, 1);
diff --git a/drivers/net/wireless/b43legacy/pio.h b/drivers/net/wireless/b43legacy/pio.h
index 5bfed0c40030..464fec05a06d 100644
--- a/drivers/net/wireless/b43legacy/pio.h
+++ b/drivers/net/wireless/b43legacy/pio.h
@@ -41,7 +41,6 @@ struct b43legacy_xmitstatus;
 struct b43legacy_pio_txpacket {
         struct b43legacy_pioqueue *queue;
         struct sk_buff *skb;
-        struct ieee80211_tx_status txstat;
         struct list_head list;
 };
 
@@ -104,8 +103,7 @@ int b43legacy_pio_init(struct b43legacy_wldev *dev);
 void b43legacy_pio_free(struct b43legacy_wldev *dev);
 
 int b43legacy_pio_tx(struct b43legacy_wldev *dev,
-                   struct sk_buff *skb,
-                   struct ieee80211_tx_control *ctl);
+                   struct sk_buff *skb);
 void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
                                  const struct b43legacy_txstatus *status);
 void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
@@ -132,8 +130,7 @@ void b43legacy_pio_free(struct b43legacy_wldev *dev)
 }
 static inline
 int b43legacy_pio_tx(struct b43legacy_wldev *dev,
-                   struct sk_buff *skb,
-                   struct ieee80211_tx_control *ctl)
+                   struct sk_buff *skb)
 {
         return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index bed9e041d6c5..82dc04d59446 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -188,11 +188,11 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
                                struct b43legacy_txhdr_fw3 *txhdr,
                                const unsigned char *fragment_data,
                                unsigned int fragment_len,
-                               const struct ieee80211_tx_control *txctl,
+                               const struct ieee80211_tx_info *info,
                                u16 cookie)
 {
         const struct ieee80211_hdr *wlhdr;
-        int use_encryption = (!(txctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT));
+        int use_encryption = (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT));
         u16 fctl;
         u8 rate;
         struct ieee80211_rate *rate_fb;
@@ -201,15 +201,18 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
         unsigned int plcp_fragment_len;
         u32 mac_ctl = 0;
         u16 phy_ctl = 0;
+        struct ieee80211_rate *tx_rate;
 
         wlhdr = (const struct ieee80211_hdr *)fragment_data;
         fctl = le16_to_cpu(wlhdr->frame_control);
 
         memset(txhdr, 0, sizeof(*txhdr));
 
-        rate = txctl->tx_rate->hw_value;
+        tx_rate = ieee80211_get_tx_rate(dev->wl->hw, info);
+
+        rate = tx_rate->hw_value;
         rate_ofdm = b43legacy_is_ofdm_rate(rate);
-        rate_fb = txctl->alt_retry_rate ? : txctl->tx_rate;
+        rate_fb = ieee80211_get_alt_retry_rate(dev->wl->hw, info) ? : tx_rate;
         rate_fb_ofdm = b43legacy_is_ofdm_rate(rate_fb->hw_value);
 
         txhdr->mac_frame_ctl = wlhdr->frame_control;
@@ -225,14 +228,14 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
                 txhdr->dur_fb = wlhdr->duration_id;
         } else {
                 txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
-                                                         txctl->vif,
+                                                         info->control.vif,
                                                          fragment_len,
                                                          rate_fb);
         }
 
         plcp_fragment_len = fragment_len + FCS_LEN;
         if (use_encryption) {
-                u8 key_idx = txctl->hw_key->hw_key_idx;
+                u8 key_idx = info->control.hw_key->hw_key_idx;
                 struct b43legacy_key *key;
                 int wlhdr_len;
                 size_t iv_len;
@@ -242,7 +245,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 
                 if (key->enabled) {
                         /* Hardware appends ICV. */
-                        plcp_fragment_len += txctl->icv_len;
+                        plcp_fragment_len += info->control.icv_len;
 
                         key_idx = b43legacy_kidx_to_fw(dev, key_idx);
                         mac_ctl |= (key_idx << B43legacy_TX4_MAC_KEYIDX_SHIFT) &
@@ -251,7 +254,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
                                    B43legacy_TX4_MAC_KEYALG_SHIFT) &
                                    B43legacy_TX4_MAC_KEYALG;
                         wlhdr_len = ieee80211_get_hdrlen(fctl);
-                        iv_len = min((size_t)txctl->iv_len,
+                        iv_len = min((size_t)info->control.iv_len,
                                      ARRAY_SIZE(txhdr->iv));
                         memcpy(txhdr->iv, ((u8 *)wlhdr) + wlhdr_len, iv_len);
                 } else {
@@ -275,7 +278,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
                 phy_ctl |= B43legacy_TX4_PHY_OFDM;
         if (dev->short_preamble)
                 phy_ctl |= B43legacy_TX4_PHY_SHORTPRMBL;
-        switch (txctl->antenna_sel_tx) {
+        switch (info->antenna_sel_tx) {
         case 0:
                 phy_ctl |= B43legacy_TX4_PHY_ANTLAST;
                 break;
@@ -290,21 +293,21 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
         }
 
         /* MAC control */
-        if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
                 mac_ctl |= B43legacy_TX4_MAC_ACK;
         if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
               ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
                 mac_ctl |= B43legacy_TX4_MAC_HWSEQ;
-        if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+        if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
                 mac_ctl |= B43legacy_TX4_MAC_STMSDU;
         if (rate_fb_ofdm)
                 mac_ctl |= B43legacy_TX4_MAC_FALLBACKOFDM;
-        if (txctl->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
+        if (info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
                 mac_ctl |= B43legacy_TX4_MAC_LONGFRAME;
 
         /* Generate the RTS or CTS-to-self frame */
-        if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
-            (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+        if ((info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) ||
+            (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)) {
                 unsigned int len;
                 struct ieee80211_hdr *hdr;
                 int rts_rate;
@@ -312,26 +315,26 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
                 int rts_rate_ofdm;
                 int rts_rate_fb_ofdm;
 
-                rts_rate = txctl->rts_cts_rate->hw_value;
+                rts_rate = ieee80211_get_rts_cts_rate(dev->wl->hw, info)->hw_value;
                 rts_rate_ofdm = b43legacy_is_ofdm_rate(rts_rate);
                 rts_rate_fb = b43legacy_calc_fallback_rate(rts_rate);
                 rts_rate_fb_ofdm = b43legacy_is_ofdm_rate(rts_rate_fb);
                 if (rts_rate_fb_ofdm)
                         mac_ctl |= B43legacy_TX4_MAC_CTSFALLBACKOFDM;
 
-                if (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+                if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                         ieee80211_ctstoself_get(dev->wl->hw,
-                                                txctl->vif,
+                                                info->control.vif,
                                                 fragment_data,
-                                                fragment_len, txctl,
+                                                fragment_len, info,
                                                 (struct ieee80211_cts *)
                                                 (txhdr->rts_frame));
                         mac_ctl |= B43legacy_TX4_MAC_SENDCTS;
                         len = sizeof(struct ieee80211_cts);
                 } else {
                         ieee80211_rts_get(dev->wl->hw,
-                                          txctl->vif,
-                                          fragment_data, fragment_len, txctl,
+                                          info->control.vif,
+                                          fragment_data, fragment_len, info,
                                           (struct ieee80211_rts *)
                                           (txhdr->rts_frame));
                         mac_ctl |= B43legacy_TX4_MAC_SENDRTS;
@@ -362,12 +365,12 @@ int b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
                               u8 *txhdr,
                               const unsigned char *fragment_data,
                               unsigned int fragment_len,
-                              const struct ieee80211_tx_control *txctl,
+                              const struct ieee80211_tx_info *info,
                               u16 cookie)
 {
         return generate_txhdr_fw3(dev, (struct b43legacy_txhdr_fw3 *)txhdr,
                            fragment_data, fragment_len,
-                           txctl, cookie);
+                           info, cookie);
 }
 
 static s8 b43legacy_rssi_postprocess(struct b43legacy_wldev *dev,
diff --git a/drivers/net/wireless/b43legacy/xmit.h b/drivers/net/wireless/b43legacy/xmit.h
index bab47928a0c9..e56777e0feab 100644
--- a/drivers/net/wireless/b43legacy/xmit.h
+++ b/drivers/net/wireless/b43legacy/xmit.h
@@ -80,7 +80,7 @@ int b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
                               u8 *txhdr,
                               const unsigned char *fragment_data,
                               unsigned int fragment_len,
-                              const struct ieee80211_tx_control *txctl,
+                              const struct ieee80211_tx_info *info,
                               u16 cookie);
 
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
index e51eeeff6992..10c64bdb314c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
@@ -445,8 +445,7 @@ static int rs_adjust_next_rate(struct iwl3945_priv *priv, int rate)
  */
 static void rs_tx_status(void *priv_rate,
                          struct net_device *dev,
-                         struct sk_buff *skb,
-                         struct ieee80211_tx_status *tx_resp)
+                         struct sk_buff *skb)
 {
         u8 retries, current_count;
         int scale_rate_index, first_index, last_index;
@@ -457,14 +456,15 @@ static void rs_tx_status(void *priv_rate,
         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
         struct iwl3945_rs_sta *rs_sta;
         struct ieee80211_supported_band *sband;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
         IWL_DEBUG_RATE("enter\n");
 
         sband = local->hw.wiphy->bands[local->hw.conf.channel->band];
 
 
-        retries = tx_resp->retry_count;
-        first_index = tx_resp->control.tx_rate->hw_value;
+        retries = info->status.retry_count;
+        first_index = sband->bitrates[info->tx_rate_idx].hw_value;
         if ((first_index < 0) || (first_index >= IWL_RATE_COUNT)) {
                 IWL_DEBUG_RATE("leave: Rate out of bounds: %d\n", first_index);
                 return;
@@ -525,11 +525,11 @@ static void rs_tx_status(void *priv_rate,
         /* Update the last index window with success/failure based on ACK */
         IWL_DEBUG_RATE("Update rate %d with %s.\n",
                        last_index,
-                       (tx_resp->flags & IEEE80211_TX_STATUS_ACK) ?
+                       (info->flags & IEEE80211_TX_STAT_ACK) ?
                        "success" : "failure");
         iwl3945_collect_tx_data(rs_sta,
                             &rs_sta->win[last_index],
-                            tx_resp->flags & IEEE80211_TX_STATUS_ACK, 1);
+                            info->flags & IEEE80211_TX_STAT_ACK, 1);
 
         /* We updated the rate scale window -- if its been more than
          * flush_time since the last run, schedule the flush
@@ -669,7 +669,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
             is_multicast_ether_addr(hdr->addr1) ||
             !sta || !sta->rate_ctrl_priv) {
                 IWL_DEBUG_RATE("leave: No STA priv data to update!\n");
-                sel->rate = rate_lowest(local, sband, sta);
+                sel->rate_idx = rate_lowest_index(local, sband, sta);
                 rcu_read_unlock();
                 return;
         }
@@ -813,7 +813,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 
         IWL_DEBUG_RATE("leave: %d\n", index);
 
-        sel->rate = &sband->bitrates[sta->txrate_idx];
+        sel->rate_idx = sta->txrate_idx;
 }
 
 static struct rate_control_ops rs_ops = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
index ad4e7b74ca24..0ba6889dfd41 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -283,8 +283,7 @@ static void iwl3945_tx_queue_reclaim(struct iwl3945_priv *priv,
                 q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
 
                 tx_info = &txq->txb[txq->q.read_ptr];
-                ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0],
-                                            &tx_info->status);
+                ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0]);
                 tx_info->skb[0] = NULL;
                 iwl3945_hw_txq_free_tfd(priv, txq);
         }
@@ -306,7 +305,7 @@ static void iwl3945_rx_reply_tx(struct iwl3945_priv *priv,
         int txq_id = SEQ_TO_QUEUE(sequence);
         int index = SEQ_TO_INDEX(sequence);
         struct iwl3945_tx_queue *txq = &priv->txq[txq_id];
-        struct ieee80211_tx_status *tx_status;
+        struct ieee80211_tx_info *info;
         struct iwl3945_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
         u32  status = le32_to_cpu(tx_resp->status);
         int rate_idx;
@@ -319,19 +318,22 @@ static void iwl3945_rx_reply_tx(struct iwl3945_priv *priv,
                 return;
         }
 
-        tx_status = &(txq->txb[txq->q.read_ptr].status);
+        info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+        memset(&info->status, 0, sizeof(info->status));
 
-        tx_status->retry_count = tx_resp->failure_frame;
+        info->status.retry_count = tx_resp->failure_frame;
         /* tx_status->rts_retry_count = tx_resp->failure_rts; */
-        tx_status->flags = ((status & TX_STATUS_MSK) == TX_STATUS_SUCCESS) ?
-                                IEEE80211_TX_STATUS_ACK : 0;
+        info->flags |= ((status & TX_STATUS_MSK) == TX_STATUS_SUCCESS) ?
+                                IEEE80211_TX_STAT_ACK : 0;
 
         IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) plcp rate %d retries %d\n",
                         txq_id, iwl3945_get_tx_fail_reason(status), status,
                         tx_resp->rate, tx_resp->failure_frame);
 
         rate_idx = iwl3945_hwrate_to_plcp_idx(tx_resp->rate);
-        tx_status->control.tx_rate = &priv->ieee_rates[rate_idx];
+        if (info->band == IEEE80211_BAND_5GHZ)
+                rate_idx -= IWL_FIRST_OFDM_RATE;
+        info->tx_rate_idx = rate_idx;
         IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
         iwl3945_tx_queue_reclaim(priv, txq_id, index);
 
@@ -958,11 +960,12 @@ u8 iwl3945_hw_find_station(struct iwl3945_priv *priv, const u8 *addr)
 */
 void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
                               struct iwl3945_cmd *cmd,
-                              struct ieee80211_tx_control *ctrl,
+                              struct ieee80211_tx_info *info,
                               struct ieee80211_hdr *hdr, int sta_id, int tx_id)
 {
         unsigned long flags;
-        u16 rate_index = min(ctrl->tx_rate->hw_value & 0xffff, IWL_RATE_COUNT - 1);
+        u16 hw_value = ieee80211_get_tx_rate(priv->hw, info)->hw_value;
+        u16 rate_index = min(hw_value & 0xffff, IWL_RATE_COUNT - 1);
         u16 rate_mask;
         int rate;
         u8 rts_retry_limit;
@@ -974,7 +977,7 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
         tx_flags = cmd->cmd.tx.tx_flags;
 
         /* We need to figure out how to get the sta->supp_rates while
-         * in this running context; perhaps encoding into ctrl->tx_rate? */
+         * in this running context */
         rate_mask = IWL_RATES_MASK;
 
         spin_lock_irqsave(&priv->sta_lock, flags);
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.h b/drivers/net/wireless/iwlwifi/iwl-3945.h
index 9fdc1405e853..a9b3edad3868 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.h
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.h
@@ -124,7 +124,6 @@ int iwl3945_x2_queue_used(const struct iwl3945_queue *q, int i);
 
 /* One for each TFD */
 struct iwl3945_tx_info {
-        struct ieee80211_tx_status status;
         struct sk_buff *skb[MAX_NUM_OF_TBS];
 };
 
@@ -645,7 +644,7 @@ extern unsigned int iwl3945_hw_get_beacon_cmd(struct iwl3945_priv *priv,
 extern int iwl3945_hw_get_rx_read(struct iwl3945_priv *priv);
 extern void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
                                      struct iwl3945_cmd *cmd,
-                                     struct ieee80211_tx_control *ctrl,
+                                     struct ieee80211_tx_info *info,
                                      struct ieee80211_hdr *hdr,
                                      int sta_id, int tx_id);
 extern int iwl3945_hw_reg_send_txpower(struct iwl3945_priv *priv);
@@ -836,8 +835,6 @@ struct iwl3945_priv {
 
         u8 mac80211_registered;
 
-        u32 notif_missed_beacons;
-
         /* Rx'd packet timing information */
         u32 last_beacon_time;
         u64 last_tsf;
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
index ee55b283226b..fc118335b60f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
@@ -100,9 +100,14 @@
 
 #include "iwl-commands.h"
 
-#define PCI_LINK_CTRL      0x0F0
+/* PCI registers */
+#define PCI_LINK_CTRL      0x0F0        /* 1 byte */
 #define PCI_POWER_SOURCE   0x0C8
 #define PCI_REG_WUM8       0x0E8
+
+/* PCI register values */
+#define PCI_LINK_VAL_L0S_EN     0x01
+#define PCI_LINK_VAL_L1_EN      0x02
 #define PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT         (0x80000000)
 
 #define TFD_QUEUE_SIZE_MAX      (256)
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
index 8e3660ebba7d..a955f9c1b9ee 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
@@ -282,14 +282,20 @@ static void rs_tl_rm_old_stats(struct iwl4965_traffic_load *tl, u32 curr_time)
  *      increment traffic load value for tid and also remove
  *      any old values if passed the certain time period
  */
-static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data, u8 tid)
+static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data,
+                             struct ieee80211_hdr *hdr)
 {
         u32 curr_time = jiffies_to_msecs(jiffies);
         u32 time_diff;
         s32 index;
         struct iwl4965_traffic_load *tl = NULL;
+        u16 fc = le16_to_cpu(hdr->frame_control);
+        u8 tid;
 
-        if (tid >= TID_MAX_LOAD_COUNT)
+        if (ieee80211_is_qos_data(fc)) {
+                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                tid = qc[0] & 0xf;
+        } else
                 return;
 
         tl = &lq_data->load[tid];
@@ -481,7 +487,7 @@ static u32 rate_n_flags_from_tbl(struct iwl4965_scale_tbl_info *tbl,
         u32 rate_n_flags = 0;
 
         if (is_legacy(tbl->lq_type)) {
-                rate_n_flags = iwl4965_rates[index].plcp;
+                rate_n_flags = iwl_rates[index].plcp;
                 if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
                         rate_n_flags |= RATE_MCS_CCK_MSK;
 
@@ -493,11 +499,11 @@ static u32 rate_n_flags_from_tbl(struct iwl4965_scale_tbl_info *tbl,
                 rate_n_flags = RATE_MCS_HT_MSK;
 
                 if (is_siso(tbl->lq_type))
-                        rate_n_flags |= iwl4965_rates[index].plcp_siso;
+                        rate_n_flags |= iwl_rates[index].plcp_siso;
                 else if (is_mimo2(tbl->lq_type))
-                        rate_n_flags |= iwl4965_rates[index].plcp_mimo2;
+                        rate_n_flags |= iwl_rates[index].plcp_mimo2;
                 else
-                        rate_n_flags |= iwl4965_rates[index].plcp_mimo3;
+                        rate_n_flags |= iwl_rates[index].plcp_mimo3;
         } else {
                 IWL_ERROR("Invalid tbl->lq_type %d\n", tbl->lq_type);
         }
@@ -697,7 +703,7 @@ static u16 rs_get_adjacent_rate(struct iwl_priv *priv, u8 index, u16 rate_mask,
 
         low = index;
         while (low != IWL_RATE_INVALID) {
-                low = iwl4965_rates[low].prev_rs;
+                low = iwl_rates[low].prev_rs;
                 if (low == IWL_RATE_INVALID)
                         break;
                 if (rate_mask & (1 << low))
@@ -707,7 +713,7 @@ static u16 rs_get_adjacent_rate(struct iwl_priv *priv, u8 index, u16 rate_mask,
 
         high = index;
         while (high != IWL_RATE_INVALID) {
-                high = iwl4965_rates[high].next_rs;
+                high = iwl_rates[high].next_rs;
                 if (high == IWL_RATE_INVALID)
                         break;
                 if (rate_mask & (1 << high))
@@ -779,8 +785,7 @@ out:
  * mac80211 sends us Tx status
  */
 static void rs_tx_status(void *priv_rate, struct net_device *dev,
-                         struct sk_buff *skb,
-                         struct ieee80211_tx_status *tx_resp)
+                         struct sk_buff *skb)
 {
         int status;
         u8 retries;
@@ -792,6 +797,7 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
         struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
         struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
         struct ieee80211_hw *hw = local_to_hw(local);
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct iwl4965_rate_scale_data *window = NULL;
         struct iwl4965_rate_scale_data *search_win = NULL;
         u32 tx_rate;
@@ -807,11 +813,11 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
                 return;
 
         /* This packet was aggregated but doesn't carry rate scale info */
-        if ((tx_resp->control.flags & IEEE80211_TXCTL_AMPDU) &&
-            !(tx_resp->flags & IEEE80211_TX_STATUS_AMPDU))
+        if ((info->flags & IEEE80211_TX_CTL_AMPDU) &&
+            !(info->flags & IEEE80211_TX_STAT_AMPDU))
                 return;
 
-        retries = tx_resp->retry_count;
+        retries = info->status.retry_count;
 
         if (retries > 15)
                 retries = 15;
@@ -856,20 +862,20 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
         if (priv->band == IEEE80211_BAND_5GHZ)
                 rs_index -= IWL_FIRST_OFDM_RATE;
 
-        if ((tx_resp->control.tx_rate == NULL) ||
+        if ((info->tx_rate_idx < 0) ||
             (tbl_type.is_SGI ^
-                !!(tx_resp->control.flags & IEEE80211_TXCTL_SHORT_GI)) ||
+                !!(info->flags & IEEE80211_TX_CTL_SHORT_GI)) ||
             (tbl_type.is_fat ^
-                !!(tx_resp->control.flags & IEEE80211_TXCTL_40_MHZ_WIDTH)) ||
+                !!(info->flags & IEEE80211_TX_CTL_40_MHZ_WIDTH)) ||
             (tbl_type.is_dup ^
-                !!(tx_resp->control.flags & IEEE80211_TXCTL_DUP_DATA)) ||
-            (tbl_type.ant_type ^ tx_resp->control.antenna_sel_tx) ||
+                !!(info->flags & IEEE80211_TX_CTL_DUP_DATA)) ||
+            (tbl_type.ant_type ^ info->antenna_sel_tx) ||
             (!!(tx_rate & RATE_MCS_HT_MSK) ^
-                !!(tx_resp->control.flags & IEEE80211_TXCTL_OFDM_HT)) ||
+                !!(info->flags & IEEE80211_TX_CTL_OFDM_HT)) ||
             (!!(tx_rate & RATE_MCS_GF_MSK) ^
-                !!(tx_resp->control.flags & IEEE80211_TXCTL_GREEN_FIELD)) ||
+                !!(info->flags & IEEE80211_TX_CTL_GREEN_FIELD)) ||
             (hw->wiphy->bands[priv->band]->bitrates[rs_index].bitrate !=
-                tx_resp->control.tx_rate->bitrate)) {
+             hw->wiphy->bands[info->band]->bitrates[info->tx_rate_idx].bitrate)) {
                 IWL_DEBUG_RATE("initial rate does not match 0x%x\n", tx_rate);
                 goto out;
         }
@@ -923,10 +929,7 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
         rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
 
         /* Update frame history window with "success" if Tx got ACKed ... */
-        if (tx_resp->flags & IEEE80211_TX_STATUS_ACK)
-                status = 1;
-        else
-                status = 0;
+        status = !!(info->flags & IEEE80211_TX_STAT_ACK);
 
         /* If type matches "search" table,
          * add final tx status to "search" history */
@@ -937,10 +940,10 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
                         tpt = search_tbl->expected_tpt[rs_index];
                 else
                         tpt = 0;
-                if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU)
+                if (info->flags & IEEE80211_TX_CTL_AMPDU)
                         rs_collect_tx_data(search_win, rs_index, tpt,
-                                           tx_resp->ampdu_ack_len,
-                                           tx_resp->ampdu_ack_map);
+                                           info->status.ampdu_ack_len,
+                                           info->status.ampdu_ack_map);
                 else
                         rs_collect_tx_data(search_win, rs_index, tpt,
                                            1, status);
@@ -953,10 +956,10 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
                         tpt = curr_tbl->expected_tpt[rs_index];
                 else
                         tpt = 0;
-                if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU)
+                if (info->flags & IEEE80211_TX_CTL_AMPDU)
                         rs_collect_tx_data(window, rs_index, tpt,
-                                           tx_resp->ampdu_ack_len,
-                                           tx_resp->ampdu_ack_map);
+                                           info->status.ampdu_ack_len,
+                                           info->status.ampdu_ack_map);
                 else
                         rs_collect_tx_data(window, rs_index, tpt,
                                            1, status);
@@ -965,10 +968,10 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
         /* If not searching for new mode, increment success/failed counter
          * ... these help determine when to start searching again */
         if (lq_sta->stay_in_tbl) {
-                if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU) {
-                        lq_sta->total_success += tx_resp->ampdu_ack_map;
+                if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+                        lq_sta->total_success += info->status.ampdu_ack_map;
                         lq_sta->total_failed +=
-                             (tx_resp->ampdu_ack_len - tx_resp->ampdu_ack_map);
+                             (info->status.ampdu_ack_len - info->status.ampdu_ack_map);
                 } else {
                         if (status)
                                 lq_sta->total_success++;
@@ -1333,7 +1336,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
                                 lq_sta->search_better_tbl = 1;
                                 goto out;
                         }
-
+                        break;
                 case IWL_LEGACY_SWITCH_SISO:
                         IWL_DEBUG_RATE("LQ: Legacy switch to SISO\n");
 
@@ -1419,9 +1422,9 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
                                 lq_sta->search_better_tbl = 1;
                                 goto out;
                         }
-
+                        break;
                 case IWL_SISO_SWITCH_MIMO2:
-                        IWL_DEBUG_RATE("LQ: SISO switch to MIMO\n");
+                        IWL_DEBUG_RATE("LQ: SISO switch to MIMO2\n");
                         memcpy(search_tbl, tbl, sz);
                         search_tbl->is_SGI = 0;
                         search_tbl->ant_type = ANT_AB; /*FIXME:RS*/
@@ -1433,6 +1436,15 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
                         }
                         break;
                 case IWL_SISO_SWITCH_GI:
+                        if (!tbl->is_fat &&
+                                !(priv->current_ht_config.sgf &
+                                                HT_SHORT_GI_20MHZ))
+                                break;
+                        if (tbl->is_fat &&
+                                !(priv->current_ht_config.sgf &
+                                                HT_SHORT_GI_40MHZ))
+                                break;
+
                         IWL_DEBUG_RATE("LQ: SISO toggle SGI/NGI\n");
 
                         memcpy(search_tbl, tbl, sz);
@@ -1515,6 +1527,15 @@ static int rs_move_mimo_to_other(struct iwl_priv *priv,
                         break;
 
                 case IWL_MIMO_SWITCH_GI:
+                        if (!tbl->is_fat &&
+                                !(priv->current_ht_config.sgf &
+                                                HT_SHORT_GI_20MHZ))
+                                break;
+                        if (tbl->is_fat &&
+                                !(priv->current_ht_config.sgf &
+                                                HT_SHORT_GI_40MHZ))
+                                break;
+
                         IWL_DEBUG_RATE("LQ: MIMO toggle SGI/NGI\n");
 
                         /* Set up new search table for MIMO */
@@ -1668,9 +1689,9 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
         u8 active_tbl = 0;
         u8 done_search = 0;
         u16 high_low;
+        s32 sr;
 #ifdef CONFIG_IWL4965_HT
         u8 tid = MAX_TID_COUNT;
-        __le16 *qc;
 #endif
 
         IWL_DEBUG_RATE("rate scale calculate new rate for skb\n");
@@ -1693,11 +1714,7 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
         lq_sta = (struct iwl4965_lq_sta *)sta->rate_ctrl_priv;
 
 #ifdef CONFIG_IWL4965_HT
-        qc = ieee80211_get_qos_ctrl(hdr);
-        if (qc) {
-                tid = (u8)(le16_to_cpu(*qc) & 0xf);
-                rs_tl_add_packet(lq_sta, tid);
-        }
+        rs_tl_add_packet(lq_sta, hdr);
 #endif
         /*
          * Select rate-scale / modulation-mode table to work with in
@@ -1848,6 +1865,8 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
         low = high_low & 0xff;
         high = (high_low >> 8) & 0xff;
 
+        sr = window->success_ratio;
+
         /* Collect measured throughputs for current and adjacent rates */
         current_tpt = window->average_tpt;
         if (low != IWL_RATE_INVALID)
@@ -1855,19 +1874,22 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
         if (high != IWL_RATE_INVALID)
                 high_tpt = tbl->win[high].average_tpt;
 
-        /* Assume rate increase */
-        scale_action = 1;
+        scale_action = 0;
 
         /* Too many failures, decrease rate */
-        if ((window->success_ratio <= IWL_RATE_DECREASE_TH) ||
-            (current_tpt == 0)) {
+        if ((sr <= IWL_RATE_DECREASE_TH) || (current_tpt == 0)) {
                 IWL_DEBUG_RATE("decrease rate because of low success_ratio\n");
                 scale_action = -1;
 
         /* No throughput measured yet for adjacent rates; try increase. */
         } else if ((low_tpt == IWL_INVALID_VALUE) &&
-                   (high_tpt == IWL_INVALID_VALUE))
-                scale_action = 1;
+                   (high_tpt == IWL_INVALID_VALUE)) {
+
+                if (high != IWL_RATE_INVALID && sr >= IWL_RATE_INCREASE_TH)
+                        scale_action = 1;
+                else if (low != IWL_RATE_INVALID)
+                        scale_action = -1;
+        }
 
         /* Both adjacent throughputs are measured, but neither one has better
          * throughput; we're using the best rate, don't change it! */
@@ -1883,9 +1905,10 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
                 /* Higher adjacent rate's throughput is measured */
                 if (high_tpt != IWL_INVALID_VALUE) {
                         /* Higher rate has better throughput */
-                        if (high_tpt > current_tpt)
+                        if (high_tpt > current_tpt &&
+                                        sr >= IWL_RATE_INCREASE_TH) {
                                 scale_action = 1;
-                        else {
+                        } else {
                                 IWL_DEBUG_RATE
                                     ("decrease rate because of high tpt\n");
                                 scale_action = -1;
@@ -1898,23 +1921,17 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
                                 IWL_DEBUG_RATE
                                     ("decrease rate because of low tpt\n");
                                 scale_action = -1;
-                        } else
+                        } else if (sr >= IWL_RATE_INCREASE_TH) {
                                 scale_action = 1;
+                        }
                 }
         }
 
         /* Sanity check; asked for decrease, but success rate or throughput
          * has been good at old rate.  Don't change it. */
-        if (scale_action == -1) {
-                if ((low != IWL_RATE_INVALID) &&
-                    ((window->success_ratio > IWL_RATE_HIGH_TH) ||
+        if ((scale_action == -1) && (low != IWL_RATE_INVALID) &&
+                    ((sr > IWL_RATE_HIGH_TH) ||
                      (current_tpt > (100 * tbl->expected_tpt[low]))))
-                        scale_action = 0;
-
-        /* Sanity check; asked for increase, but success rate has not been great
-         * even at old rate, higher rate will be worse.  Don't change it. */
-        } else if ((scale_action == 1) &&
-                   (window->success_ratio < IWL_RATE_INCREASE_TH))
                 scale_action = 0;
 
         switch (scale_action) {
@@ -1943,7 +1960,7 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
                     "high %d type %d\n",
                      index, scale_action, low, high, tbl->lq_type);
 
- lq_update:
+lq_update:
         /* Replace uCode's rate table for the destination station. */
         if (update_lq) {
                 rate = rate_n_flags_from_tbl(tbl, index, is_green);
@@ -2088,7 +2105,7 @@ static void rs_initialize_lq(struct iwl_priv *priv,
                 i = 0;
 
         /* FIXME:RS: This is also wrong in 4965 */
-        rate = iwl4965_rates[i].plcp;
+        rate = iwl_rates[i].plcp;
         rate |= RATE_MCS_ANT_B_MSK;
         rate &= ~RATE_MCS_ANT_A_MSK;
 
@@ -2135,7 +2152,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
         fc = le16_to_cpu(hdr->frame_control);
         if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1) ||
             !sta || !sta->rate_ctrl_priv) {
-                sel->rate = rate_lowest(local, sband, sta);
+                sel->rate_idx = rate_lowest_index(local, sband, sta);
                 goto out;
         }
 
@@ -2150,7 +2167,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
                 if (sta_id == IWL_INVALID_STATION) {
                         IWL_DEBUG_RATE("LQ: ADD station %s\n",
                                        print_mac(mac, hdr->addr1));
-                        sta_id = iwl4965_add_station_flags(priv, hdr->addr1,
+                        sta_id = iwl_add_station_flags(priv, hdr->addr1,
                                                         0, CMD_ASYNC, NULL);
                 }
                 if ((sta_id != IWL_INVALID_STATION)) {
@@ -2165,11 +2182,13 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 
 done:
         if ((i < 0) || (i > IWL_RATE_COUNT)) {
-                sel->rate = rate_lowest(local, sband, sta);
+                sel->rate_idx = rate_lowest_index(local, sband, sta);
                 goto out;
         }
 
-        sel->rate = &priv->ieee_rates[i];
+        if (sband->band == IEEE80211_BAND_5GHZ)
+                i -= IWL_FIRST_OFDM_RATE;
+        sel->rate_idx = i;
 out:
         rcu_read_unlock();
 }
@@ -2234,7 +2253,7 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
                 if (sta_id == IWL_INVALID_STATION) {
                         IWL_DEBUG_RATE("LQ: ADD station %s\n",
                                        print_mac(mac, sta->addr));
-                        sta_id = iwl4965_add_station_flags(priv, sta->addr,
+                        sta_id = iwl_add_station_flags(priv, sta->addr,
                                                         0, CMD_ASYNC, NULL);
                 }
                 if ((sta_id != IWL_INVALID_STATION)) {
@@ -2425,6 +2444,7 @@ static void rs_fill_link_cmd(const struct iwl_priv *priv,
                 repeat_rate--;
         }
 
+        lq_cmd->agg_params.agg_frame_cnt_limit = 64;
         lq_cmd->agg_params.agg_dis_start_th = 3;
         lq_cmd->agg_params.agg_time_limit = cpu_to_le16(4000);
 }
@@ -2691,7 +2711,7 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
                 int active = lq_sta->active_tbl;
 
                 cnt +=
-                    sprintf(&buf[cnt], " %2dMbs: ", iwl4965_rates[i].ieee / 2);
+                    sprintf(&buf[cnt], " %2dMbs: ", iwl_rates[i].ieee / 2);
 
                 mask = (1ULL << (IWL_RATE_MAX_WINDOW - 1));
                 for (j = 0; j < IWL_RATE_MAX_WINDOW; j++, mask >>= 1)
@@ -2702,7 +2722,7 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
                 samples += lq_sta->lq_info[active].win[i].counter;
                 good += lq_sta->lq_info[active].win[i].success_counter;
                 success += lq_sta->lq_info[active].win[i].success_counter *
-                           iwl4965_rates[i].ieee;
+                           iwl_rates[i].ieee;
 
                 if (lq_sta->lq_info[active].win[i].stamp) {
                         int delta =
@@ -2722,10 +2742,11 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
                 i = j;
         }
 
-        /* Display the average rate of all samples taken.
-         *
-         * NOTE:  We multiply # of samples by 2 since the IEEE measurement
-         * added from iwl4965_rates is actually 2X the rate */
+        /*
+         * Display the average rate of all samples taken.
+         * NOTE: We multiply # of samples by 2 since the IEEE measurement
+         * added from iwl_rates is actually 2X the rate.
+         */
         if (samples)
                 cnt += sprintf(&buf[cnt],
                          "\nAverage rate is %3d.%02dMbs over last %4dms\n"
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
index 7ea2041a22e0..1dd4124227a5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
@@ -29,7 +29,7 @@
 
 #include "iwl-dev.h"
 
-struct iwl4965_rate_info {
+struct iwl_rate_info {
         u8 plcp;        /* uCode API:  IWL_RATE_6M_PLCP, etc. */
         u8 plcp_siso;   /* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
         u8 plcp_mimo2;  /* uCode API:  IWL_RATE_MIMO2_6M_PLCP, etc. */
@@ -45,7 +45,7 @@ struct iwl4965_rate_info {
 
 /*
  * These serve as indexes into
- * struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT];
+ * struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
  */
 enum {
         IWL_RATE_1M_INDEX = 0,
@@ -240,7 +240,7 @@ enum {
 #define TID_MAX_TIME_DIFF ((TID_QUEUE_MAX_SIZE - 1) * TID_QUEUE_CELL_SPACING)
 #define TIME_WRAP_AROUND(x, y) (((y) > (x)) ? (y) - (x) : (0-(x)) + (y))
 
-extern const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT];
+extern const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
 
 enum iwl_table_type {
         LQ_NONE,
@@ -279,7 +279,7 @@ static inline u8 num_of_ant(u8 mask)
 
 static inline u8 iwl4965_get_prev_ieee_rate(u8 rate_index)
 {
-        u8 rate = iwl4965_rates[rate_index].prev_ieee;
+        u8 rate = iwl_rates[rate_index].prev_ieee;
 
         if (rate == IWL_RATE_INVALID)
                 rate = rate_index;
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index 17847f981e11..aee7014bcb94 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -44,6 +44,7 @@
 #include "iwl-io.h"
 #include "iwl-helpers.h"
 #include "iwl-calib.h"
+#include "iwl-sta.h"
 
 /* module parameters */
 static struct iwl_mod_params iwl4965_mod_params = {
@@ -54,30 +55,6 @@ static struct iwl_mod_params iwl4965_mod_params = {
         /* the rest are 0 by default */
 };
 
-#ifdef CONFIG_IWL4965_HT
-
-static const u16 default_tid_to_tx_fifo[] = {
-        IWL_TX_FIFO_AC1,
-        IWL_TX_FIFO_AC0,
-        IWL_TX_FIFO_AC0,
-        IWL_TX_FIFO_AC1,
-        IWL_TX_FIFO_AC2,
-        IWL_TX_FIFO_AC2,
-        IWL_TX_FIFO_AC3,
-        IWL_TX_FIFO_AC3,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_NONE,
-        IWL_TX_FIFO_AC3
-};
-
-#endif  /*CONFIG_IWL4965_HT */
-
 /* check contents of special bootstrap uCode SRAM */
 static int iwl4965_verify_bsm(struct iwl_priv *priv)
 {
@@ -156,15 +133,18 @@ static int iwl4965_load_bsm(struct iwl_priv *priv)
 
         IWL_DEBUG_INFO("Begin load bsm\n");
 
+        priv->ucode_type = UCODE_RT;
+
         /* make sure bootstrap program is no larger than BSM's SRAM size */
         if (len > IWL_MAX_BSM_SIZE)
                 return -EINVAL;
 
         /* Tell bootstrap uCode where to find the "Initialize" uCode
          *   in host DRAM ... host DRAM physical address bits 35:4 for 4965.
-         * NOTE:  iwl4965_initialize_alive_start() will replace these values,
+         * NOTE:  iwl_init_alive_start() will replace these values,
          *        after the "initialize" uCode has run, to point to
-         *        runtime/protocol instructions and backup data cache. */
+         *        runtime/protocol instructions and backup data cache.
+         */
         pinst = priv->ucode_init.p_addr >> 4;
         pdata = priv->ucode_init_data.p_addr >> 4;
         inst_len = priv->ucode_init.len;
@@ -345,8 +325,8 @@ int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
 
         /* 4965 legacy rate format, search for match in table */
         } else {
-                for (idx = 0; idx < ARRAY_SIZE(iwl4965_rates); idx++)
-                        if (iwl4965_rates[idx].plcp == (rate_n_flags & 0xFF))
+                for (idx = 0; idx < ARRAY_SIZE(iwl_rates); idx++)
+                        if (iwl_rates[idx].plcp == (rate_n_flags & 0xFF))
                                 return idx;
         }
 
@@ -357,55 +337,26 @@ int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
  * translate ucode response to mac80211 tx status control values
  */
 void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
-                                  struct ieee80211_tx_control *control)
+                                  struct ieee80211_tx_info *control)
 {
         int rate_index;
 
         control->antenna_sel_tx =
                 ((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
         if (rate_n_flags & RATE_MCS_HT_MSK)
-                control->flags |= IEEE80211_TXCTL_OFDM_HT;
+                control->flags |= IEEE80211_TX_CTL_OFDM_HT;
         if (rate_n_flags & RATE_MCS_GF_MSK)
-                control->flags |= IEEE80211_TXCTL_GREEN_FIELD;
+                control->flags |= IEEE80211_TX_CTL_GREEN_FIELD;
         if (rate_n_flags & RATE_MCS_FAT_MSK)
-                control->flags |= IEEE80211_TXCTL_40_MHZ_WIDTH;
+                control->flags |= IEEE80211_TX_CTL_40_MHZ_WIDTH;
         if (rate_n_flags & RATE_MCS_DUP_MSK)
-                control->flags |= IEEE80211_TXCTL_DUP_DATA;
+                control->flags |= IEEE80211_TX_CTL_DUP_DATA;
         if (rate_n_flags & RATE_MCS_SGI_MSK)
-                control->flags |= IEEE80211_TXCTL_SHORT_GI;
-        /* since iwl4965_hwrate_to_plcp_idx is band indifferent, we always use
-         * IEEE80211_BAND_2GHZ band as it contains all the rates */
+                control->flags |= IEEE80211_TX_CTL_SHORT_GI;
         rate_index = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
-        if (rate_index == -1)
-                control->tx_rate = NULL;
-        else
-                control->tx_rate =
-                        &priv->bands[IEEE80211_BAND_2GHZ].bitrates[rate_index];
-}
-
-int iwl4965_hw_rxq_stop(struct iwl_priv *priv)
-{
-        int rc;
-        unsigned long flags;
-
-        spin_lock_irqsave(&priv->lock, flags);
-        rc = iwl_grab_nic_access(priv);
-        if (rc) {
-                spin_unlock_irqrestore(&priv->lock, flags);
-                return rc;
-        }
-
-        /* stop Rx DMA */
-        iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
-        rc = iwl_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
-                                     (1 << 24), 1000);
-        if (rc < 0)
-                IWL_ERROR("Can't stop Rx DMA.\n");
-
-        iwl_release_nic_access(priv);
-        spin_unlock_irqrestore(&priv->lock, flags);
-
-        return 0;
+        if (control->band == IEEE80211_BAND_5GHZ)
+                rate_index -= IWL_FIRST_OFDM_RATE;
+        control->tx_rate_idx = rate_index;
 }
 
 /*
@@ -467,25 +418,13 @@ int iwl4965_set_pwr_src(struct iwl_priv *priv, enum iwl_pwr_src src)
         return ret;
 }
 
-static int iwl4965_disable_tx_fifo(struct iwl_priv *priv)
+/*
+ * Activate/Deactivat Tx DMA/FIFO channels according tx fifos mask
+ * must be called under priv->lock and mac access
+ */
+static void iwl4965_txq_set_sched(struct iwl_priv *priv, u32 mask)
 {
-        unsigned long flags;
-        int ret;
-
-        spin_lock_irqsave(&priv->lock, flags);
-
-        ret = iwl_grab_nic_access(priv);
-        if (unlikely(ret)) {
-                IWL_ERROR("Tx fifo reset failed");
-                spin_unlock_irqrestore(&priv->lock, flags);
-                return ret;
-        }
-
-        iwl_write_prph(priv, IWL49_SCD_TXFACT, 0);
-        iwl_release_nic_access(priv);
-        spin_unlock_irqrestore(&priv->lock, flags);
-
-        return 0;
+        iwl_write_prph(priv, IWL49_SCD_TXFACT, mask);
 }
 
 static int iwl4965_apm_init(struct iwl_priv *priv)
@@ -495,6 +434,10 @@ static int iwl4965_apm_init(struct iwl_priv *priv)
         iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
                           CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 
+        /* disable L0s without affecting L1 :don't wait for ICH L0s bug W/A) */
+        iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+                          CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+
         /* set "initialization complete" bit to move adapter
          * D0U* --> D0A* state */
         iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
@@ -513,11 +456,12 @@ static int iwl4965_apm_init(struct iwl_priv *priv)
                 goto out;
 
         /* enable DMA */
-        iwl_write_prph(priv, APMG_CLK_CTRL_REG,
-                        APMG_CLK_VAL_DMA_CLK_RQT | APMG_CLK_VAL_BSM_CLK_RQT);
+        iwl_write_prph(priv, APMG_CLK_CTRL_REG, APMG_CLK_VAL_DMA_CLK_RQT |
+                                                APMG_CLK_VAL_BSM_CLK_RQT);
 
         udelay(20);
 
+        /* disable L1-Active */
         iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
                           APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
@@ -545,8 +489,13 @@ static void iwl4965_nic_config(struct iwl_priv *priv)
 
         pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
 
-        /* disable L1 entry -- workaround for pre-B1 */
-        pci_write_config_byte(priv->pci_dev, PCI_LINK_CTRL, val_link & ~0x02);
+        /* L1 is enabled by BIOS */
+        if ((val_link & PCI_LINK_VAL_L1_EN) == PCI_LINK_VAL_L1_EN)
+                /* diable L0S disabled L1A enabled */
+                iwl_set_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
+        else
+                /* L0S enabled L1A disabled */
+                iwl_clear_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
 
         radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
@@ -568,10 +517,9 @@ static void iwl4965_nic_config(struct iwl_priv *priv)
         spin_unlock_irqrestore(&priv->lock, flags);
 }
 
-int iwl4965_hw_nic_stop_master(struct iwl_priv *priv)
+static int iwl4965_apm_stop_master(struct iwl_priv *priv)
 {
-        int rc = 0;
-        u32 reg_val;
+        int ret = 0;
         unsigned long flags;
 
         spin_lock_irqsave(&priv->lock, flags);
@@ -579,64 +527,41 @@ int iwl4965_hw_nic_stop_master(struct iwl_priv *priv)
         /* set stop master bit */
         iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
 
-        reg_val = iwl_read32(priv, CSR_GP_CNTRL);
-
-        if (CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE ==
-            (reg_val & CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE))
-                IWL_DEBUG_INFO("Card in power save, master is already "
-                               "stopped\n");
-        else {
-                rc = iwl_poll_bit(priv, CSR_RESET,
+        ret = iwl_poll_bit(priv, CSR_RESET,
                                   CSR_RESET_REG_FLAG_MASTER_DISABLED,
                                   CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
-                if (rc < 0) {
-                        spin_unlock_irqrestore(&priv->lock, flags);
-                        return rc;
-                }
-        }
+        if (ret < 0)
+                goto out;
 
+out:
         spin_unlock_irqrestore(&priv->lock, flags);
         IWL_DEBUG_INFO("stop master\n");
 
-        return rc;
+        return ret;
 }
 
-/**
- * iwl4965_hw_txq_ctx_stop - Stop all Tx DMA channels, free Tx queue memory
- */
-void iwl4965_hw_txq_ctx_stop(struct iwl_priv *priv)
+static void iwl4965_apm_stop(struct iwl_priv *priv)
 {
-
-        int txq_id;
         unsigned long flags;
 
-        /* Stop each Tx DMA channel, and wait for it to be idle */
-        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
-                spin_lock_irqsave(&priv->lock, flags);
-                if (iwl_grab_nic_access(priv)) {
-                        spin_unlock_irqrestore(&priv->lock, flags);
-                        continue;
-                }
+        iwl4965_apm_stop_master(priv);
 
-                iwl_write_direct32(priv,
-                                   FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
-                iwl_poll_direct_bit(priv, FH_TSSR_TX_STATUS_REG,
-                                    FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
-                                    (txq_id), 200);
-                iwl_release_nic_access(priv);
-                spin_unlock_irqrestore(&priv->lock, flags);
-        }
+        spin_lock_irqsave(&priv->lock, flags);
+
+        iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
 
-        /* Deallocate memory for all Tx queues */
-        iwl_hw_txq_ctx_free(priv);
+        udelay(10);
+
+        iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+        spin_unlock_irqrestore(&priv->lock, flags);
 }
 
-int iwl4965_hw_nic_reset(struct iwl_priv *priv)
+static int iwl4965_apm_reset(struct iwl_priv *priv)
 {
-        int rc = 0;
+        int ret = 0;
         unsigned long flags;
 
-        iwl4965_hw_nic_stop_master(priv);
+        iwl4965_apm_stop_master(priv);
 
         spin_lock_irqsave(&priv->lock, flags);
 
@@ -644,34 +569,41 @@ int iwl4965_hw_nic_reset(struct iwl_priv *priv)
 
         udelay(10);
 
+        /* FIXME: put here L1A -L0S w/a */
+
         iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
-        rc = iwl_poll_bit(priv, CSR_RESET,
+
+        ret = iwl_poll_bit(priv, CSR_RESET,
                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
                           CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25);
 
+        if (ret)
+                goto out;
+
         udelay(10);
 
-        rc = iwl_grab_nic_access(priv);
-        if (!rc) {
-                iwl_write_prph(priv, APMG_CLK_EN_REG,
-                                APMG_CLK_VAL_DMA_CLK_RQT |
-                                APMG_CLK_VAL_BSM_CLK_RQT);
+        ret = iwl_grab_nic_access(priv);
+        if (ret)
+                goto out;
+        /* Enable DMA and BSM Clock */
+        iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT |
+                                              APMG_CLK_VAL_BSM_CLK_RQT);
 
-                udelay(10);
+        udelay(10);
 
-                iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
-                                        APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+        /* disable L1A */
+        iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+                          APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
-                iwl_release_nic_access(priv);
-        }
+        iwl_release_nic_access(priv);
 
         clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
         wake_up_interruptible(&priv->wait_command_queue);
 
+out:
         spin_unlock_irqrestore(&priv->lock, flags);
 
-        return rc;
-
+        return ret;
 }
 
 #define REG_RECALIB_PERIOD (60)
@@ -911,16 +843,6 @@ static const u16 default_queue_to_tx_fifo[] = {
         IWL_TX_FIFO_HCCA_2
 };
 
-static inline void iwl4965_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
-{
-        set_bit(txq_id, &priv->txq_ctx_active_msk);
-}
-
-static inline void iwl4965_txq_ctx_deactivate(struct iwl_priv *priv, int txq_id)
-{
-        clear_bit(txq_id, &priv->txq_ctx_active_msk);
-}
-
 int iwl4965_alive_notify(struct iwl_priv *priv)
 {
         u32 a;
@@ -930,15 +852,6 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
 
         spin_lock_irqsave(&priv->lock, flags);
 
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
-        memset(&(priv->sensitivity_data), 0,
-               sizeof(struct iwl_sensitivity_data));
-        memset(&(priv->chain_noise_data), 0,
-               sizeof(struct iwl_chain_noise_data));
-        for (i = 0; i < NUM_RX_CHAINS; i++)
-                priv->chain_noise_data.delta_gain_code[i] =
-                                CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
-#endif /* CONFIG_IWL4965_RUN_TIME_CALIB*/
         ret = iwl_grab_nic_access(priv);
         if (ret) {
                 spin_unlock_irqrestore(&priv->lock, flags);
@@ -990,24 +903,20 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
                                  (1 << priv->hw_params.max_txq_num) - 1);
 
         /* Activate all Tx DMA/FIFO channels */
-        iwl_write_prph(priv, IWL49_SCD_TXFACT,
-                                 SCD_TXFACT_REG_TXFIFO_MASK(0, 7));
+        priv->cfg->ops->lib->txq_set_sched(priv, IWL_MASK(0, 7));
 
         iwl4965_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
 
         /* Map each Tx/cmd queue to its corresponding fifo */
         for (i = 0; i < ARRAY_SIZE(default_queue_to_tx_fifo); i++) {
                 int ac = default_queue_to_tx_fifo[i];
-                iwl4965_txq_ctx_activate(priv, i);
+                iwl_txq_ctx_activate(priv, i);
                 iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
         }
 
         iwl_release_nic_access(priv);
         spin_unlock_irqrestore(&priv->lock, flags);
 
-        /* Ask for statistics now, the uCode will send statistics notification
-         * periodically after association */
-        iwl_send_statistics_request(priv, CMD_ASYNC);
         return ret;
 }
 
@@ -1053,7 +962,6 @@ int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
 
         priv->hw_params.max_txq_num = priv->cfg->mod_params->num_of_queues;
         priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
-        priv->hw_params.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
         priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
         priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
         if (priv->cfg->mod_params->amsdu_size_8K)
@@ -1857,8 +1765,8 @@ static int iwl4965_send_rxon_assoc(struct iwl_priv *priv)
 {
         int ret = 0;
         struct iwl4965_rxon_assoc_cmd rxon_assoc;
-        const struct iwl4965_rxon_cmd *rxon1 = &priv->staging_rxon;
-        const struct iwl4965_rxon_cmd *rxon2 = &priv->active_rxon;
+        const struct iwl_rxon_cmd *rxon1 = &priv->staging_rxon;
+        const struct iwl_rxon_cmd *rxon2 = &priv->active_rxon;
 
         if ((rxon1->flags == rxon2->flags) &&
             (rxon1->filter_flags == rxon2->filter_flags) &&
@@ -1934,76 +1842,6 @@ int iwl4965_hw_channel_switch(struct iwl_priv *priv, u16 channel)
         return rc;
 }
 
-#define RTS_HCCA_RETRY_LIMIT            3
-#define RTS_DFAULT_RETRY_LIMIT          60
-
-void iwl4965_hw_build_tx_cmd_rate(struct iwl_priv *priv,
-                              struct iwl_cmd *cmd,
-                              struct ieee80211_tx_control *ctrl,
-                              struct ieee80211_hdr *hdr, int sta_id,
-                              int is_hcca)
-{
-        struct iwl4965_tx_cmd *tx = &cmd->cmd.tx;
-        u8 rts_retry_limit = 0;
-        u8 data_retry_limit = 0;
-        u16 fc = le16_to_cpu(hdr->frame_control);
-        u8 rate_plcp;
-        u16 rate_flags = 0;
-        int rate_idx = min(ctrl->tx_rate->hw_value & 0xffff, IWL_RATE_COUNT - 1);
-
-        rate_plcp = iwl4965_rates[rate_idx].plcp;
-
-        rts_retry_limit = (is_hcca) ?
-            RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
-
-        if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
-                rate_flags |= RATE_MCS_CCK_MSK;
-
-
-        if (ieee80211_is_probe_response(fc)) {
-                data_retry_limit = 3;
-                if (data_retry_limit < rts_retry_limit)
-                        rts_retry_limit = data_retry_limit;
-        } else
-                data_retry_limit = IWL_DEFAULT_TX_RETRY;
-
-        if (priv->data_retry_limit != -1)
-                data_retry_limit = priv->data_retry_limit;
-
-
-        if (ieee80211_is_data(fc)) {
-                tx->initial_rate_index = 0;
-                tx->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
-        } else {
-                switch (fc & IEEE80211_FCTL_STYPE) {
-                case IEEE80211_STYPE_AUTH:
-                case IEEE80211_STYPE_DEAUTH:
-                case IEEE80211_STYPE_ASSOC_REQ:
-                case IEEE80211_STYPE_REASSOC_REQ:
-                        if (tx->tx_flags & TX_CMD_FLG_RTS_MSK) {
-                                tx->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-                                tx->tx_flags |= TX_CMD_FLG_CTS_MSK;
-                        }
-                        break;
-                default:
-                        break;
-                }
-
-                /* Alternate between antenna A and B for successive frames */
-                if (priv->use_ant_b_for_management_frame) {
-                        priv->use_ant_b_for_management_frame = 0;
-                        rate_flags |= RATE_MCS_ANT_B_MSK;
-                } else {
-                        priv->use_ant_b_for_management_frame = 1;
-                        rate_flags |= RATE_MCS_ANT_A_MSK;
-                }
-        }
-
-        tx->rts_retry_limit = rts_retry_limit;
-        tx->data_retry_limit = data_retry_limit;
-        tx->rate_n_flags = iwl4965_hw_set_rate_n_flags(rate_plcp, rate_flags);
-}
-
 static int iwl4965_shared_mem_rx_idx(struct iwl_priv *priv)
 {
         struct iwl4965_shared *s = priv->shared_virt;
@@ -2016,7 +1854,7 @@ int iwl4965_hw_get_temperature(struct iwl_priv *priv)
 }
 
 unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
-                          struct iwl4965_frame *frame, u8 rate)
+                          struct iwl_frame *frame, u8 rate)
 {
         struct iwl4965_tx_beacon_cmd *tx_beacon_cmd;
         unsigned int frame_size;
@@ -2029,7 +1867,7 @@ unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
 
         frame_size = iwl4965_fill_beacon_frame(priv,
                                 tx_beacon_cmd->frame,
-                                iwl4965_broadcast_addr,
+                                iwl_bcast_addr,
                                 sizeof(frame->u) - sizeof(*tx_beacon_cmd));
 
         BUG_ON(frame_size > MAX_MPDU_SIZE);
@@ -2047,40 +1885,6 @@ unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
         return (sizeof(*tx_beacon_cmd) + frame_size);
 }
 
-int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *ptr,
-                                 dma_addr_t addr, u16 len)
-{
-        int index, is_odd;
-        struct iwl_tfd_frame *tfd = ptr;
-        u32 num_tbs = IWL_GET_BITS(*tfd, num_tbs);
-
-        /* Each TFD can point to a maximum 20 Tx buffers */
-        if ((num_tbs >= MAX_NUM_OF_TBS) || (num_tbs < 0)) {
-                IWL_ERROR("Error can not send more than %d chunks\n",
-                          MAX_NUM_OF_TBS);
-                return -EINVAL;
-        }
-
-        index = num_tbs / 2;
-        is_odd = num_tbs & 0x1;
-
-        if (!is_odd) {
-                tfd->pa[index].tb1_addr = cpu_to_le32(addr);
-                IWL_SET_BITS(tfd->pa[index], tb1_addr_hi,
-                             iwl_get_dma_hi_address(addr));
-                IWL_SET_BITS(tfd->pa[index], tb1_len, len);
-        } else {
-                IWL_SET_BITS(tfd->pa[index], tb2_addr_lo16,
-                             (u32) (addr & 0xffff));
-                IWL_SET_BITS(tfd->pa[index], tb2_addr_hi20, addr >> 16);
-                IWL_SET_BITS(tfd->pa[index], tb2_len, len);
-        }
-
-        IWL_SET_BITS(*tfd, num_tbs, num_tbs + 1);
-
-        return 0;
-}
-
 static int iwl4965_alloc_shared_mem(struct iwl_priv *priv)
 {
         priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
@@ -2436,7 +2240,7 @@ static void iwl4965_add_radiotap(struct iwl_priv *priv,
         if (rate == -1)
                 iwl4965_rt->rt_rate = 0;
         else
-                iwl4965_rt->rt_rate = iwl4965_rates[rate].ieee;
+                iwl4965_rt->rt_rate = iwl_rates[rate].ieee;
 
         /*
          * "antenna number"
@@ -2494,6 +2298,7 @@ static int iwl4965_set_decrypted_flag(struct iwl_priv *priv,
                     RX_RES_STATUS_BAD_KEY_TTAK)
                         break;
 
+        case RX_RES_STATUS_SEC_TYPE_WEP:
                 if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
                     RX_RES_STATUS_BAD_ICV_MIC) {
                         /* bad ICV, the packet is destroyed since the
@@ -2501,7 +2306,6 @@ static int iwl4965_set_decrypted_flag(struct iwl_priv *priv,
                         IWL_DEBUG_RX("Packet destroyed\n");
                         return -1;
                 }
-        case RX_RES_STATUS_SEC_TYPE_WEP:
         case RX_RES_STATUS_SEC_TYPE_CCMP:
                 if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
                     RX_RES_STATUS_DECRYPT_OK) {
@@ -2848,7 +2652,7 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
                 if (unlikely(rate_idx == -1))
                         bitrate = 0;
                 else
-                        bitrate = iwl4965_rates[rate_idx].ieee / 2;
+                        bitrate = iwl_rates[rate_idx].ieee / 2;
 
                 /* print frame summary.
                  * MAC addresses show just the last byte (for brevity),
@@ -2885,7 +2689,7 @@ static inline void iwl4965_dbg_report_frame(struct iwl_priv *priv,
 
 /* Called for REPLY_RX (legacy ABG frames), or
  * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
-static void iwl4965_rx_reply_rx(struct iwl_priv *priv,
+void iwl4965_rx_reply_rx(struct iwl_priv *priv,
                                 struct iwl_rx_mem_buffer *rxb)
 {
         struct ieee80211_hdr *header;
@@ -3059,57 +2863,9 @@ static void iwl4965_rx_reply_rx(struct iwl_priv *priv,
         }
 }
 
-/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
- * This will be used later in iwl4965_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
-static void iwl4965_rx_reply_rx_phy(struct iwl_priv *priv,
-                                    struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        priv->last_phy_res[0] = 1;
-        memcpy(&priv->last_phy_res[1], &(pkt->u.raw[0]),
-               sizeof(struct iwl4965_rx_phy_res));
-}
-static void iwl4965_rx_missed_beacon_notif(struct iwl_priv *priv,
-                                           struct iwl_rx_mem_buffer *rxb)
-
-{
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_missed_beacon_notif *missed_beacon;
-
-        missed_beacon = &pkt->u.missed_beacon;
-        if (le32_to_cpu(missed_beacon->consequtive_missed_beacons) > 5) {
-                IWL_DEBUG_CALIB("missed bcn cnsq %d totl %d rcd %d expctd %d\n",
-                    le32_to_cpu(missed_beacon->consequtive_missed_beacons),
-                    le32_to_cpu(missed_beacon->total_missed_becons),
-                    le32_to_cpu(missed_beacon->num_recvd_beacons),
-                    le32_to_cpu(missed_beacon->num_expected_beacons));
-                if (!test_bit(STATUS_SCANNING, &priv->status))
-                        iwl_init_sensitivity(priv);
-        }
-#endif /*CONFIG_IWL4965_RUN_TIME_CALIB*/
-}
 #ifdef CONFIG_IWL4965_HT
 
 /**
- * iwl4965_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
- */
-static void iwl4965_sta_modify_enable_tid_tx(struct iwl_priv *priv,
-                                         int sta_id, int tid)
-{
-        unsigned long flags;
-
-        /* Remove "disable" flag, to enable Tx for this TID */
-        spin_lock_irqsave(&priv->sta_lock, flags);
-        priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_TID_DISABLE_TX;
-        priv->stations[sta_id].sta.tid_disable_tx &= cpu_to_le16(~(1 << tid));
-        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
-        spin_unlock_irqrestore(&priv->sta_lock, flags);
-
-        iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
-}
-
-/**
  * iwl4965_tx_status_reply_compressed_ba - Update tx status from block-ack
  *
  * Go through block-ack's bitmap of ACK'd frames, update driver's record of
@@ -3126,7 +2882,7 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
         u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
         u64 bitmap;
         int successes = 0;
-        struct ieee80211_tx_status *tx_status;
+        struct ieee80211_tx_info *info;
 
         if (unlikely(!agg->wait_for_ba))  {
                 IWL_ERROR("Received BA when not expected\n");
@@ -3164,13 +2920,13 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
                         agg->start_idx + i);
         }
 
-        tx_status = &priv->txq[scd_flow].txb[agg->start_idx].status;
-        tx_status->flags = IEEE80211_TX_STATUS_ACK;
-        tx_status->flags |= IEEE80211_TX_STATUS_AMPDU;
-        tx_status->ampdu_ack_map = successes;
-        tx_status->ampdu_ack_len = agg->frame_count;
-        iwl4965_hwrate_to_tx_control(priv, agg->rate_n_flags,
-                                     &tx_status->control);
+        info = IEEE80211_SKB_CB(priv->txq[scd_flow].txb[agg->start_idx].skb[0]);
+        memset(&info->status, 0, sizeof(info->status));
+        info->flags = IEEE80211_TX_STAT_ACK;
+        info->flags |= IEEE80211_TX_STAT_AMPDU;
+        info->status.ampdu_ack_map = successes;
+        info->status.ampdu_ack_len = agg->frame_count;
+        iwl4965_hwrate_to_tx_control(priv, agg->rate_n_flags, info);
 
         IWL_DEBUG_TX_REPLY("Bitmap %llx\n", (unsigned long long)bitmap);
 
@@ -3195,8 +2951,8 @@ static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv,
  * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
  * priv->lock must be held by the caller
  */
-static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
-                                        u16 ssn_idx, u8 tx_fifo)
+static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
+                                   u16 ssn_idx, u8 tx_fifo)
 {
         int ret = 0;
 
@@ -3220,7 +2976,7 @@ static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
         iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
 
         iwl_clear_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
-        iwl4965_txq_ctx_deactivate(priv, txq_id);
+        iwl_txq_ctx_deactivate(priv, txq_id);
         iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
 
         iwl_release_nic_access(priv);
@@ -3228,49 +2984,6 @@ static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
         return 0;
 }
 
-int iwl4965_check_empty_hw_queue(struct iwl_priv *priv, int sta_id,
-                                         u8 tid, int txq_id)
-{
-        struct iwl4965_queue *q = &priv->txq[txq_id].q;
-        u8 *addr = priv->stations[sta_id].sta.sta.addr;
-        struct iwl_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
-
-        switch (priv->stations[sta_id].tid[tid].agg.state) {
-        case IWL_EMPTYING_HW_QUEUE_DELBA:
-                /* We are reclaiming the last packet of the */
-                /* aggregated HW queue */
-                if (txq_id  == tid_data->agg.txq_id &&
-                    q->read_ptr == q->write_ptr) {
-                        u16 ssn = SEQ_TO_SN(tid_data->seq_number);
-                        int tx_fifo = default_tid_to_tx_fifo[tid];
-                        IWL_DEBUG_HT("HW queue empty: continue DELBA flow\n");
-                        iwl4965_tx_queue_agg_disable(priv, txq_id,
-                                                     ssn, tx_fifo);
-                        tid_data->agg.state = IWL_AGG_OFF;
-                        ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, addr, tid);
-                }
-                break;
-        case IWL_EMPTYING_HW_QUEUE_ADDBA:
-                /* We are reclaiming the last packet of the queue */
-                if (tid_data->tfds_in_queue == 0) {
-                        IWL_DEBUG_HT("HW queue empty: continue ADDBA flow\n");
-                        tid_data->agg.state = IWL_AGG_ON;
-                        ieee80211_start_tx_ba_cb_irqsafe(priv->hw, addr, tid);
-                }
-                break;
-        }
-        return 0;
-}
-
-/**
- * iwl4965_queue_dec_wrap - Decrement queue index, wrap back to end if needed
- * @index -- current index
- * @n_bd -- total number of entries in queue (s/b power of 2)
- */
-static inline int iwl4965_queue_dec_wrap(int index, int n_bd)
-{
-        return (index == 0) ? n_bd - 1 : index - 1;
-}
 
 /**
  * iwl4965_rx_reply_compressed_ba - Handler for REPLY_COMPRESSED_BA
@@ -3304,7 +3017,7 @@ static void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
         agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
 
         /* Find index just before block-ack window */
-        index = iwl4965_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
+        index = iwl_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
 
         /* TODO: Need to get this copy more safely - now good for debug */
 
@@ -3334,15 +3047,16 @@ static void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
                 /* calculate mac80211 ampdu sw queue to wake */
                 int ampdu_q =
                    scd_flow - IWL_BACK_QUEUE_FIRST_ID + priv->hw->queues;
-                int freed = iwl4965_tx_queue_reclaim(priv, scd_flow, index);
+                int freed = iwl_tx_queue_reclaim(priv, scd_flow, index);
                 priv->stations[ba_resp->sta_id].
                         tid[ba_resp->tid].tfds_in_queue -= freed;
-                if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
+                if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
                         priv->mac80211_registered &&
                         agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)
                         ieee80211_wake_queue(priv->hw, ampdu_q);
-                iwl4965_check_empty_hw_queue(priv, ba_resp->sta_id,
-                        ba_resp->tid, scd_flow);
+
+                iwl_txq_check_empty(priv, ba_resp->sta_id,
+                                    ba_resp->tid, scd_flow);
         }
 }
 
@@ -3356,7 +3070,7 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
         u32 tbl_dw;
         u16 scd_q2ratid;
 
-        scd_q2ratid = ra_tid & IWL49_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+        scd_q2ratid = ra_tid & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
 
         tbl_dw_addr = priv->scd_base_addr +
                         IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
@@ -3380,12 +3094,11 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
  * NOTE:  txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID,
  *        i.e. it must be one of the higher queues used for aggregation
  */
-static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
-                                       int tx_fifo, int sta_id, int tid,
-                                       u16 ssn_idx)
+static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
+                                  int tx_fifo, int sta_id, int tid, u16 ssn_idx)
 {
         unsigned long flags;
-        int rc;
+        int ret;
         u16 ra_tid;
 
         if (IWL_BACK_QUEUE_FIRST_ID > txq_id)
@@ -3395,13 +3108,13 @@ static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
         ra_tid = BUILD_RAxTID(sta_id, tid);
 
         /* Modify device's station table to Tx this TID */
-        iwl4965_sta_modify_enable_tid_tx(priv, sta_id, tid);
+        iwl_sta_modify_enable_tid_tx(priv, sta_id, tid);
 
         spin_lock_irqsave(&priv->lock, flags);
-        rc = iwl_grab_nic_access(priv);
-        if (rc) {
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
                 spin_unlock_irqrestore(&priv->lock, flags);
-                return rc;
+                return ret;
         }
 
         /* Stop this Tx queue before configuring it */
@@ -3443,109 +3156,8 @@ static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
 
 #endif /* CONFIG_IWL4965_HT */
 
-/**
- * iwl4965_add_station - Initialize a station's hardware rate table
- *
- * The uCode's station table contains a table of fallback rates
- * for automatic fallback during transmission.
- *
- * NOTE: This sets up a default set of values.  These will be replaced later
- *       if the driver's iwl-4965-rs rate scaling algorithm is used, instead of
- *       rc80211_simple.
- *
- * NOTE: Run REPLY_ADD_STA command to set up station table entry, before
- *       calling this function (which runs REPLY_TX_LINK_QUALITY_CMD,
- *       which requires station table entry to exist).
- */
-void iwl4965_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
-{
-        int i, r;
-        struct iwl_link_quality_cmd link_cmd = {
-                .reserved1 = 0,
-        };
-        u16 rate_flags;
-
-        /* Set up the rate scaling to start at selected rate, fall back
-         * all the way down to 1M in IEEE order, and then spin on 1M */
-        if (is_ap)
-                r = IWL_RATE_54M_INDEX;
-        else if (priv->band == IEEE80211_BAND_5GHZ)
-                r = IWL_RATE_6M_INDEX;
-        else
-                r = IWL_RATE_1M_INDEX;
-
-        for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
-                rate_flags = 0;
-                if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
-                        rate_flags |= RATE_MCS_CCK_MSK;
-
-                /* Use Tx antenna B only */
-                rate_flags |= RATE_MCS_ANT_B_MSK; /*FIXME:RS*/
-
-                link_cmd.rs_table[i].rate_n_flags =
-                        iwl4965_hw_set_rate_n_flags(iwl4965_rates[r].plcp, rate_flags);
-                r = iwl4965_get_prev_ieee_rate(r);
-        }
-
-        link_cmd.general_params.single_stream_ant_msk = 2;
-        link_cmd.general_params.dual_stream_ant_msk = 3;
-        link_cmd.agg_params.agg_dis_start_th = 3;
-        link_cmd.agg_params.agg_time_limit = cpu_to_le16(4000);
-
-        /* Update the rate scaling for control frame Tx to AP */
-        link_cmd.sta_id = is_ap ? IWL_AP_ID : priv->hw_params.bcast_sta_id;
-
-        iwl_send_cmd_pdu_async(priv, REPLY_TX_LINK_QUALITY_CMD,
-                               sizeof(link_cmd), &link_cmd, NULL);
-}
 
 #ifdef CONFIG_IWL4965_HT
-
-void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index,
-                                struct ieee80211_ht_info *sta_ht_inf)
-{
-        __le32 sta_flags;
-        u8 mimo_ps_mode;
-
-        if (!sta_ht_inf || !sta_ht_inf->ht_supported)
-                goto done;
-
-        mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2;
-
-        sta_flags = priv->stations[index].sta.station_flags;
-
-        sta_flags &= ~(STA_FLG_RTS_MIMO_PROT_MSK | STA_FLG_MIMO_DIS_MSK);
-
-        switch (mimo_ps_mode) {
-        case WLAN_HT_CAP_MIMO_PS_STATIC:
-                sta_flags |= STA_FLG_MIMO_DIS_MSK;
-                break;
-        case WLAN_HT_CAP_MIMO_PS_DYNAMIC:
-                sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
-                break;
-        case WLAN_HT_CAP_MIMO_PS_DISABLED:
-                break;
-        default:
-                IWL_WARNING("Invalid MIMO PS mode %d", mimo_ps_mode);
-                break;
-        }
-
-        sta_flags |= cpu_to_le32(
-              (u32)sta_ht_inf->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
-
-        sta_flags |= cpu_to_le32(
-              (u32)sta_ht_inf->ampdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
-
-        if (iwl_is_fat_tx_allowed(priv, sta_ht_inf))
-                sta_flags |= STA_FLG_FAT_EN_MSK;
-        else
-                sta_flags &= ~STA_FLG_FAT_EN_MSK;
-
-        priv->stations[index].sta.station_flags = sta_flags;
- done:
-        return;
-}
-
 static int iwl4965_rx_agg_start(struct iwl_priv *priv,
                                 const u8 *addr, int tid, u16 ssn)
 {
@@ -3589,137 +3201,6 @@ static int iwl4965_rx_agg_stop(struct iwl_priv *priv,
                                         CMD_ASYNC);
 }
 
-/*
- * Find first available (lowest unused) Tx Queue, mark it "active".
- * Called only when finding queue for aggregation.
- * Should never return anything < 7, because they should already
- * be in use as EDCA AC (0-3), Command (4), HCCA (5, 6).
- */
-static int iwl4965_txq_ctx_activate_free(struct iwl_priv *priv)
-{
-        int txq_id;
-
-        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
-                if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
-                        return txq_id;
-        return -1;
-}
-
-static int iwl4965_tx_agg_start(struct ieee80211_hw *hw, const u8 *ra,
-                                u16 tid, u16 *start_seq_num)
-{
-        struct iwl_priv *priv = hw->priv;
-        int sta_id;
-        int tx_fifo;
-        int txq_id;
-        int ssn = -1;
-        int ret = 0;
-        unsigned long flags;
-        struct iwl_tid_data *tid_data;
-        DECLARE_MAC_BUF(mac);
-
-        if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
-                tx_fifo = default_tid_to_tx_fifo[tid];
-        else
-                return -EINVAL;
-
-        IWL_WARNING("%s on ra = %s tid = %d\n",
-                        __func__, print_mac(mac, ra), tid);
-
-        sta_id = iwl_find_station(priv, ra);
-        if (sta_id == IWL_INVALID_STATION)
-                return -ENXIO;
-
-        if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
-                IWL_ERROR("Start AGG when state is not IWL_AGG_OFF !\n");
-                return -ENXIO;
-        }
-
-        txq_id = iwl4965_txq_ctx_activate_free(priv);
-        if (txq_id == -1)
-                return -ENXIO;
-
-        spin_lock_irqsave(&priv->sta_lock, flags);
-        tid_data = &priv->stations[sta_id].tid[tid];
-        ssn = SEQ_TO_SN(tid_data->seq_number);
-        tid_data->agg.txq_id = txq_id;
-        spin_unlock_irqrestore(&priv->sta_lock, flags);
-
-        *start_seq_num = ssn;
-        ret = iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
-                                          sta_id, tid, ssn);
-        if (ret)
-                return ret;
-
-        ret = 0;
-        if (tid_data->tfds_in_queue == 0) {
-                printk(KERN_ERR "HW queue is empty\n");
-                tid_data->agg.state = IWL_AGG_ON;
-                ieee80211_start_tx_ba_cb_irqsafe(hw, ra, tid);
-        } else {
-                IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
-                                tid_data->tfds_in_queue);
-                tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
-        }
-        return ret;
-}
-
-static int iwl4965_tx_agg_stop(struct ieee80211_hw *hw, const u8 *ra, u16 tid)
-{
-        struct iwl_priv *priv = hw->priv;
-        int tx_fifo_id, txq_id, sta_id, ssn = -1;
-        struct iwl_tid_data *tid_data;
-        int ret, write_ptr, read_ptr;
-        unsigned long flags;
-        DECLARE_MAC_BUF(mac);
-
-        if (!ra) {
-                IWL_ERROR("ra = NULL\n");
-                return -EINVAL;
-        }
-
-        if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
-                tx_fifo_id = default_tid_to_tx_fifo[tid];
-        else
-                return -EINVAL;
-
-        sta_id = iwl_find_station(priv, ra);
-
-        if (sta_id == IWL_INVALID_STATION)
-                return -ENXIO;
-
-        if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_ON)
-                IWL_WARNING("Stopping AGG while state not IWL_AGG_ON\n");
-
-        tid_data = &priv->stations[sta_id].tid[tid];
-        ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
-        txq_id = tid_data->agg.txq_id;
-        write_ptr = priv->txq[txq_id].q.write_ptr;
-        read_ptr = priv->txq[txq_id].q.read_ptr;
-
-        /* The queue is not empty */
-        if (write_ptr != read_ptr) {
-                IWL_DEBUG_HT("Stopping a non empty AGG HW QUEUE\n");
-                priv->stations[sta_id].tid[tid].agg.state =
-                                IWL_EMPTYING_HW_QUEUE_DELBA;
-                return 0;
-        }
-
-        IWL_DEBUG_HT("HW queue is empty\n");
-        priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
-
-        spin_lock_irqsave(&priv->lock, flags);
-        ret = iwl4965_tx_queue_agg_disable(priv, txq_id, ssn, tx_fifo_id);
-        spin_unlock_irqrestore(&priv->lock, flags);
-
-        if (ret)
-                return ret;
-
-        ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, ra, tid);
-
-        return 0;
-}
-
 int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
                              enum ieee80211_ampdu_mlme_action action,
                              const u8 *addr, u16 tid, u16 *ssn)
@@ -3739,10 +3220,10 @@ int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
                 return iwl4965_rx_agg_stop(priv, addr, tid);
         case IEEE80211_AMPDU_TX_START:
                 IWL_DEBUG_HT("start Tx\n");
-                return iwl4965_tx_agg_start(hw, addr, tid, ssn);
+                return iwl_tx_agg_start(priv, addr, tid, ssn);
         case IEEE80211_AMPDU_TX_STOP:
                 IWL_DEBUG_HT("stop Tx\n");
-                return iwl4965_tx_agg_stop(hw, addr, tid);
+                return iwl_tx_agg_stop(priv, addr, tid);
         default:
                 IWL_DEBUG_HT("unknown\n");
                 return -EINVAL;
@@ -3753,6 +3234,16 @@ int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
 #endif /* CONFIG_IWL4965_HT */
 
 
+static u16 iwl4965_get_hcmd_size(u8 cmd_id, u16 len)
+{
+        switch (cmd_id) {
+        case REPLY_RXON:
+                return (u16) sizeof(struct iwl4965_rxon_cmd);
+        default:
+                return len;
+        }
+}
+
 static u16 iwl4965_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 {
         struct iwl4965_addsta_cmd *addsta = (struct iwl4965_addsta_cmd *)data;
@@ -3770,18 +3261,258 @@ static u16 iwl4965_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 
         return (u16)sizeof(struct iwl4965_addsta_cmd);
 }
+
+#ifdef CONFIG_IWL4965_HT
+static inline u32 iwl4965_get_scd_ssn(struct iwl4965_tx_resp *tx_resp)
+{
+        __le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
+                                tx_resp->frame_count);
+        return le32_to_cpu(*scd_ssn) & MAX_SN;
+
+}
+
+/**
+ * iwl4965_tx_status_reply_tx - Handle Tx rspnse for frames in aggregation queue
+ */
+static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
+                                      struct iwl_ht_agg *agg,
+                                      struct iwl4965_tx_resp_agg *tx_resp,
+                                      u16 start_idx)
+{
+        u16 status;
+        struct agg_tx_status *frame_status = &tx_resp->status;
+        struct ieee80211_tx_info *info = NULL;
+        struct ieee80211_hdr *hdr = NULL;
+        int i, sh;
+        int txq_id, idx;
+        u16 seq;
+
+        if (agg->wait_for_ba)
+                IWL_DEBUG_TX_REPLY("got tx response w/o block-ack\n");
+
+        agg->frame_count = tx_resp->frame_count;
+        agg->start_idx = start_idx;
+        agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+        agg->bitmap = 0;
+
+        /* # frames attempted by Tx command */
+        if (agg->frame_count == 1) {
+                /* Only one frame was attempted; no block-ack will arrive */
+                status = le16_to_cpu(frame_status[0].status);
+                seq  = le16_to_cpu(frame_status[0].sequence);
+                idx = SEQ_TO_INDEX(seq);
+                txq_id = SEQ_TO_QUEUE(seq);
+
+                /* FIXME: code repetition */
+                IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
+                                   agg->frame_count, agg->start_idx, idx);
+
+                info = IEEE80211_SKB_CB(priv->txq[txq_id].txb[idx].skb[0]);
+                info->status.retry_count = tx_resp->failure_frame;
+                info->flags &= ~IEEE80211_TX_CTL_AMPDU;
+                info->flags |= iwl_is_tx_success(status)?
+                        IEEE80211_TX_STAT_ACK : 0;
+                iwl4965_hwrate_to_tx_control(priv,
+                                             le32_to_cpu(tx_resp->rate_n_flags),
+                                             info);
+                /* FIXME: code repetition end */
+
+                IWL_DEBUG_TX_REPLY("1 Frame 0x%x failure :%d\n",
+                                    status & 0xff, tx_resp->failure_frame);
+                IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n",
+                        iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
+
+                agg->wait_for_ba = 0;
+        } else {
+                /* Two or more frames were attempted; expect block-ack */
+                u64 bitmap = 0;
+                int start = agg->start_idx;
+
+                /* Construct bit-map of pending frames within Tx window */
+                for (i = 0; i < agg->frame_count; i++) {
+                        u16 sc;
+                        status = le16_to_cpu(frame_status[i].status);
+                        seq  = le16_to_cpu(frame_status[i].sequence);
+                        idx = SEQ_TO_INDEX(seq);
+                        txq_id = SEQ_TO_QUEUE(seq);
+
+                        if (status & (AGG_TX_STATE_FEW_BYTES_MSK |
+                                      AGG_TX_STATE_ABORT_MSK))
+                                continue;
+
+                        IWL_DEBUG_TX_REPLY("FrameCnt = %d, txq_id=%d idx=%d\n",
+                                           agg->frame_count, txq_id, idx);
+
+                        hdr = iwl_tx_queue_get_hdr(priv, txq_id, idx);
+
+                        sc = le16_to_cpu(hdr->seq_ctrl);
+                        if (idx != (SEQ_TO_SN(sc) & 0xff)) {
+                                IWL_ERROR("BUG_ON idx doesn't match seq control"
+                                          " idx=%d, seq_idx=%d, seq=%d\n",
+                                          idx, SEQ_TO_SN(sc),
+                                          hdr->seq_ctrl);
+                                return -1;
+                        }
+
+                        IWL_DEBUG_TX_REPLY("AGG Frame i=%d idx %d seq=%d\n",
+                                           i, idx, SEQ_TO_SN(sc));
+
+                        sh = idx - start;
+                        if (sh > 64) {
+                                sh = (start - idx) + 0xff;
+                                bitmap = bitmap << sh;
+                                sh = 0;
+                                start = idx;
+                        } else if (sh < -64)
+                                sh  = 0xff - (start - idx);
+                        else if (sh < 0) {
+                                sh = start - idx;
+                                start = idx;
+                                bitmap = bitmap << sh;
+                                sh = 0;
+                        }
+                        bitmap |= (1 << sh);
+                        IWL_DEBUG_TX_REPLY("start=%d bitmap=0x%x\n",
+                                           start, (u32)(bitmap & 0xFFFFFFFF));
+                }
+
+                agg->bitmap = bitmap;
+                agg->start_idx = start;
+                agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+                IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%llx\n",
+                                   agg->frame_count, agg->start_idx,
+                                   (unsigned long long)agg->bitmap);
+
+                if (bitmap)
+                        agg->wait_for_ba = 1;
+        }
+        return 0;
+}
+#endif
+
+/**
+ * iwl4965_rx_reply_tx - Handle standard (non-aggregation) Tx response
+ */
+static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
+                                struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+        int txq_id = SEQ_TO_QUEUE(sequence);
+        int index = SEQ_TO_INDEX(sequence);
+        struct iwl_tx_queue *txq = &priv->txq[txq_id];
+        struct ieee80211_tx_info *info;
+        struct iwl4965_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+        u32  status = le32_to_cpu(tx_resp->status);
+#ifdef CONFIG_IWL4965_HT
+        int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
+        u16 fc;
+        struct ieee80211_hdr *hdr;
+        u8 *qc = NULL;
+#endif
+
+        if ((index >= txq->q.n_bd) || (iwl_queue_used(&txq->q, index) == 0)) {
+                IWL_ERROR("Read index for DMA queue txq_id (%d) index %d "
+                          "is out of range [0-%d] %d %d\n", txq_id,
+                          index, txq->q.n_bd, txq->q.write_ptr,
+                          txq->q.read_ptr);
+                return;
+        }
+
+        info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+        memset(&info->status, 0, sizeof(info->status));
+
+#ifdef CONFIG_IWL4965_HT
+        hdr = iwl_tx_queue_get_hdr(priv, txq_id, index);
+        fc = le16_to_cpu(hdr->frame_control);
+        if (ieee80211_is_qos_data(fc)) {
+                qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                tid = qc[0] & 0xf;
+        }
+
+        sta_id = iwl_get_ra_sta_id(priv, hdr);
+        if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
+                IWL_ERROR("Station not known\n");
+                return;
+        }
+
+        if (txq->sched_retry) {
+                const u32 scd_ssn = iwl4965_get_scd_ssn(tx_resp);
+                struct iwl_ht_agg *agg = NULL;
+
+                if (!qc)
+                        return;
+
+                agg = &priv->stations[sta_id].tid[tid].agg;
+
+                iwl4965_tx_status_reply_tx(priv, agg,
+                                (struct iwl4965_tx_resp_agg *)tx_resp, index);
+
+                if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status)) {
+                        /* TODO: send BAR */
+                }
+
+                if (txq->q.read_ptr != (scd_ssn & 0xff)) {
+                        int freed, ampdu_q;
+                        index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
+                        IWL_DEBUG_TX_REPLY("Retry scheduler reclaim scd_ssn "
+                                           "%d index %d\n", scd_ssn , index);
+                        freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+
+                        if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+                            txq_id >= 0 && priv->mac80211_registered &&
+                            agg->state != IWL_EMPTYING_HW_QUEUE_DELBA) {
+                                /* calculate mac80211 ampdu sw queue to wake */
+                                ampdu_q = txq_id - IWL_BACK_QUEUE_FIRST_ID +
+                                          priv->hw->queues;
+                                if (agg->state == IWL_AGG_OFF)
+                                        ieee80211_wake_queue(priv->hw, txq_id);
+                                else
+                                        ieee80211_wake_queue(priv->hw, ampdu_q);
+                        }
+                        iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+                }
+        } else {
+#endif /* CONFIG_IWL4965_HT */
+
+        info->status.retry_count = tx_resp->failure_frame;
+        info->flags |= iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+        iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
+                                     info);
+
+        IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
+                     "retries %d\n", txq_id, iwl_get_tx_fail_reason(status),
+                     status, le32_to_cpu(tx_resp->rate_n_flags),
+                     tx_resp->failure_frame);
+
+        IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
+#ifdef CONFIG_IWL4965_HT
+        if (index != -1) {
+                int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                if (tid != MAX_TID_COUNT)
+                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+                if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+                        (txq_id >= 0) && priv->mac80211_registered)
+                        ieee80211_wake_queue(priv->hw, txq_id);
+                if (tid != MAX_TID_COUNT)
+                        iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+        }
+        }
+#endif /* CONFIG_IWL4965_HT */
+
+        if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+                IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
+}
+
+
 /* Set up 4965-specific Rx frame reply handlers */
 static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
 {
         /* Legacy Rx frames */
         priv->rx_handlers[REPLY_RX] = iwl4965_rx_reply_rx;
-
-        /* High-throughput (HT) Rx frames */
-        priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl4965_rx_reply_rx_phy;
-        priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl4965_rx_reply_rx;
-
-        priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
-            iwl4965_rx_missed_beacon_notif;
+        /* Tx response */
+        priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
 
 #ifdef CONFIG_IWL4965_HT
         priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
@@ -3812,7 +3543,7 @@ static struct iwl_hcmd_ops iwl4965_hcmd = {
 };
 
 static struct iwl_hcmd_utils_ops iwl4965_hcmd_utils = {
-        .enqueue_hcmd = iwl4965_enqueue_hcmd,
+        .get_hcmd_size = iwl4965_get_hcmd_size,
         .build_addsta_hcmd = iwl4965_build_addsta_hcmd,
 #ifdef CONFIG_IWL4965_RUN_TIME_CALIB
         .chain_noise_reset = iwl4965_chain_noise_reset,
@@ -3826,7 +3557,11 @@ static struct iwl_lib_ops iwl4965_lib = {
         .free_shared_mem = iwl4965_free_shared_mem,
         .shared_mem_rx_idx = iwl4965_shared_mem_rx_idx,
         .txq_update_byte_cnt_tbl = iwl4965_txq_update_byte_cnt_tbl,
-        .disable_tx_fifo = iwl4965_disable_tx_fifo,
+        .txq_set_sched = iwl4965_txq_set_sched,
+#ifdef CONFIG_IWL4965_HT
+        .txq_agg_enable = iwl4965_txq_agg_enable,
+        .txq_agg_disable = iwl4965_txq_agg_disable,
+#endif
         .rx_handler_setup = iwl4965_rx_handler_setup,
         .is_valid_rtc_data_addr = iwl4965_hw_valid_rtc_data_addr,
         .alive_notify = iwl4965_alive_notify,
@@ -3834,6 +3569,8 @@ static struct iwl_lib_ops iwl4965_lib = {
         .load_ucode = iwl4965_load_bsm,
         .apm_ops = {
                 .init = iwl4965_apm_init,
+                .reset = iwl4965_apm_reset,
+                .stop = iwl4965_apm_stop,
                 .config = iwl4965_nic_config,
                 .set_pwr_src = iwl4965_set_pwr_src,
         },
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index b5e28b811796..7e525ad45135 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -46,6 +46,41 @@
 
 #define IWL5000_UCODE_API  "-1"
 
+static const u16 iwl5000_default_queue_to_tx_fifo[] = {
+        IWL_TX_FIFO_AC3,
+        IWL_TX_FIFO_AC2,
+        IWL_TX_FIFO_AC1,
+        IWL_TX_FIFO_AC0,
+        IWL50_CMD_FIFO_NUM,
+        IWL_TX_FIFO_HCCA_1,
+        IWL_TX_FIFO_HCCA_2
+};
+
+/* FIXME: same implementation as 4965 */
+static int iwl5000_apm_stop_master(struct iwl_priv *priv)
+{
+        int ret = 0;
+        unsigned long flags;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        /* set stop master bit */
+        iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
+
+        ret = iwl_poll_bit(priv, CSR_RESET,
+                                  CSR_RESET_REG_FLAG_MASTER_DISABLED,
+                                  CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+        if (ret < 0)
+                goto out;
+
+out:
+        spin_unlock_irqrestore(&priv->lock, flags);
+        IWL_DEBUG_INFO("stop master\n");
+
+        return ret;
+}
+
+
 static int iwl5000_apm_init(struct iwl_priv *priv)
 {
         int ret = 0;
@@ -53,6 +88,10 @@ static int iwl5000_apm_init(struct iwl_priv *priv)
         iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
                     CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
 
+        /* disable L0s without affecting L1 :don't wait for ICH L0s bug W/A) */
+        iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+                    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+
         iwl_set_bit(priv, CSR_ANA_PLL_CFG, CSR50_ANA_PLL_CFG_VAL);
 
         /* set "initialization complete" bit to move adapter
@@ -73,19 +112,91 @@ static int iwl5000_apm_init(struct iwl_priv *priv)
                 return ret;
 
         /* enable DMA */
-        iwl_write_prph(priv, APMG_CLK_EN_REG,
-                        APMG_CLK_VAL_DMA_CLK_RQT);
+        iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT);
 
         udelay(20);
 
+        /* disable L1-Active */
         iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
-                        APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+                          APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
         iwl_release_nic_access(priv);
 
         return ret;
 }
 
+/* FIXME: this is indentical to 4965 */
+static void iwl5000_apm_stop(struct iwl_priv *priv)
+{
+        unsigned long flags;
+
+        iwl5000_apm_stop_master(priv);
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+        udelay(10);
+
+        iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+
+static int iwl5000_apm_reset(struct iwl_priv *priv)
+{
+        int ret = 0;
+        unsigned long flags;
+
+        iwl5000_apm_stop_master(priv);
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+        udelay(10);
+
+
+        /* FIXME: put here L1A -L0S w/a */
+
+        iwl_set_bit(priv, CSR_ANA_PLL_CFG, CSR50_ANA_PLL_CFG_VAL);
+
+        /* set "initialization complete" bit to move adapter
+         * D0U* --> D0A* state */
+        iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+        /* wait for clock stabilization */
+        ret = iwl_poll_bit(priv, CSR_GP_CNTRL,
+                          CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+                          CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+        if (ret < 0) {
+                IWL_DEBUG_INFO("Failed to init the card\n");
+                goto out;
+        }
+
+        ret = iwl_grab_nic_access(priv);
+        if (ret)
+                goto out;
+
+        /* enable DMA */
+        iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT);
+
+        udelay(20);
+
+        /* disable L1-Active */
+        iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+                          APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+        iwl_release_nic_access(priv);
+
+out:
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        return ret;
+}
+
+
 static void iwl5000_nic_config(struct iwl_priv *priv)
 {
         unsigned long flags;
@@ -96,8 +207,13 @@ static void iwl5000_nic_config(struct iwl_priv *priv)
 
         pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
 
-        /* disable L1 entry -- workaround for pre-B1 */
-        pci_write_config_byte(priv->pci_dev, PCI_LINK_CTRL, val_link & ~0x02);
+        /* L1 is enabled by BIOS */
+        if ((val_link & PCI_LINK_VAL_L1_EN) == PCI_LINK_VAL_L1_EN)
+                /* diable L0S disabled L1A enabled */
+                iwl_set_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
+        else
+                /* L0S enabled L1A disabled */
+                iwl_clear_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
 
         radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
@@ -279,6 +395,8 @@ static struct iwl_sensitivity_ranges iwl5000_sensitivity = {
 
 #endif /* CONFIG_IWL5000_RUN_TIME_CALIB */
 
+
+
 static const u8 *iwl5000_eeprom_query_addr(const struct iwl_priv *priv,
                                            size_t offset)
 {
@@ -287,6 +405,423 @@ static const u8 *iwl5000_eeprom_query_addr(const struct iwl_priv *priv,
         return &priv->eeprom[address];
 }
 
+/*
+ *  Calibration
+ */
+static int iwl5000_send_Xtal_calib(struct iwl_priv *priv)
+{
+        u16 *xtal_calib = (u16 *)iwl_eeprom_query_addr(priv, EEPROM_5000_XTAL);
+
+        struct iwl5000_calibration cal_cmd = {
+                .op_code = IWL5000_PHY_CALIBRATE_CRYSTAL_FRQ_CMD,
+                .data = {
+                        (u8)xtal_calib[0],
+                        (u8)xtal_calib[1],
+                }
+        };
+
+        return iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+                                sizeof(cal_cmd), &cal_cmd);
+}
+
+static int iwl5000_send_calib_results(struct iwl_priv *priv)
+{
+        int ret = 0;
+
+        struct iwl_host_cmd hcmd = {
+                .id = REPLY_PHY_CALIBRATION_CMD,
+                .meta.flags = CMD_SIZE_HUGE,
+        };
+
+        if (priv->calib_results.lo_res) {
+                hcmd.len = priv->calib_results.lo_res_len;
+                hcmd.data = priv->calib_results.lo_res;
+                ret = iwl_send_cmd_sync(priv, &hcmd);
+
+                if (ret)
+                        goto err;
+        }
+
+        if (priv->calib_results.tx_iq_res) {
+                hcmd.len = priv->calib_results.tx_iq_res_len;
+                hcmd.data = priv->calib_results.tx_iq_res;
+                ret = iwl_send_cmd_sync(priv, &hcmd);
+
+                if (ret)
+                        goto err;
+        }
+
+        if (priv->calib_results.tx_iq_perd_res) {
+                hcmd.len = priv->calib_results.tx_iq_perd_res_len;
+                hcmd.data = priv->calib_results.tx_iq_perd_res;
+                ret = iwl_send_cmd_sync(priv, &hcmd);
+
+                if (ret)
+                        goto err;
+        }
+
+        return 0;
+err:
+        IWL_ERROR("Error %d\n", ret);
+        return ret;
+}
+
+static int iwl5000_send_calib_cfg(struct iwl_priv *priv)
+{
+        struct iwl5000_calib_cfg_cmd calib_cfg_cmd;
+        struct iwl_host_cmd cmd = {
+                .id = CALIBRATION_CFG_CMD,
+                .len = sizeof(struct iwl5000_calib_cfg_cmd),
+                .data = &calib_cfg_cmd,
+        };
+
+        memset(&calib_cfg_cmd, 0, sizeof(calib_cfg_cmd));
+        calib_cfg_cmd.ucd_calib_cfg.once.is_enable = IWL_CALIB_INIT_CFG_ALL;
+        calib_cfg_cmd.ucd_calib_cfg.once.start = IWL_CALIB_INIT_CFG_ALL;
+        calib_cfg_cmd.ucd_calib_cfg.once.send_res = IWL_CALIB_INIT_CFG_ALL;
+        calib_cfg_cmd.ucd_calib_cfg.flags = IWL_CALIB_INIT_CFG_ALL;
+
+        return iwl_send_cmd(priv, &cmd);
+}
+
+static void iwl5000_rx_calib_result(struct iwl_priv *priv,
+                             struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+        struct iwl5000_calib_hdr *hdr = (struct iwl5000_calib_hdr *)pkt->u.raw;
+        int len = le32_to_cpu(pkt->len) & FH_RSCSR_FRAME_SIZE_MSK;
+
+        iwl_free_calib_results(priv);
+
+        /* reduce the size of the length field itself */
+        len -= 4;
+
+        switch (hdr->op_code) {
+        case IWL5000_PHY_CALIBRATE_LO_CMD:
+                priv->calib_results.lo_res = kzalloc(len, GFP_ATOMIC);
+                priv->calib_results.lo_res_len = len;
+                memcpy(priv->calib_results.lo_res, pkt->u.raw, len);
+                break;
+        case IWL5000_PHY_CALIBRATE_TX_IQ_CMD:
+                priv->calib_results.tx_iq_res = kzalloc(len, GFP_ATOMIC);
+                priv->calib_results.tx_iq_res_len = len;
+                memcpy(priv->calib_results.tx_iq_res, pkt->u.raw, len);
+                break;
+        case IWL5000_PHY_CALIBRATE_TX_IQ_PERD_CMD:
+                priv->calib_results.tx_iq_perd_res = kzalloc(len, GFP_ATOMIC);
+                priv->calib_results.tx_iq_perd_res_len = len;
+                memcpy(priv->calib_results.tx_iq_perd_res, pkt->u.raw, len);
+                break;
+        default:
+                IWL_ERROR("Unknown calibration notification %d\n",
+                          hdr->op_code);
+                return;
+        }
+}
+
+static void iwl5000_rx_calib_complete(struct iwl_priv *priv,
+                               struct iwl_rx_mem_buffer *rxb)
+{
+        IWL_DEBUG_INFO("Init. calibration is completed, restarting fw.\n");
+        queue_work(priv->workqueue, &priv->restart);
+}
+
+/*
+ * ucode
+ */
+static int iwl5000_load_section(struct iwl_priv *priv,
+                                struct fw_desc *image,
+                                u32 dst_addr)
+{
+        int ret = 0;
+        unsigned long flags;
+
+        dma_addr_t phy_addr = image->p_addr;
+        u32 byte_cnt = image->len;
+
+        spin_lock_irqsave(&priv->lock, flags);
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                return ret;
+        }
+
+        iwl_write_direct32(priv,
+                FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+                FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE);
+
+        iwl_write_direct32(priv,
+                FH_SRVC_CHNL_SRAM_ADDR_REG(FH_SRVC_CHNL), dst_addr);
+
+        iwl_write_direct32(priv,
+                FH_TFDIB_CTRL0_REG(FH_SRVC_CHNL),
+                phy_addr & FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK);
+
+        /* FIME: write the MSB of the phy_addr in CTRL1
+         * iwl_write_direct32(priv,
+                IWL_FH_TFDIB_CTRL1_REG(IWL_FH_SRVC_CHNL),
+                ((phy_addr & MSB_MSK)
+                        << FH_MEM_TFDIB_REG1_ADDR_BITSHIFT) | byte_count);
+         */
+        iwl_write_direct32(priv,
+                FH_TFDIB_CTRL1_REG(FH_SRVC_CHNL), byte_cnt);
+        iwl_write_direct32(priv,
+                FH_TCSR_CHNL_TX_BUF_STS_REG(FH_SRVC_CHNL),
+                1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM |
+                1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX |
+                FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID);
+
+        iwl_write_direct32(priv,
+                FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+                FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE       |
+                FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE_VAL |
+                FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD);
+
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+        return 0;
+}
+
+static int iwl5000_load_given_ucode(struct iwl_priv *priv,
+                struct fw_desc *inst_image,
+                struct fw_desc *data_image)
+{
+        int ret = 0;
+
+        ret = iwl5000_load_section(
+                priv, inst_image, RTC_INST_LOWER_BOUND);
+        if (ret)
+                return ret;
+
+        IWL_DEBUG_INFO("INST uCode section being loaded...\n");
+        ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+                                priv->ucode_write_complete, 5 * HZ);
+        if (ret == -ERESTARTSYS) {
+                IWL_ERROR("Could not load the INST uCode section due "
+                        "to interrupt\n");
+                return ret;
+        }
+        if (!ret) {
+                IWL_ERROR("Could not load the INST uCode section\n");
+                return -ETIMEDOUT;
+        }
+
+        priv->ucode_write_complete = 0;
+
+        ret = iwl5000_load_section(
+                priv, data_image, RTC_DATA_LOWER_BOUND);
+        if (ret)
+                return ret;
+
+        IWL_DEBUG_INFO("DATA uCode section being loaded...\n");
+
+        ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+                                priv->ucode_write_complete, 5 * HZ);
+        if (ret == -ERESTARTSYS) {
+                IWL_ERROR("Could not load the INST uCode section due "
+                        "to interrupt\n");
+                return ret;
+        } else if (!ret) {
+                IWL_ERROR("Could not load the DATA uCode section\n");
+                return -ETIMEDOUT;
+        } else
+                ret = 0;
+
+        priv->ucode_write_complete = 0;
+
+        return ret;
+}
+
+static int iwl5000_load_ucode(struct iwl_priv *priv)
+{
+        int ret = 0;
+
+        /* check whether init ucode should be loaded, or rather runtime ucode */
+        if (priv->ucode_init.len && (priv->ucode_type == UCODE_NONE)) {
+                IWL_DEBUG_INFO("Init ucode found. Loading init ucode...\n");
+                ret = iwl5000_load_given_ucode(priv,
+                        &priv->ucode_init, &priv->ucode_init_data);
+                if (!ret) {
+                        IWL_DEBUG_INFO("Init ucode load complete.\n");
+                        priv->ucode_type = UCODE_INIT;
+                }
+        } else {
+                IWL_DEBUG_INFO("Init ucode not found, or already loaded. "
+                        "Loading runtime ucode...\n");
+                ret = iwl5000_load_given_ucode(priv,
+                        &priv->ucode_code, &priv->ucode_data);
+                if (!ret) {
+                        IWL_DEBUG_INFO("Runtime ucode load complete.\n");
+                        priv->ucode_type = UCODE_RT;
+                }
+        }
+
+        return ret;
+}
+
+static void iwl5000_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("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.  */
+        if (iwl_verify_ucode(priv)) {
+                /* Runtime instruction load was bad;
+                 * take it all the way back down so we can try again */
+                IWL_DEBUG_INFO("Bad \"initialize\" uCode load.\n");
+                goto restart;
+        }
+
+        iwlcore_clear_stations_table(priv);
+        ret = priv->cfg->ops->lib->alive_notify(priv);
+        if (ret) {
+                IWL_WARNING("Could not complete ALIVE transition: %d\n", ret);
+                goto restart;
+        }
+
+        iwl5000_send_calib_cfg(priv);
+        return;
+
+restart:
+        /* real restart (first load init_ucode) */
+        queue_work(priv->workqueue, &priv->restart);
+}
+
+static void iwl5000_set_wr_ptrs(struct iwl_priv *priv,
+                                int txq_id, u32 index)
+{
+        iwl_write_direct32(priv, HBUS_TARG_WRPTR,
+                        (index & 0xff) | (txq_id << 8));
+        iwl_write_prph(priv, IWL50_SCD_QUEUE_RDPTR(txq_id), index);
+}
+
+static void iwl5000_tx_queue_set_status(struct iwl_priv *priv,
+                                        struct iwl_tx_queue *txq,
+                                        int tx_fifo_id, int scd_retry)
+{
+        int txq_id = txq->q.id;
+        int active = test_bit(txq_id, &priv->txq_ctx_active_msk)?1:0;
+
+        iwl_write_prph(priv, IWL50_SCD_QUEUE_STATUS_BITS(txq_id),
+                        (active << IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+                        (tx_fifo_id << IWL50_SCD_QUEUE_STTS_REG_POS_TXF) |
+                        (1 << IWL50_SCD_QUEUE_STTS_REG_POS_WSL) |
+                        IWL50_SCD_QUEUE_STTS_REG_MSK);
+
+        txq->sched_retry = scd_retry;
+
+        IWL_DEBUG_INFO("%s %s Queue %d on AC %d\n",
+                       active ? "Activate" : "Deactivate",
+                       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
+}
+
+static int iwl5000_send_wimax_coex(struct iwl_priv *priv)
+{
+        struct iwl_wimax_coex_cmd coex_cmd;
+
+        memset(&coex_cmd, 0, sizeof(coex_cmd));
+
+        return iwl_send_cmd_pdu(priv, COEX_PRIORITY_TABLE_CMD,
+                                sizeof(coex_cmd), &coex_cmd);
+}
+
+static int iwl5000_alive_notify(struct iwl_priv *priv)
+{
+        u32 a;
+        int i = 0;
+        unsigned long flags;
+        int ret;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                return ret;
+        }
+
+        priv->scd_base_addr = iwl_read_prph(priv, IWL50_SCD_SRAM_BASE_ADDR);
+        a = priv->scd_base_addr + IWL50_SCD_CONTEXT_DATA_OFFSET;
+        for (; a < priv->scd_base_addr + IWL50_SCD_TX_STTS_BITMAP_OFFSET;
+                a += 4)
+                iwl_write_targ_mem(priv, a, 0);
+        for (; a < priv->scd_base_addr + IWL50_SCD_TRANSLATE_TBL_OFFSET;
+                a += 4)
+                iwl_write_targ_mem(priv, a, 0);
+        for (; a < sizeof(u16) * priv->hw_params.max_txq_num; a += 4)
+                iwl_write_targ_mem(priv, a, 0);
+
+        iwl_write_prph(priv, IWL50_SCD_DRAM_BASE_ADDR,
+                (priv->shared_phys +
+                 offsetof(struct iwl5000_shared, queues_byte_cnt_tbls)) >> 10);
+        iwl_write_prph(priv, IWL50_SCD_QUEUECHAIN_SEL,
+                IWL50_SCD_QUEUECHAIN_SEL_ALL(
+                        priv->hw_params.max_txq_num));
+        iwl_write_prph(priv, IWL50_SCD_AGGR_SEL, 0);
+
+        /* initiate the queues */
+        for (i = 0; i < priv->hw_params.max_txq_num; i++) {
+                iwl_write_prph(priv, IWL50_SCD_QUEUE_RDPTR(i), 0);
+                iwl_write_direct32(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
+                iwl_write_targ_mem(priv, priv->scd_base_addr +
+                                IWL50_SCD_CONTEXT_QUEUE_OFFSET(i), 0);
+                iwl_write_targ_mem(priv, priv->scd_base_addr +
+                                IWL50_SCD_CONTEXT_QUEUE_OFFSET(i) +
+                                sizeof(u32),
+                                ((SCD_WIN_SIZE <<
+                                IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
+                                IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
+                                ((SCD_FRAME_LIMIT <<
+                                IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+                                IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+        }
+
+        iwl_write_prph(priv, IWL50_SCD_INTERRUPT_MASK,
+                        IWL_MASK(0, priv->hw_params.max_txq_num));
+
+        /* Activate all Tx DMA/FIFO channels */
+        priv->cfg->ops->lib->txq_set_sched(priv, IWL_MASK(0, 7));
+
+        iwl5000_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
+        /* map qos queues to fifos one-to-one */
+        for (i = 0; i < ARRAY_SIZE(iwl5000_default_queue_to_tx_fifo); i++) {
+                int ac = iwl5000_default_queue_to_tx_fifo[i];
+                iwl_txq_ctx_activate(priv, i);
+                iwl5000_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
+        }
+        /* TODO - need to initialize those FIFOs inside the loop above,
+         * not only mark them as active */
+        iwl_txq_ctx_activate(priv, 4);
+        iwl_txq_ctx_activate(priv, 7);
+        iwl_txq_ctx_activate(priv, 8);
+        iwl_txq_ctx_activate(priv, 9);
+
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+
+        iwl5000_send_wimax_coex(priv);
+
+        iwl5000_send_Xtal_calib(priv);
+
+        if (priv->ucode_type == UCODE_RT) {
+                iwl5000_send_calib_results(priv);
+                set_bit(STATUS_READY, &priv->status);
+                priv->is_open = 1;
+        }
+
+        return 0;
+}
+
 static int iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 {
         if ((priv->cfg->mod_params->num_of_queues > IWL50_NUM_QUEUES) ||
@@ -298,7 +833,6 @@ static int iwl5000_hw_set_hw_params(struct iwl_priv *priv)
 
         priv->hw_params.max_txq_num = priv->cfg->mod_params->num_of_queues;
         priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
-        priv->hw_params.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
         priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
         priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
         if (priv->cfg->mod_params->amsdu_size_8K)
@@ -430,6 +964,26 @@ static void iwl5000_txq_update_byte_cnt_tbl(struct iwl_priv *priv,
         }
 }
 
+static void iwl5000_txq_inval_byte_cnt_tbl(struct iwl_priv *priv,
+                                           struct iwl_tx_queue *txq)
+{
+        int txq_id = txq->q.id;
+        struct iwl5000_shared *shared_data = priv->shared_virt;
+        u8 sta = 0;
+
+        if (txq_id != IWL_CMD_QUEUE_NUM)
+                sta = txq->cmd[txq->q.read_ptr].cmd.tx.sta_id;
+
+        shared_data->queues_byte_cnt_tbls[txq_id].tfd_offset[txq->q.read_ptr].
+                                        val = cpu_to_le16(1 | (sta << 12));
+
+        if (txq->q.write_ptr < IWL50_MAX_WIN_SIZE) {
+                shared_data->queues_byte_cnt_tbls[txq_id].
+                        tfd_offset[IWL50_QUEUE_SIZE + txq->q.read_ptr].
+                                val = cpu_to_le16(1 | (sta << 12));
+        }
+}
+
 static u16 iwl5000_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 {
         u16 size = (u16)sizeof(struct iwl_addsta_cmd);
@@ -438,31 +992,326 @@ static u16 iwl5000_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 }
 
 
-static int iwl5000_disable_tx_fifo(struct iwl_priv *priv)
+/*
+ * Activate/Deactivat Tx DMA/FIFO channels according tx fifos mask
+ * must be called under priv->lock and mac access
+ */
+static void iwl5000_txq_set_sched(struct iwl_priv *priv, u32 mask)
 {
-        unsigned long flags;
-        int ret;
+        iwl_write_prph(priv, IWL50_SCD_TXFACT, mask);
+}
 
-        spin_lock_irqsave(&priv->lock, flags);
 
-        ret = iwl_grab_nic_access(priv);
-        if (unlikely(ret)) {
-                IWL_ERROR("Tx fifo reset failed");
-                spin_unlock_irqrestore(&priv->lock, flags);
-                return ret;
-        }
+static inline u32 iwl5000_get_scd_ssn(struct iwl5000_tx_resp *tx_resp)
+{
+        __le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
+                                tx_resp->frame_count);
+        return le32_to_cpu(*scd_ssn) & MAX_SN;
 
-        iwl_write_prph(priv, IWL50_SCD_TXFACT, 0);
-        iwl_release_nic_access(priv);
-        spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int iwl5000_tx_status_reply_tx(struct iwl_priv *priv,
+                                      struct iwl_ht_agg *agg,
+                                      struct iwl5000_tx_resp *tx_resp,
+                                      u16 start_idx)
+{
+        u16 status;
+        struct agg_tx_status *frame_status = &tx_resp->status;
+        struct ieee80211_tx_info *info = NULL;
+        struct ieee80211_hdr *hdr = NULL;
+        int i, sh;
+        int txq_id, idx;
+        u16 seq;
+
+        if (agg->wait_for_ba)
+                IWL_DEBUG_TX_REPLY("got tx response w/o block-ack\n");
+
+        agg->frame_count = tx_resp->frame_count;
+        agg->start_idx = start_idx;
+        agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+        agg->bitmap = 0;
+
+        /* # frames attempted by Tx command */
+        if (agg->frame_count == 1) {
+                /* Only one frame was attempted; no block-ack will arrive */
+                status = le16_to_cpu(frame_status[0].status);
+                seq  = le16_to_cpu(frame_status[0].sequence);
+                idx = SEQ_TO_INDEX(seq);
+                txq_id = SEQ_TO_QUEUE(seq);
+
+                /* FIXME: code repetition */
+                IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
+                                   agg->frame_count, agg->start_idx, idx);
+
+                info = IEEE80211_SKB_CB(priv->txq[txq_id].txb[idx].skb[0]);
+                info->status.retry_count = tx_resp->failure_frame;
+                info->flags &= ~IEEE80211_TX_CTL_AMPDU;
+                info->flags |= iwl_is_tx_success(status)?
+                        IEEE80211_TX_STAT_ACK : 0;
+                iwl4965_hwrate_to_tx_control(priv,
+                                             le32_to_cpu(tx_resp->rate_n_flags),
+                                             info);
+                /* FIXME: code repetition end */
+
+                IWL_DEBUG_TX_REPLY("1 Frame 0x%x failure :%d\n",
+                                    status & 0xff, tx_resp->failure_frame);
+                IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n",
+                        iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
+
+                agg->wait_for_ba = 0;
+        } else {
+                /* Two or more frames were attempted; expect block-ack */
+                u64 bitmap = 0;
+                int start = agg->start_idx;
+
+                /* Construct bit-map of pending frames within Tx window */
+                for (i = 0; i < agg->frame_count; i++) {
+                        u16 sc;
+                        status = le16_to_cpu(frame_status[i].status);
+                        seq  = le16_to_cpu(frame_status[i].sequence);
+                        idx = SEQ_TO_INDEX(seq);
+                        txq_id = SEQ_TO_QUEUE(seq);
+
+                        if (status & (AGG_TX_STATE_FEW_BYTES_MSK |
+                                      AGG_TX_STATE_ABORT_MSK))
+                                continue;
+
+                        IWL_DEBUG_TX_REPLY("FrameCnt = %d, txq_id=%d idx=%d\n",
+                                           agg->frame_count, txq_id, idx);
+
+                        hdr = iwl_tx_queue_get_hdr(priv, txq_id, idx);
+
+                        sc = le16_to_cpu(hdr->seq_ctrl);
+                        if (idx != (SEQ_TO_SN(sc) & 0xff)) {
+                                IWL_ERROR("BUG_ON idx doesn't match seq control"
+                                          " idx=%d, seq_idx=%d, seq=%d\n",
+                                          idx, SEQ_TO_SN(sc),
+                                          hdr->seq_ctrl);
+                                return -1;
+                        }
+
+                        IWL_DEBUG_TX_REPLY("AGG Frame i=%d idx %d seq=%d\n",
+                                           i, idx, SEQ_TO_SN(sc));
+
+                        sh = idx - start;
+                        if (sh > 64) {
+                                sh = (start - idx) + 0xff;
+                                bitmap = bitmap << sh;
+                                sh = 0;
+                                start = idx;
+                        } else if (sh < -64)
+                                sh  = 0xff - (start - idx);
+                        else if (sh < 0) {
+                                sh = start - idx;
+                                start = idx;
+                                bitmap = bitmap << sh;
+                                sh = 0;
+                        }
+                        bitmap |= (1 << sh);
+                        IWL_DEBUG_TX_REPLY("start=%d bitmap=0x%x\n",
+                                           start, (u32)(bitmap & 0xFFFFFFFF));
+                }
+
+                agg->bitmap = bitmap;
+                agg->start_idx = start;
+                agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+                IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%llx\n",
+                                   agg->frame_count, agg->start_idx,
+                                   (unsigned long long)agg->bitmap);
 
+                if (bitmap)
+                        agg->wait_for_ba = 1;
+        }
         return 0;
 }
 
+static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
+                                struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+        int txq_id = SEQ_TO_QUEUE(sequence);
+        int index = SEQ_TO_INDEX(sequence);
+        struct iwl_tx_queue *txq = &priv->txq[txq_id];
+        struct ieee80211_tx_info *info;
+        struct iwl5000_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+        u32  status = le16_to_cpu(tx_resp->status.status);
+#ifdef CONFIG_IWL4965_HT
+        int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
+        u16 fc;
+        struct ieee80211_hdr *hdr;
+        u8 *qc = NULL;
+#endif
+
+        if ((index >= txq->q.n_bd) || (iwl_queue_used(&txq->q, index) == 0)) {
+                IWL_ERROR("Read index for DMA queue txq_id (%d) index %d "
+                          "is out of range [0-%d] %d %d\n", txq_id,
+                          index, txq->q.n_bd, txq->q.write_ptr,
+                          txq->q.read_ptr);
+                return;
+        }
+
+        info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+        memset(&info->status, 0, sizeof(info->status));
+
+#ifdef CONFIG_IWL4965_HT
+        hdr = iwl_tx_queue_get_hdr(priv, txq_id, index);
+        fc = le16_to_cpu(hdr->frame_control);
+        if (ieee80211_is_qos_data(fc)) {
+                qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                tid = qc[0] & 0xf;
+        }
+
+        sta_id = iwl_get_ra_sta_id(priv, hdr);
+        if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
+                IWL_ERROR("Station not known\n");
+                return;
+        }
+
+        if (txq->sched_retry) {
+                const u32 scd_ssn = iwl5000_get_scd_ssn(tx_resp);
+                struct iwl_ht_agg *agg = NULL;
+
+                if (!qc)
+                        return;
+
+                agg = &priv->stations[sta_id].tid[tid].agg;
+
+                iwl5000_tx_status_reply_tx(priv, agg, tx_resp, index);
+
+                if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status)) {
+                        /* TODO: send BAR */
+                }
+
+                if (txq->q.read_ptr != (scd_ssn & 0xff)) {
+                        int freed, ampdu_q;
+                        index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
+                        IWL_DEBUG_TX_REPLY("Retry scheduler reclaim scd_ssn "
+                                           "%d index %d\n", scd_ssn , index);
+                        freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+
+                        if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+                            txq_id >= 0 && priv->mac80211_registered &&
+                            agg->state != IWL_EMPTYING_HW_QUEUE_DELBA) {
+                                /* calculate mac80211 ampdu sw queue to wake */
+                                ampdu_q = txq_id - IWL_BACK_QUEUE_FIRST_ID +
+                                          priv->hw->queues;
+                                if (agg->state == IWL_AGG_OFF)
+                                        ieee80211_wake_queue(priv->hw, txq_id);
+                                else
+                                        ieee80211_wake_queue(priv->hw, ampdu_q);
+                        }
+                        iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+                }
+        } else {
+#endif /* CONFIG_IWL4965_HT */
+
+        info->status.retry_count = tx_resp->failure_frame;
+        info->flags = iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+        iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
+                                     info);
+
+        IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
+                     "retries %d\n", txq_id, iwl_get_tx_fail_reason(status),
+                     status, le32_to_cpu(tx_resp->rate_n_flags),
+                     tx_resp->failure_frame);
+
+        IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
+#ifdef CONFIG_IWL4965_HT
+        if (index != -1) {
+                int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                if (tid != MAX_TID_COUNT)
+                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+                if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+                        (txq_id >= 0) && priv->mac80211_registered)
+                        ieee80211_wake_queue(priv->hw, txq_id);
+                if (tid != MAX_TID_COUNT)
+                        iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+        }
+        }
+#endif /* CONFIG_IWL4965_HT */
+
+        if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+                IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
+}
+
+/* Currently 5000 is the supperset of everything */
+static u16 iwl5000_get_hcmd_size(u8 cmd_id, u16 len)
+{
+        return len;
+}
+
+static void iwl5000_rx_handler_setup(struct iwl_priv *priv)
+{
+        /* init calibration handlers */
+        priv->rx_handlers[CALIBRATION_RES_NOTIFICATION] =
+                                        iwl5000_rx_calib_result;
+        priv->rx_handlers[CALIBRATION_COMPLETE_NOTIFICATION] =
+                                        iwl5000_rx_calib_complete;
+        priv->rx_handlers[REPLY_TX] = iwl5000_rx_reply_tx;
+}
+
+
+static int iwl5000_hw_valid_rtc_data_addr(u32 addr)
+{
+        return (addr >= RTC_DATA_LOWER_BOUND) &&
+                (addr < IWL50_RTC_DATA_UPPER_BOUND);
+}
+
+static int iwl5000_send_rxon_assoc(struct iwl_priv *priv)
+{
+        int ret = 0;
+        struct iwl5000_rxon_assoc_cmd rxon_assoc;
+        const struct iwl_rxon_cmd *rxon1 = &priv->staging_rxon;
+        const struct iwl_rxon_cmd *rxon2 = &priv->active_rxon;
+
+        if ((rxon1->flags == rxon2->flags) &&
+            (rxon1->filter_flags == rxon2->filter_flags) &&
+            (rxon1->cck_basic_rates == rxon2->cck_basic_rates) &&
+            (rxon1->ofdm_ht_single_stream_basic_rates ==
+             rxon2->ofdm_ht_single_stream_basic_rates) &&
+            (rxon1->ofdm_ht_dual_stream_basic_rates ==
+             rxon2->ofdm_ht_dual_stream_basic_rates) &&
+            (rxon1->ofdm_ht_triple_stream_basic_rates ==
+             rxon2->ofdm_ht_triple_stream_basic_rates) &&
+            (rxon1->acquisition_data == rxon2->acquisition_data) &&
+            (rxon1->rx_chain == rxon2->rx_chain) &&
+            (rxon1->ofdm_basic_rates == rxon2->ofdm_basic_rates)) {
+                IWL_DEBUG_INFO("Using current RXON_ASSOC.  Not resending.\n");
+                return 0;
+        }
+
+        rxon_assoc.flags = priv->staging_rxon.flags;
+        rxon_assoc.filter_flags = priv->staging_rxon.filter_flags;
+        rxon_assoc.ofdm_basic_rates = priv->staging_rxon.ofdm_basic_rates;
+        rxon_assoc.cck_basic_rates = priv->staging_rxon.cck_basic_rates;
+        rxon_assoc.reserved1 = 0;
+        rxon_assoc.reserved2 = 0;
+        rxon_assoc.reserved3 = 0;
+        rxon_assoc.ofdm_ht_single_stream_basic_rates =
+            priv->staging_rxon.ofdm_ht_single_stream_basic_rates;
+        rxon_assoc.ofdm_ht_dual_stream_basic_rates =
+            priv->staging_rxon.ofdm_ht_dual_stream_basic_rates;
+        rxon_assoc.rx_chain_select_flags = priv->staging_rxon.rx_chain;
+        rxon_assoc.ofdm_ht_triple_stream_basic_rates =
+                 priv->staging_rxon.ofdm_ht_triple_stream_basic_rates;
+        rxon_assoc.acquisition_data = priv->staging_rxon.acquisition_data;
+
+        ret = iwl_send_cmd_pdu_async(priv, REPLY_RXON_ASSOC,
+                                     sizeof(rxon_assoc), &rxon_assoc, NULL);
+        if (ret)
+                return ret;
+
+        return ret;
+}
+
 static struct iwl_hcmd_ops iwl5000_hcmd = {
+        .rxon_assoc = iwl5000_send_rxon_assoc,
 };
 
 static struct iwl_hcmd_utils_ops iwl5000_hcmd_utils = {
+        .get_hcmd_size = iwl5000_get_hcmd_size,
         .build_addsta_hcmd = iwl5000_build_addsta_hcmd,
 #ifdef CONFIG_IWL5000_RUN_TIME_CALIB
         .gain_computation = iwl5000_gain_computation,
@@ -476,9 +1325,17 @@ static struct iwl_lib_ops iwl5000_lib = {
         .free_shared_mem = iwl5000_free_shared_mem,
         .shared_mem_rx_idx = iwl5000_shared_mem_rx_idx,
         .txq_update_byte_cnt_tbl = iwl5000_txq_update_byte_cnt_tbl,
-        .disable_tx_fifo = iwl5000_disable_tx_fifo,
+        .txq_inval_byte_cnt_tbl = iwl5000_txq_inval_byte_cnt_tbl,
+        .txq_set_sched = iwl5000_txq_set_sched,
+        .rx_handler_setup = iwl5000_rx_handler_setup,
+        .is_valid_rtc_data_addr = iwl5000_hw_valid_rtc_data_addr,
+        .load_ucode = iwl5000_load_ucode,
+        .init_alive_start = iwl5000_init_alive_start,
+        .alive_notify = iwl5000_alive_notify,
         .apm_ops = {
                 .init = iwl5000_apm_init,
+                .reset = iwl5000_apm_reset,
+                .stop = iwl5000_apm_stop,
                 .config = iwl5000_nic_config,
                 .set_pwr_src = iwl4965_set_pwr_src,
         },
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.c b/drivers/net/wireless/iwlwifi/iwl-calib.c
index 1289d4c91abe..a6c7f0d9a414 100644
--- a/drivers/net/wireless/iwlwifi/iwl-calib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.c
@@ -426,6 +426,9 @@ void iwl_init_sensitivity(struct iwl_priv *priv)
         struct iwl_sensitivity_data *data = NULL;
         const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
 
+        if (priv->disable_sens_cal)
+                return;
+
         IWL_DEBUG_CALIB("Start iwl_init_sensitivity\n");
 
         /* Clear driver's sensitivity algo data */
@@ -486,6 +489,9 @@ void iwl_sensitivity_calibration(struct iwl_priv *priv,
         unsigned long flags;
         struct statistics_general_data statis;
 
+        if (priv->disable_sens_cal)
+                return;
+
         data = &(priv->sensitivity_data);
 
         if (!iwl_is_associated(priv)) {
@@ -608,6 +614,9 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv,
         unsigned long flags;
         struct statistics_rx_non_phy *rx_info = &(stat_resp->rx.general);
 
+        if (priv->disable_chain_noise_cal)
+                return;
+
         data = &(priv->chain_noise_data);
 
         /* Accumulate just the first 20 beacons after the first association,
@@ -777,3 +786,21 @@ void iwl_chain_noise_calibration(struct iwl_priv *priv,
 }
 EXPORT_SYMBOL(iwl_chain_noise_calibration);
 
+
+void iwl_reset_run_time_calib(struct iwl_priv *priv)
+{
+        int i;
+        memset(&(priv->sensitivity_data), 0,
+               sizeof(struct iwl_sensitivity_data));
+        memset(&(priv->chain_noise_data), 0,
+               sizeof(struct iwl_chain_noise_data));
+        for (i = 0; i < NUM_RX_CHAINS; i++)
+                priv->chain_noise_data.delta_gain_code[i] =
+                                CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
+
+        /* Ask for statistics now, the uCode will send notification
+         * periodically after association */
+        iwl_send_statistics_request(priv, CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_reset_run_time_calib);
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.h b/drivers/net/wireless/iwlwifi/iwl-calib.h
index 933b0b0a797b..b8e57c59eac8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-calib.h
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.h
@@ -78,10 +78,12 @@ void iwl_sensitivity_calibration(struct iwl_priv *priv,
                                 struct iwl4965_notif_statistics *resp);
 
 void iwl_init_sensitivity(struct iwl_priv *priv);
-
+void iwl_reset_run_time_calib(struct iwl_priv *priv);
 static inline void iwl_chain_noise_reset(struct iwl_priv *priv)
 {
-        if (priv->cfg->ops->utils->chain_noise_reset)
+
+        if (!priv->disable_chain_noise_cal &&
+            priv->cfg->ops->utils->chain_noise_reset)
                 priv->cfg->ops->utils->chain_noise_reset(priv);
 }
 #else
@@ -99,6 +101,9 @@ static inline void iwl_init_sensitivity(struct iwl_priv *priv)
 static inline void iwl_chain_noise_reset(struct iwl_priv *priv)
 {
 }
+static inline void iwl_reset_run_time_calib(struct iwl_priv *priv)
+{
+}
 #endif
 
 #endif /* __iwl_calib_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index d16a853f376a..fb6f5ffb9f1d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -93,6 +93,11 @@ enum {
         REPLY_LEDS_CMD = 0x48,
         REPLY_TX_LINK_QUALITY_CMD = 0x4e, /* 4965 only */
 
+        /* WiMAX coexistence */
+        COEX_PRIORITY_TABLE_CMD = 0x5a, /*5000 only */
+        COEX_MEDIUM_NOTIFICATION = 0x5b,
+        COEX_EVENT_CMD = 0x5c,
+
         /* 802.11h related */
         RADAR_NOTIFICATION = 0x70,      /* not used */
         REPLY_QUIET_CMD = 0x71,         /* not used */
@@ -368,7 +373,7 @@ struct iwl4965_tx_power_db {
  * 3)  Tx gain compensation to balance 4965's 2 Tx chains for MIMO operation,
  *     for each of 5 frequency ranges.
  */
-struct iwl4965_init_alive_resp {
+struct iwl_init_alive_resp {
         u8 ucode_minor;
         u8 ucode_major;
         __le16 reserved1;
@@ -444,7 +449,7 @@ struct iwl4965_init_alive_resp {
  * The Linux driver can print both logs to the system log when a uCode error
  * occurs.
  */
-struct iwl4965_alive_resp {
+struct iwl_alive_resp {
         u8 ucode_minor;
         u8 ucode_major;
         __le16 reserved1;
@@ -468,7 +473,7 @@ union tsf {
 /*
  * REPLY_ERROR = 0x2 (response only, not a command)
  */
-struct iwl4965_error_resp {
+struct iwl_error_resp {
         __le32 error_type;
         u8 cmd_id;
         u8 reserved1;
@@ -600,6 +605,46 @@ struct iwl4965_rxon_cmd {
         u8 ofdm_ht_dual_stream_basic_rates;
 } __attribute__ ((packed));
 
+/* 5000 HW just extend this cmmand */
+struct iwl_rxon_cmd {
+        u8 node_addr[6];
+        __le16 reserved1;
+        u8 bssid_addr[6];
+        __le16 reserved2;
+        u8 wlap_bssid_addr[6];
+        __le16 reserved3;
+        u8 dev_type;
+        u8 air_propagation;
+        __le16 rx_chain;
+        u8 ofdm_basic_rates;
+        u8 cck_basic_rates;
+        __le16 assoc_id;
+        __le32 flags;
+        __le32 filter_flags;
+        __le16 channel;
+        u8 ofdm_ht_single_stream_basic_rates;
+        u8 ofdm_ht_dual_stream_basic_rates;
+        u8 ofdm_ht_triple_stream_basic_rates;
+        u8 reserved5;
+        __le16 acquisition_data;
+        __le16 reserved6;
+} __attribute__ ((packed));
+
+struct iwl5000_rxon_assoc_cmd {
+        __le32 flags;
+        __le32 filter_flags;
+        u8 ofdm_basic_rates;
+        u8 cck_basic_rates;
+        __le16 reserved1;
+        u8 ofdm_ht_single_stream_basic_rates;
+        u8 ofdm_ht_dual_stream_basic_rates;
+        u8 ofdm_ht_triple_stream_basic_rates;
+        u8 reserved2;
+        __le16 rx_chain_select_flags;
+        __le16 acquisition_data;
+        __le32 reserved3;
+} __attribute__ ((packed));
+
 /*
  * REPLY_RXON_ASSOC = 0x11 (command, has simple generic response)
  */
@@ -614,6 +659,9 @@ struct iwl4965_rxon_assoc_cmd {
         __le16 reserved;
 } __attribute__ ((packed));
 
+
+
+
 /*
  * REPLY_RXON_TIMING = 0x14 (command, has simple generic response)
  */
@@ -897,10 +945,28 @@ struct iwl_addsta_cmd {
 /*
  * REPLY_ADD_STA = 0x18 (response)
  */
-struct iwl4965_add_sta_resp {
+struct iwl_add_sta_resp {
         u8 status;      /* ADD_STA_* */
 } __attribute__ ((packed));
 
+#define REM_STA_SUCCESS_MSK              0x1
+/*
+ *  REPLY_REM_STA = 0x19 (response)
+ */
+struct iwl_rem_sta_resp {
+        u8 status;
+} __attribute__ ((packed));
+
+/*
+ *  REPLY_REM_STA = 0x19 (command)
+ */
+struct iwl_rem_sta_cmd {
+        u8 num_sta;     /* number of removed stations */
+        u8 reserved[3];
+        u8 addr[ETH_ALEN]; /* MAC addr of the first station */
+        u8 reserved2[2];
+} __attribute__ ((packed));
+
 /*
  * REPLY_WEP_KEY = 0x20
  */
@@ -1170,7 +1236,7 @@ struct iwl4965_dram_scratch {
 /*
  * REPLY_TX = 0x1c (command)
  */
-struct iwl4965_tx_cmd {
+struct iwl_tx_cmd {
         /*
          * MPDU byte count:
          * MAC header (24/26/30/32 bytes) + 2 bytes pad if 26/30 header size,
@@ -1316,6 +1382,15 @@ enum {
         TX_ABORT_REQUIRED_MSK = 0x80000000,     /* bits 31:31 */
 };
 
+static inline int iwl_is_tx_success(u32 status)
+{
+        status &= TX_STATUS_MSK;
+        return (status == TX_STATUS_SUCCESS)
+            || (status == TX_STATUS_DIRECT_DONE);
+}
+
+
+
 /* *******************************
  * TX aggregation status
  ******************************* */
@@ -1370,6 +1445,11 @@ enum {
  *     within the sending station (this 4965), rather than whether it was
  *     received successfully by the destination station.
  */
+struct agg_tx_status {
+        __le16 status;
+        __le16 sequence;
+} __attribute__ ((packed));
+
 struct iwl4965_tx_resp {
         u8 frame_count;         /* 1 no aggregation, >1 aggregation */
         u8 bt_kill_count;       /* # blocked by bluetooth (unused for agg) */
@@ -1404,11 +1484,6 @@ struct iwl4965_tx_resp {
         __le32 status;  /* TX status (for aggregation status of 1st frame) */
 } __attribute__ ((packed));
 
-struct agg_tx_status {
-        __le16 status;
-        __le16 sequence;
-} __attribute__ ((packed));
-
 struct iwl4965_tx_resp_agg {
         u8 frame_count;         /* 1 no aggregation, >1 aggregation */
         u8 reserved1;
@@ -1423,6 +1498,44 @@ struct iwl4965_tx_resp_agg {
                                         /* of 1st frame) */
 } __attribute__ ((packed));
 
+struct iwl5000_tx_resp {
+        u8 frame_count;         /* 1 no aggregation, >1 aggregation */
+        u8 bt_kill_count;       /* # blocked by bluetooth (unused for agg) */
+        u8 failure_rts;         /* # failures due to unsuccessful RTS */
+        u8 failure_frame;       /* # failures due to no ACK (unused for agg) */
+
+        /* For non-agg:  Rate at which frame was successful.
+         * For agg:  Rate at which all frames were transmitted. */
+        __le32 rate_n_flags;    /* RATE_MCS_*  */
+
+        /* For non-agg:  RTS + CTS + frame tx attempts time + ACK.
+         * For agg:  RTS + CTS + aggregation tx time + block-ack time. */
+        __le16 wireless_media_time;     /* uSecs */
+
+        __le16 reserved;
+        __le32 pa_power1;       /* RF power amplifier measurement (not used) */
+        __le32 pa_power2;
+
+        __le32 tfd_info;
+        __le16 seq_ctl;
+        __le16 byte_cnt;
+        __le32 tlc_info;
+        /*
+         * For non-agg:  frame status TX_STATUS_*
+         * For agg:  status of 1st frame, AGG_TX_STATE_*; other frame status
+         *           fields follow this one, up to frame_count.
+         *           Bit fields:
+         *           11- 0:  AGG_TX_STATE_* status code
+         *           15-12:  Retry count for 1st frame in aggregation (retries
+         *                   occur if tx failed for this frame when it was a
+         *                   member of a previous aggregation block).  If rate
+         *                   scaling is used, retry count indicates the rate
+         *                   table entry used for all frames in the new agg.
+         *           31-16:  Sequence # for this frame's Tx cmd (not SSN!)
+         */
+        struct agg_tx_status status;    /* TX status (in aggregation -
+                                         * status of 1st frame) */
+} __attribute__ ((packed));
 /*
  * REPLY_COMPRESSED_BA = 0xc5 (response only, not a command)
  *
@@ -2109,7 +2222,7 @@ struct iwl4965_scan_cmd {
 
         /* For active scans (set to all-0s for passive scans).
          * Does not include payload.  Must specify Tx rate; no rate scaling. */
-        struct iwl4965_tx_cmd tx_cmd;
+        struct iwl_tx_cmd tx_cmd;
 
         /* For directed active scans (set to all-0s otherwise) */
         struct iwl4965_ssid_ie direct_scan[PROBE_OPTION_MAX];
@@ -2206,7 +2319,7 @@ struct iwl4965_beacon_notif {
  * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
  */
 struct iwl4965_tx_beacon_cmd {
-        struct iwl4965_tx_cmd tx;
+        struct iwl_tx_cmd tx;
         __le16 tim_idx;
         u8 tim_size;
         u8 reserved1;
@@ -2729,10 +2842,59 @@ enum {
         IWL5000_PHY_CALIBRATE_AGC_TABLE_CMD     = 14,
         IWL5000_PHY_CALIBRATE_CRYSTAL_FRQ_CMD   = 15,
         IWL5000_PHY_CALIBRATE_BASE_BAND_CMD     = 16,
+        IWL5000_PHY_CALIBRATE_TX_IQ_PERD_CMD    = 17,
         IWL5000_PHY_CALIBRATE_CHAIN_NOISE_RESET_CMD = 18,
         IWL5000_PHY_CALIBRATE_CHAIN_NOISE_GAIN_CMD = 19,
 };
 
+enum {
+        CALIBRATION_CFG_CMD = 0x65,
+        CALIBRATION_RES_NOTIFICATION = 0x66,
+        CALIBRATION_COMPLETE_NOTIFICATION = 0x67
+};
+
+struct iwl_cal_crystal_freq_cmd {
+        u8 cap_pin1;
+        u8 cap_pin2;
+} __attribute__ ((packed));
+
+struct iwl5000_calibration {
+        u8 op_code;
+        u8 first_group;
+        u8 num_groups;
+        u8 all_data_valid;
+        struct iwl_cal_crystal_freq_cmd data;
+} __attribute__ ((packed));
+
+#define IWL_CALIB_INIT_CFG_ALL  __constant_cpu_to_le32(0xffffffff)
+
+struct iwl_calib_cfg_elmnt_s {
+        __le32 is_enable;
+        __le32 start;
+        __le32 send_res;
+        __le32 apply_res;
+        __le32 reserved;
+} __attribute__ ((packed));
+
+struct iwl_calib_cfg_status_s {
+        struct iwl_calib_cfg_elmnt_s once;
+        struct iwl_calib_cfg_elmnt_s perd;
+        __le32 flags;
+} __attribute__ ((packed));
+
+struct iwl5000_calib_cfg_cmd {
+        struct iwl_calib_cfg_status_s ucd_calib_cfg;
+        struct iwl_calib_cfg_status_s drv_calib_cfg;
+        __le32 reserved1;
+} __attribute__ ((packed));
+
+struct iwl5000_calib_hdr {
+        u8 op_code;
+        u8 first_group;
+        u8 groups_num;
+        u8 data_valid;
+} __attribute__ ((packed));
+
 struct iwl5000_calibration_chain_noise_reset_cmd {
         u8 op_code;     /* IWL5000_PHY_CALIBRATE_CHAIN_NOISE_RESET_CMD */
         u8 flags;       /* not used */
@@ -2771,6 +2933,55 @@ struct iwl4965_led_cmd {
         u8 reserved;
 } __attribute__ ((packed));
 
+/*
+ * Coexistence WIFI/WIMAX  Command
+ * COEX_PRIORITY_TABLE_CMD = 0x5a
+ *
+ */
+enum {
+        COEX_UNASSOC_IDLE               = 0,
+        COEX_UNASSOC_MANUAL_SCAN        = 1,
+        COEX_UNASSOC_AUTO_SCAN          = 2,
+        COEX_CALIBRATION                = 3,
+        COEX_PERIODIC_CALIBRATION       = 4,
+        COEX_CONNECTION_ESTAB           = 5,
+        COEX_ASSOCIATED_IDLE            = 6,
+        COEX_ASSOC_MANUAL_SCAN          = 7,
+        COEX_ASSOC_AUTO_SCAN            = 8,
+        COEX_ASSOC_ACTIVE_LEVEL         = 9,
+        COEX_RF_ON                      = 10,
+        COEX_RF_OFF                     = 11,
+        COEX_STAND_ALONE_DEBUG          = 12,
+        COEX_IPAN_ASSOC_LEVEL           = 13,
+        COEX_RSRVD1                     = 14,
+        COEX_RSRVD2                     = 15,
+        COEX_NUM_OF_EVENTS              = 16
+};
+
+struct iwl_wimax_coex_event_entry {
+        u8 request_prio;
+        u8 win_medium_prio;
+        u8 reserved;
+        u8 flags;
+} __attribute__ ((packed));
+
+/* COEX flag masks */
+
+/* Staion table is valid */
+#define COEX_FLAGS_STA_TABLE_VALID_MSK      (0x1)
+/* UnMask wakeup src at unassociated sleep */
+#define COEX_FLAGS_UNASSOC_WA_UNMASK_MSK    (0x4)
+/* UnMask wakeup src at associated sleep */
+#define COEX_FLAGS_ASSOC_WA_UNMASK_MSK      (0x8)
+/* Enable CoEx feature. */
+#define COEX_FLAGS_COEX_ENABLE_MSK          (0x80)
+
+struct iwl_wimax_coex_cmd {
+        u8 flags;
+        u8 reserved[3];
+        struct iwl_wimax_coex_event_entry sta_prio[COEX_NUM_OF_EVENTS];
+} __attribute__ ((packed));
+
 /******************************************************************************
  * (13)
  * Union of all expected notifications/responses:
@@ -2781,20 +2992,22 @@ struct iwl_rx_packet {
         __le32 len;
         struct iwl_cmd_header hdr;
         union {
-                struct iwl4965_alive_resp alive_frame;
+                struct iwl_alive_resp alive_frame;
                 struct iwl4965_rx_frame rx_frame;
                 struct iwl4965_tx_resp tx_resp;
                 struct iwl4965_spectrum_notification spectrum_notif;
                 struct iwl4965_csa_notification csa_notif;
-                struct iwl4965_error_resp err_resp;
+                struct iwl_error_resp err_resp;
                 struct iwl4965_card_state_notif card_state_notif;
                 struct iwl4965_beacon_notif beacon_status;
-                struct iwl4965_add_sta_resp add_sta;
+                struct iwl_add_sta_resp add_sta;
+                struct iwl_rem_sta_resp rem_sta;
                 struct iwl4965_sleep_notification sleep_notif;
                 struct iwl4965_spectrum_resp spectrum;
                 struct iwl4965_notif_statistics stats;
                 struct iwl4965_compressed_ba_resp compressed_ba;
                 struct iwl4965_missed_beacon_notif missed_beacon;
+                struct iwl5000_calibration calib;
                 __le32 status;
                 u8 raw[0];
         } u;
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index d3cbad2bf877..61716ba90427 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -67,7 +67,7 @@ MODULE_LICENSE("GPL");
  * maps to IWL_RATE_INVALID
  *
  */
-const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT] = {
+const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT] = {
         IWL_DECLARE_RATE_INFO(1, INV, INV, 2, INV, 2, INV, 2),    /*  1mbps */
         IWL_DECLARE_RATE_INFO(2, INV, 1, 5, 1, 5, 1, 5),          /*  2mbps */
         IWL_DECLARE_RATE_INFO(5, INV, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
@@ -83,7 +83,12 @@ const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT] = {
         IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
         /* FIXME:RS:          ^^    should be INV (legacy) */
 };
-EXPORT_SYMBOL(iwl4965_rates);
+EXPORT_SYMBOL(iwl_rates);
+
+
+const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+EXPORT_SYMBOL(iwl_bcast_addr);
+
 
 /* This function both allocates and initializes hw and priv. */
 struct ieee80211_hw *iwl_alloc_all(struct iwl_cfg *cfg,
@@ -317,24 +322,33 @@ void iwl_reset_qos(struct iwl_priv *priv)
 EXPORT_SYMBOL(iwl_reset_qos);
 
 #ifdef CONFIG_IWL4965_HT
+#define MAX_BIT_RATE_40_MHZ 0x96; /* 150 Mbps */
+#define MAX_BIT_RATE_20_MHZ 0x48; /* 72 Mbps */
 static void iwlcore_init_ht_hw_capab(const struct iwl_priv *priv,
                               struct ieee80211_ht_info *ht_info,
                               enum ieee80211_band band)
 {
+        u16 max_bit_rate = 0;
+        u8 rx_chains_num = priv->hw_params.rx_chains_num;
+        u8 tx_chains_num = priv->hw_params.tx_chains_num;
+
         ht_info->cap = 0;
         memset(ht_info->supp_mcs_set, 0, 16);
 
         ht_info->ht_supported = 1;
 
+        ht_info->cap |= (u16)IEEE80211_HT_CAP_GRN_FLD;
+        ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_20;
+        ht_info->cap |= (u16)(IEEE80211_HT_CAP_MIMO_PS &
+                             (IWL_MIMO_PS_NONE << 2));
+
+        max_bit_rate = MAX_BIT_RATE_20_MHZ;
         if (priv->hw_params.fat_channel & BIT(band)) {
                 ht_info->cap |= (u16)IEEE80211_HT_CAP_SUP_WIDTH;
                 ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_40;
                 ht_info->supp_mcs_set[4] = 0x01;
+                max_bit_rate = MAX_BIT_RATE_40_MHZ;
         }
-        ht_info->cap |= (u16)IEEE80211_HT_CAP_GRN_FLD;
-        ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_20;
-        ht_info->cap |= (u16)(IEEE80211_HT_CAP_MIMO_PS &
-                             (IWL_MIMO_PS_NONE << 2));
 
         if (priv->cfg->mod_params->amsdu_size_8K)
                 ht_info->cap |= (u16)IEEE80211_HT_CAP_MAX_AMSDU;
@@ -343,10 +357,22 @@ static void iwlcore_init_ht_hw_capab(const struct iwl_priv *priv,
         ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
 
         ht_info->supp_mcs_set[0] = 0xFF;
-        if (priv->hw_params.tx_chains_num >= 2)
+        if (rx_chains_num >= 2)
                 ht_info->supp_mcs_set[1] = 0xFF;
-        if (priv->hw_params.tx_chains_num >= 3)
+        if (rx_chains_num >= 3)
                 ht_info->supp_mcs_set[2] = 0xFF;
+
+        /* Highest supported Rx data rate */
+        max_bit_rate *= rx_chains_num;
+        ht_info->supp_mcs_set[10] = (u8)(max_bit_rate & 0x00FF);
+        ht_info->supp_mcs_set[11] = (u8)((max_bit_rate & 0xFF00) >> 8);
+
+        /* Tx MCS capabilities */
+        ht_info->supp_mcs_set[12] = IEEE80211_HT_CAP_MCS_TX_DEFINED;
+        if (tx_chains_num != rx_chains_num) {
+                ht_info->supp_mcs_set[12] |= IEEE80211_HT_CAP_MCS_TX_RX_DIFF;
+                ht_info->supp_mcs_set[12] |= ((tx_chains_num - 1) << 2);
+        }
 }
 #else
 static inline void iwlcore_init_ht_hw_capab(const struct iwl_priv *priv,
@@ -362,7 +388,7 @@ static void iwlcore_init_hw_rates(struct iwl_priv *priv,
         int i;
 
         for (i = 0; i < IWL_RATE_COUNT; i++) {
-                rates[i].bitrate = iwl4965_rates[i].ieee * 5;
+                rates[i].bitrate = iwl_rates[i].ieee * 5;
                 rates[i].hw_value = i; /* Rate scaling will work on indexes */
                 rates[i].hw_value_short = i;
                 rates[i].flags = 0;
@@ -371,7 +397,7 @@ static void iwlcore_init_hw_rates(struct iwl_priv *priv,
                          * If CCK != 1M then set short preamble rate flag.
                          */
                         rates[i].flags |=
-                                (iwl4965_rates[i].plcp == IWL_RATE_1M_PLCP) ?
+                                (iwl_rates[i].plcp == IWL_RATE_1M_PLCP) ?
                                         0 : IEEE80211_RATE_SHORT_PREAMBLE;
                 }
         }
@@ -460,6 +486,25 @@ static int iwlcore_init_geos(struct iwl_priv *priv)
                         if (ch->flags & EEPROM_CHANNEL_RADAR)
                                 geo_ch->flags |= IEEE80211_CHAN_RADAR;
 
+                        switch (ch->fat_extension_channel) {
+                        case HT_IE_EXT_CHANNEL_ABOVE:
+                                /* only above is allowed, disable below */
+                                geo_ch->flags |= IEEE80211_CHAN_NO_FAT_BELOW;
+                                break;
+                        case HT_IE_EXT_CHANNEL_BELOW:
+                                /* only below is allowed, disable above */
+                                geo_ch->flags |= IEEE80211_CHAN_NO_FAT_ABOVE;
+                                break;
+                        case HT_IE_EXT_CHANNEL_NONE:
+                                /* fat not allowed: disable both*/
+                                geo_ch->flags |= (IEEE80211_CHAN_NO_FAT_ABOVE |
+                                                  IEEE80211_CHAN_NO_FAT_BELOW);
+                                break;
+                        case HT_IE_EXT_CHANNEL_MAX:
+                                /* both above and below are permitted */
+                                break;
+                        }
+
                         if (ch->max_power_avg > priv->max_channel_txpower_limit)
                                 priv->max_channel_txpower_limit =
                                     ch->max_power_avg;
@@ -492,12 +537,6 @@ static int iwlcore_init_geos(struct iwl_priv *priv)
                priv->bands[IEEE80211_BAND_2GHZ].n_channels,
                priv->bands[IEEE80211_BAND_5GHZ].n_channels);
 
-        if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
-                priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
-                        &priv->bands[IEEE80211_BAND_2GHZ];
-        if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
-                priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-                        &priv->bands[IEEE80211_BAND_5GHZ];
 
         set_bit(STATUS_GEO_CONFIGURED, &priv->status);
 
@@ -507,13 +546,12 @@ static int iwlcore_init_geos(struct iwl_priv *priv)
 /*
  * iwlcore_free_geos - undo allocations in iwlcore_init_geos
  */
-void iwlcore_free_geos(struct iwl_priv *priv)
+static void iwlcore_free_geos(struct iwl_priv *priv)
 {
         kfree(priv->ieee_channels);
         kfree(priv->ieee_rates);
         clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
 }
-EXPORT_SYMBOL(iwlcore_free_geos);
 
 #ifdef CONFIG_IWL4965_HT
 static u8 is_single_rx_stream(struct iwl_priv *priv)
@@ -567,7 +605,7 @@ EXPORT_SYMBOL(iwl_is_fat_tx_allowed);
 
 void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info)
 {
-        struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
+        struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
         u32 val;
 
         if (!ht_info->is_ht)
@@ -741,8 +779,9 @@ int iwl_set_rxon_channel(struct iwl_priv *priv,
 }
 EXPORT_SYMBOL(iwl_set_rxon_channel);
 
-static void iwlcore_init_hw(struct iwl_priv *priv)
+int iwl_setup_mac(struct iwl_priv *priv)
 {
+        int ret;
         struct ieee80211_hw *hw = priv->hw;
         hw->rate_control_algorithm = "iwl-4965-rs";
 
@@ -756,9 +795,29 @@ static void iwlcore_init_hw(struct iwl_priv *priv)
         /* Enhanced value; more queues, to support 11n aggregation */
         hw->ampdu_queues = 12;
 #endif /* CONFIG_IWL4965_HT */
+
+        hw->conf.beacon_int = 100;
+
+        if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
+                priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
+                        &priv->bands[IEEE80211_BAND_2GHZ];
+        if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
+                priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
+                        &priv->bands[IEEE80211_BAND_5GHZ];
+
+        ret = ieee80211_register_hw(priv->hw);
+        if (ret) {
+                IWL_ERROR("Failed to register hw (error %d)\n", ret);
+                return ret;
+        }
+        priv->mac80211_registered = 1;
+
+        return 0;
 }
+EXPORT_SYMBOL(iwl_setup_mac);
 
-static int iwlcore_init_drv(struct iwl_priv *priv)
+
+int iwl_init_drv(struct iwl_priv *priv)
 {
         int ret;
         int i;
@@ -795,6 +854,9 @@ static int iwlcore_init_drv(struct iwl_priv *priv)
         /* Choose which receivers/antennas to use */
         iwl_set_rxon_chain(priv);
 
+        if (priv->cfg->mod_params->enable_qos)
+                priv->qos_data.qos_enable = 1;
+
         iwl_reset_qos(priv);
 
         priv->qos_data.qos_active = 0;
@@ -819,34 +881,39 @@ static int iwlcore_init_drv(struct iwl_priv *priv)
                 goto err_free_channel_map;
         }
 
-        ret = ieee80211_register_hw(priv->hw);
-        if (ret) {
-                IWL_ERROR("Failed to register network device (error %d)\n",
-                                ret);
-                goto err_free_geos;
-        }
-
-        priv->hw->conf.beacon_int = 100;
-        priv->mac80211_registered = 1;
-
         return 0;
 
-err_free_geos:
-        iwlcore_free_geos(priv);
 err_free_channel_map:
         iwl_free_channel_map(priv);
 err:
         return ret;
 }
+EXPORT_SYMBOL(iwl_init_drv);
 
-int iwl_setup(struct iwl_priv *priv)
+void iwl_free_calib_results(struct iwl_priv *priv)
 {
-        int ret = 0;
-        iwlcore_init_hw(priv);
-        ret = iwlcore_init_drv(priv);
-        return ret;
+        kfree(priv->calib_results.lo_res);
+        priv->calib_results.lo_res = NULL;
+        priv->calib_results.lo_res_len = 0;
+
+        kfree(priv->calib_results.tx_iq_res);
+        priv->calib_results.tx_iq_res = NULL;
+        priv->calib_results.tx_iq_res_len = 0;
+
+        kfree(priv->calib_results.tx_iq_perd_res);
+        priv->calib_results.tx_iq_perd_res = NULL;
+        priv->calib_results.tx_iq_perd_res_len = 0;
+}
+EXPORT_SYMBOL(iwl_free_calib_results);
+
+void iwl_uninit_drv(struct iwl_priv *priv)
+{
+        iwl_free_calib_results(priv);
+        iwlcore_free_geos(priv);
+        iwl_free_channel_map(priv);
+        kfree(priv->scan);
 }
-EXPORT_SYMBOL(iwl_setup);
+EXPORT_SYMBOL(iwl_uninit_drv);
 
 /* Low level driver call this function to update iwlcore with
  * driver status.
@@ -1024,3 +1091,185 @@ int iwl_verify_ucode(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwl_verify_ucode);
 
+
+static const char *desc_lookup(int i)
+{
+        switch (i) {
+        case 1:
+                return "FAIL";
+        case 2:
+                return "BAD_PARAM";
+        case 3:
+                return "BAD_CHECKSUM";
+        case 4:
+                return "NMI_INTERRUPT";
+        case 5:
+                return "SYSASSERT";
+        case 6:
+                return "FATAL_ERROR";
+        }
+
+        return "UNKNOWN";
+}
+
+#define ERROR_START_OFFSET  (1 * sizeof(u32))
+#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
+
+void iwl_dump_nic_error_log(struct iwl_priv *priv)
+{
+        u32 data2, line;
+        u32 desc, time, count, base, data1;
+        u32 blink1, blink2, ilink1, ilink2;
+        int ret;
+
+        if (priv->ucode_type == UCODE_INIT)
+                base = le32_to_cpu(priv->card_alive_init.error_event_table_ptr);
+        else
+                base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
+
+        if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+                IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
+                return;
+        }
+
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
+                IWL_WARNING("Can not read from adapter at this time.\n");
+                return;
+        }
+
+        count = iwl_read_targ_mem(priv, base);
+
+        if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
+                IWL_ERROR("Start IWL Error Log Dump:\n");
+                IWL_ERROR("Status: 0x%08lX, count: %d\n", priv->status, count);
+        }
+
+        desc = iwl_read_targ_mem(priv, base + 1 * sizeof(u32));
+        blink1 = iwl_read_targ_mem(priv, base + 3 * sizeof(u32));
+        blink2 = iwl_read_targ_mem(priv, base + 4 * sizeof(u32));
+        ilink1 = iwl_read_targ_mem(priv, base + 5 * sizeof(u32));
+        ilink2 = iwl_read_targ_mem(priv, base + 6 * sizeof(u32));
+        data1 = iwl_read_targ_mem(priv, base + 7 * sizeof(u32));
+        data2 = iwl_read_targ_mem(priv, base + 8 * sizeof(u32));
+        line = iwl_read_targ_mem(priv, base + 9 * sizeof(u32));
+        time = iwl_read_targ_mem(priv, base + 11 * sizeof(u32));
+
+        IWL_ERROR("Desc        Time       "
+                "data1      data2      line\n");
+        IWL_ERROR("%-13s (#%d) %010u 0x%08X 0x%08X %u\n",
+                desc_lookup(desc), desc, time, data1, data2, line);
+        IWL_ERROR("blink1  blink2  ilink1  ilink2\n");
+        IWL_ERROR("0x%05X 0x%05X 0x%05X 0x%05X\n", blink1, blink2,
+                ilink1, ilink2);
+
+        iwl_release_nic_access(priv);
+}
+EXPORT_SYMBOL(iwl_dump_nic_error_log);
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl_print_event_log - Dump error event log to syslog
+ *
+ * NOTE: Must be called with iwl4965_grab_nic_access() already obtained!
+ */
+void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+                                u32 num_events, u32 mode)
+{
+        u32 i;
+        u32 base;       /* SRAM byte address of event log header */
+        u32 event_size; /* 2 u32s, or 3 u32s if timestamp recorded */
+        u32 ptr;        /* SRAM byte address of log data */
+        u32 ev, time, data; /* event log data */
+
+        if (num_events == 0)
+                return;
+        if (priv->ucode_type == UCODE_INIT)
+                base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+        else
+                base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+
+        if (mode == 0)
+                event_size = 2 * sizeof(u32);
+        else
+                event_size = 3 * sizeof(u32);
+
+        ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
+
+        /* "time" is actually "data" for mode 0 (no timestamp).
+        * place event id # at far right for easier visual parsing. */
+        for (i = 0; i < num_events; i++) {
+                ev = iwl_read_targ_mem(priv, ptr);
+                ptr += sizeof(u32);
+                time = iwl_read_targ_mem(priv, ptr);
+                ptr += sizeof(u32);
+                if (mode == 0)
+                        IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
+                else {
+                        data = iwl_read_targ_mem(priv, ptr);
+                        ptr += sizeof(u32);
+                        IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
+                }
+        }
+}
+EXPORT_SYMBOL(iwl_print_event_log);
+
+
+void iwl_dump_nic_event_log(struct iwl_priv *priv)
+{
+        int ret;
+        u32 base;       /* SRAM byte address of event log header */
+        u32 capacity;   /* event log capacity in # entries */
+        u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+        u32 num_wraps;  /* # times uCode wrapped to top of log */
+        u32 next_entry; /* index of next entry to be written by uCode */
+        u32 size;       /* # entries that we'll print */
+
+        if (priv->ucode_type == UCODE_INIT)
+                base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+        else
+                base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+
+        if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+                IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
+                return;
+        }
+
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
+                IWL_WARNING("Can not read from adapter at this time.\n");
+                return;
+        }
+
+        /* event log header */
+        capacity = iwl_read_targ_mem(priv, base);
+        mode = iwl_read_targ_mem(priv, base + (1 * sizeof(u32)));
+        num_wraps = iwl_read_targ_mem(priv, base + (2 * sizeof(u32)));
+        next_entry = iwl_read_targ_mem(priv, base + (3 * sizeof(u32)));
+
+        size = num_wraps ? capacity : next_entry;
+
+        /* bail out if nothing in log */
+        if (size == 0) {
+                IWL_ERROR("Start IWL Event Log Dump: nothing in log\n");
+                iwl_release_nic_access(priv);
+                return;
+        }
+
+        IWL_ERROR("Start IWL Event Log Dump: display count %d, wraps %d\n",
+                        size, num_wraps);
+
+        /* if uCode has wrapped back to top of log, start at the oldest entry,
+         * i.e the next one that uCode would fill. */
+        if (num_wraps)
+                iwl_print_event_log(priv, next_entry,
+                                        capacity - next_entry, mode);
+        /* (then/else) start at top of log */
+        iwl_print_event_log(priv, 0, next_entry, mode);
+
+        iwl_release_nic_access(priv);
+}
+EXPORT_SYMBOL(iwl_dump_nic_event_log);
+
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index e139c8ffa9a2..6b5af7afbb25 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -86,7 +86,7 @@ struct iwl_hcmd_ops {
         int (*rxon_assoc)(struct iwl_priv *priv);
 };
 struct iwl_hcmd_utils_ops {
-        int (*enqueue_hcmd)(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+        u16 (*get_hcmd_size)(u8 cmd_id, u16 len);
         u16 (*build_addsta_hcmd)(const struct iwl_addsta_cmd *cmd, u8 *data);
 #ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
         void (*gain_computation)(struct iwl_priv *priv,
@@ -104,13 +104,22 @@ struct iwl_lib_ops {
         int (*alloc_shared_mem)(struct iwl_priv *priv);
         void (*free_shared_mem)(struct iwl_priv *priv);
         int (*shared_mem_rx_idx)(struct iwl_priv *priv);
+        /* Handling TX */
         void (*txq_update_byte_cnt_tbl)(struct iwl_priv *priv,
                                         struct iwl_tx_queue *txq,
                                         u16 byte_cnt);
+        void (*txq_inval_byte_cnt_tbl)(struct iwl_priv *priv,
+                                       struct iwl_tx_queue *txq);
+        void (*txq_set_sched)(struct iwl_priv *priv, u32 mask);
+#ifdef CONFIG_IWL4965_HT
+        /* aggregations */
+        int (*txq_agg_enable)(struct iwl_priv *priv, int txq_id, int tx_fifo,
+                              int sta_id, int tid, u16 ssn_idx);
+        int (*txq_agg_disable)(struct iwl_priv *priv, u16 txq_id, u16 ssn_idx,
+                               u8 tx_fifo);
+#endif /* CONFIG_IWL4965_HT */
         /* setup Rx handler */
         void (*rx_handler_setup)(struct iwl_priv *priv);
-        /* nic Tx fifo handling */
-        int (*disable_tx_fifo)(struct iwl_priv *priv);
         /* alive notification after init uCode load */
         void (*init_alive_start)(struct iwl_priv *priv);
         /* alive notification */
@@ -124,6 +133,8 @@ struct iwl_lib_ops {
          /* power management */
         struct {
                 int (*init)(struct iwl_priv *priv);
+                int (*reset)(struct iwl_priv *priv);
+                void (*stop)(struct iwl_priv *priv);
                 void (*config)(struct iwl_priv *priv);
                 int (*set_pwr_src)(struct iwl_priv *priv, enum iwl_pwr_src src);
         } apm_ops;
@@ -170,18 +181,19 @@ struct ieee80211_hw *iwl_alloc_all(struct iwl_cfg *cfg,
 void iwl_hw_detect(struct iwl_priv *priv);
 
 void iwlcore_clear_stations_table(struct iwl_priv *priv);
+void iwl_free_calib_results(struct iwl_priv *priv);
 void iwl_reset_qos(struct iwl_priv *priv);
 void iwl_set_rxon_chain(struct iwl_priv *priv);
 int iwl_set_rxon_channel(struct iwl_priv *priv,
                                 enum ieee80211_band band,
                                 u16 channel);
-void iwlcore_free_geos(struct iwl_priv *priv);
-int iwl_setup(struct iwl_priv *priv);
 void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info);
 u8 iwl_is_fat_tx_allowed(struct iwl_priv *priv,
                          struct ieee80211_ht_info *sta_ht_inf);
 int iwl_hw_nic_init(struct iwl_priv *priv);
-
+int iwl_setup_mac(struct iwl_priv *priv);
+int iwl_init_drv(struct iwl_priv *priv);
+void iwl_uninit_drv(struct iwl_priv *priv);
 /* "keep warm" functions */
 int iwl_kw_init(struct iwl_priv *priv);
 int iwl_kw_alloc(struct iwl_priv *priv);
@@ -202,14 +214,30 @@ int iwl_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
 int iwl_rx_queue_restock(struct iwl_priv *priv);
 int iwl_rx_queue_space(const struct iwl_rx_queue *q);
 void iwl_rx_allocate(struct iwl_priv *priv);
+void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
+int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
+/* Handlers */
+void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+                               struct iwl_rx_mem_buffer *rxb);
+
+/* TX helpers */
 
 /*****************************************************
 * TX
 ******************************************************/
 int iwl_txq_ctx_reset(struct iwl_priv *priv);
+int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb);
 /* FIXME: remove when free Tx is fully merged into iwlcore */
 int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq);
 void iwl_hw_txq_ctx_free(struct iwl_priv *priv);
+int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *tfd,
+                                        dma_addr_t addr, u16 len);
+int iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+#ifdef CONFIG_IWL4965_HT
+int iwl_tx_agg_start(struct iwl_priv *priv, const u8 *ra, u16 tid, u16 *ssn);
+int iwl_tx_agg_stop(struct iwl_priv *priv , const u8 *ra, u16 tid);
+int iwl_txq_check_empty(struct iwl_priv *priv, int sta_id, u8 tid, int txq_id);
+#endif
 
 /*****************************************************
  *   S e n d i n g     H o s t     C o m m a n d s   *
@@ -226,6 +254,17 @@ int iwl_send_cmd_pdu_async(struct iwl_priv *priv, u8 id, u16 len,
                            int (*callback)(struct iwl_priv *priv,
                                            struct iwl_cmd *cmd,
                                            struct sk_buff *skb));
+
+int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+
+/*****************************************************
+*  Error Handling Debugging
+******************************************************/
+void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+                         u32 num_events, u32 mode);
+void iwl_dump_nic_error_log(struct iwl_priv *priv);
+void iwl_dump_nic_event_log(struct iwl_priv *priv);
+
 /*************** DRIVER STATUS FUNCTIONS   *****/
 
 #define STATUS_HCMD_ACTIVE      0       /* host command in progress */
@@ -303,5 +342,10 @@ static inline int iwl_send_rxon_assoc(struct iwl_priv *priv)
         return priv->cfg->ops->hcmd->rxon_assoc(priv);
 }
 
+static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
+                        struct iwl_priv *priv, enum ieee80211_band band)
+{
+        return priv->hw->wiphy->bands[band];
+}
 
 #endif /* __iwl_core_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 9d6e5d2072d2..545ed692d889 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -87,13 +87,14 @@
 /* EEPROM reads */
 #define CSR_EEPROM_REG          (CSR_BASE+0x02c)
 #define CSR_EEPROM_GP           (CSR_BASE+0x030)
+#define CSR_GIO_REG             (CSR_BASE+0x03C)
 #define CSR_GP_UCODE            (CSR_BASE+0x044)
 #define CSR_UCODE_DRV_GP1       (CSR_BASE+0x054)
 #define CSR_UCODE_DRV_GP1_SET   (CSR_BASE+0x058)
 #define CSR_UCODE_DRV_GP1_CLR   (CSR_BASE+0x05c)
 #define CSR_UCODE_DRV_GP2       (CSR_BASE+0x060)
-#define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
 #define CSR_LED_REG             (CSR_BASE+0x094)
+#define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
 
 /* Analog phase-lock-loop configuration  */
 #define CSR_ANA_PLL_CFG         (CSR_BASE+0x20c)
@@ -213,6 +214,9 @@
 #define CSR_EEPROM_GP_BAD_SIGNATURE     (0x00000000)
 #define CSR_EEPROM_GP_IF_OWNER_MSK      (0x00000180)
 
+/* CSR GIO */
+#define CSR_GIO_REG_VAL_L0S_ENABLED     (0x00000002)
+
 /* UCODE DRV GP */
 #define CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP             (0x00000001)
 #define CSR_UCODE_SW_BIT_RFKILL                     (0x00000002)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index 2f24594c5fea..11de561c7bf8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -45,13 +45,21 @@ struct iwl_debugfs {
         const char *name;
         struct dentry *dir_drv;
         struct dentry *dir_data;
-        struct dir_data_files{
+        struct dentry *dir_rf;
+        struct dir_data_files {
                 struct dentry *file_sram;
                 struct dentry *file_eeprom;
                 struct dentry *file_stations;
                 struct dentry *file_rx_statistics;
                 struct dentry *file_tx_statistics;
+                struct dentry *file_log_event;
         } dbgfs_data_files;
+        struct dir_rf_files {
+#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
+                struct dentry *file_disable_sensitivity;
+                struct dentry *file_disable_chain_noise;
+#endif /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
+        } dbgfs_rf_files;
         u32 sram_offset;
         u32 sram_len;
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
index ad25806dfaf1..29e16ba69cdb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -55,6 +55,13 @@
                 goto err;                                               \
 } while (0)
 
+#define DEBUGFS_ADD_BOOL(name, parent, ptr) do {                        \
+        dbgfs->dbgfs_##parent##_files.file_##name =                     \
+        debugfs_create_bool(#name, 0644, dbgfs->dir_##parent, ptr);     \
+        if (IS_ERR(dbgfs->dbgfs_##parent##_files.file_##name))          \
+                goto err;                                               \
+} while (0)
+
 #define DEBUGFS_REMOVE(name)  do {              \
         debugfs_remove(name);                   \
         name = NULL;                            \
@@ -85,6 +92,14 @@ static const struct file_operations iwl_dbgfs_##name##_ops = {          \
         .open = iwl_dbgfs_open_file_generic,                            \
 };
 
+#define DEBUGFS_WRITE_FILE_OPS(name)                                    \
+        DEBUGFS_WRITE_FUNC(name);                                       \
+static const struct file_operations iwl_dbgfs_##name##_ops = {          \
+        .write = iwl_dbgfs_##name##_write,                              \
+        .open = iwl_dbgfs_open_file_generic,                            \
+};
+
+
 #define DEBUGFS_READ_WRITE_FILE_OPS(name)                               \
         DEBUGFS_READ_FUNC(name);                                        \
         DEBUGFS_WRITE_FUNC(name);                                       \
@@ -317,7 +332,29 @@ static ssize_t iwl_dbgfs_eeprom_read(struct file *file,
         return ret;
 }
 
+static ssize_t iwl_dbgfs_log_event_write(struct file *file,
+                                        const char __user *user_buf,
+                                        size_t count, loff_t *ppos)
+{
+        struct iwl_priv *priv = file->private_data;
+        u32 event_log_flag;
+        char buf[8];
+        int buf_size;
+
+        memset(buf, 0, sizeof(buf));
+        buf_size = min(count, sizeof(buf) -  1);
+        if (copy_from_user(buf, user_buf, buf_size))
+                return -EFAULT;
+        if (sscanf(buf, "%d", &event_log_flag) != 1)
+                return -EFAULT;
+        if (event_log_flag == 1)
+                iwl_dump_nic_event_log(priv);
+
+        return count;
+}
+
 DEBUGFS_READ_WRITE_FILE_OPS(sram);
+DEBUGFS_WRITE_FILE_OPS(log_event);
 DEBUGFS_READ_FILE_OPS(eeprom);
 DEBUGFS_READ_FILE_OPS(stations);
 DEBUGFS_READ_FILE_OPS(rx_statistics);
@@ -330,6 +367,7 @@ DEBUGFS_READ_FILE_OPS(tx_statistics);
 int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 {
         struct iwl_debugfs *dbgfs;
+        struct dentry *phyd = priv->hw->wiphy->debugfsdir;
 
         dbgfs = kzalloc(sizeof(struct iwl_debugfs), GFP_KERNEL);
         if (!dbgfs) {
@@ -338,18 +376,24 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 
         priv->dbgfs = dbgfs;
         dbgfs->name = name;
-        dbgfs->dir_drv = debugfs_create_dir(name, NULL);
+        dbgfs->dir_drv = debugfs_create_dir(name, phyd);
         if (!dbgfs->dir_drv || IS_ERR(dbgfs->dir_drv)){
                 goto err;
         }
 
         DEBUGFS_ADD_DIR(data, dbgfs->dir_drv);
+        DEBUGFS_ADD_DIR(rf, dbgfs->dir_drv);
         DEBUGFS_ADD_FILE(eeprom, data);
         DEBUGFS_ADD_FILE(sram, data);
+        DEBUGFS_ADD_FILE(log_event, data);
         DEBUGFS_ADD_FILE(stations, data);
         DEBUGFS_ADD_FILE(rx_statistics, data);
         DEBUGFS_ADD_FILE(tx_statistics, data);
-
+#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
+        DEBUGFS_ADD_BOOL(disable_sensitivity, rf, &priv->disable_sens_cal);
+        DEBUGFS_ADD_BOOL(disable_chain_noise, rf,
+                         &priv->disable_chain_noise_cal);
+#endif  /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
         return 0;
 
 err:
@@ -372,8 +416,14 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
         DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_rx_statistics);
         DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_tx_statistics);
         DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_sram);
+        DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_log_event);
         DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_stations);
         DEBUGFS_REMOVE(priv->dbgfs->dir_data);
+#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
+        DEBUGFS_REMOVE(priv->dbgfs->dbgfs_rf_files.file_disable_sensitivity);
+        DEBUGFS_REMOVE(priv->dbgfs->dbgfs_rf_files.file_disable_chain_noise);
+#endif /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
+        DEBUGFS_REMOVE(priv->dbgfs->dir_rf);
         DEBUGFS_REMOVE(priv->dbgfs->dir_drv);
         kfree(priv->dbgfs);
         priv->dbgfs = NULL;
@@ -381,3 +431,4 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
 EXPORT_SYMBOL(iwl_dbgfs_unregister);
 
 
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 5dccc5a8fa94..802f1a12b1aa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -102,7 +102,7 @@ struct iwl_rx_mem_buffer {
  *
  * Contains common data for Rx and Tx queues
  */
-struct iwl4965_queue {
+struct iwl_queue {
         int n_bd;              /* number of BDs in this queue */
         int write_ptr;       /* 1-st empty entry (index) host_w*/
         int read_ptr;         /* last used entry (index) host_r*/
@@ -118,8 +118,7 @@ struct iwl4965_queue {
 #define MAX_NUM_OF_TBS          (20)
 
 /* One for each TFD */
-struct iwl4965_tx_info {
-        struct ieee80211_tx_status status;
+struct iwl_tx_info {
         struct sk_buff *skb[MAX_NUM_OF_TBS];
 };
 
@@ -137,11 +136,11 @@ struct iwl4965_tx_info {
  * descriptors) and required locking structures.
  */
 struct iwl_tx_queue {
-        struct iwl4965_queue q;
+        struct iwl_queue q;
         struct iwl_tfd_frame *bd;
         struct iwl_cmd *cmd;
         dma_addr_t dma_addr_cmd;
-        struct iwl4965_tx_info *txb;
+        struct iwl_tx_info *txb;
         int need_update;
         int sched_retry;
         int active;
@@ -262,7 +261,7 @@ enum iwl_pwr_src {
 #define IEEE80211_HLEN                  (IEEE80211_4ADDR_LEN)
 #define IEEE80211_FRAME_LEN             (IEEE80211_DATA_LEN + IEEE80211_HLEN)
 
-struct iwl4965_frame {
+struct iwl_frame {
         union {
                 struct ieee80211_hdr frame;
                 struct iwl4965_tx_beacon_cmd beacon;
@@ -308,6 +307,8 @@ struct iwl_cmd_meta {
 
 } __attribute__ ((packed));
 
+#define IWL_CMD_MAX_PAYLOAD 320
+
 /**
  * struct iwl_cmd
  *
@@ -329,11 +330,12 @@ struct iwl_cmd {
                 struct iwl4965_rxon_time_cmd rxon_time;
                 struct iwl4965_powertable_cmd powertable;
                 struct iwl4965_qosparam_cmd qosparam;
-                struct iwl4965_tx_cmd tx;
+                struct iwl_tx_cmd tx;
                 struct iwl4965_tx_beacon_cmd tx_beacon;
                 struct iwl4965_rxon_assoc_cmd rxon_assoc;
+                struct iwl_rem_sta_cmd rm_sta;
                 u8 *indirect;
-                u8 payload[360];
+                u8 payload[IWL_CMD_MAX_PAYLOAD];
         } __attribute__ ((packed)) cmd;
 } __attribute__ ((packed));
 
@@ -442,7 +444,6 @@ struct iwl_hw_key {
         enum ieee80211_key_alg alg;
         int keylen;
         u8 keyidx;
-        struct ieee80211_key_conf *conf;
         u8 key[32];
 };
 
@@ -573,7 +574,6 @@ struct iwl_sensitivity_ranges {
 /**
  * struct iwl_hw_params
  * @max_txq_num: Max # Tx queues supported
- * @tx_cmd_len: Size of Tx command (but not including frame itself)
  * @tx/rx_chains_num: Number of TX/RX chains
  * @valid_tx/rx_ant: usable antennas
  * @max_rxq_size: Max # Rx frames in Rx queue (must be power-of-2)
@@ -590,7 +590,6 @@ struct iwl_sensitivity_ranges {
  */
 struct iwl_hw_params {
         u16 max_txq_num;
-        u16 tx_cmd_len;
         u8  tx_chains_num;
         u8  rx_chains_num;
         u8  valid_tx_ant;
@@ -612,8 +611,8 @@ struct iwl_hw_params {
 #endif
 };
 
-#define HT_SHORT_GI_20MHZ_ONLY  (1 << 0)
-#define HT_SHORT_GI_40MHZ_ONLY  (1 << 1)
+#define HT_SHORT_GI_20MHZ       (1 << 0)
+#define HT_SHORT_GI_40MHZ       (1 << 1)
 
 
 #define IWL_RX_HDR(x) ((struct iwl4965_rx_frame_hdr *)(\
@@ -635,8 +634,8 @@ struct iwl_hw_params {
 struct iwl_addsta_cmd;
 extern int iwl_send_add_sta(struct iwl_priv *priv,
                             struct iwl_addsta_cmd *sta, u8 flags);
-extern u8 iwl4965_add_station_flags(struct iwl_priv *priv, const u8 *addr,
-                          int is_ap, u8 flags, void *ht_data);
+u8 iwl_add_station_flags(struct iwl_priv *priv, const u8 *addr, int is_ap,
+                         u8 flags, struct ieee80211_ht_info *ht_info);
 extern int iwl4965_is_network_packet(struct iwl_priv *priv,
                                  struct ieee80211_hdr *header);
 extern int iwl4965_power_init_handle(struct iwl_priv *priv);
@@ -652,14 +651,13 @@ extern int iwl4965_calc_sig_qual(int rssi_dbm, int noise_dbm);
 extern unsigned int iwl4965_fill_beacon_frame(struct iwl_priv *priv,
                                         struct ieee80211_hdr *hdr,
                                         const u8 *dest, int left);
-extern __le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr);
 extern void iwl4965_update_chain_flags(struct iwl_priv *priv);
 int iwl4965_set_pwr_src(struct iwl_priv *priv, enum iwl_pwr_src src);
 
 int iwl4965_init_geos(struct iwl_priv *priv);
 void iwl4965_free_geos(struct iwl_priv *priv);
 
-extern const u8 iwl4965_broadcast_addr[ETH_ALEN];
+extern const u8 iwl_bcast_addr[ETH_ALEN];
 int iwl4965_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
 
 /*
@@ -687,19 +685,15 @@ extern u8 iwl4965_sync_station(struct iwl_priv *priv, int sta_id,
  ****************************************************************************/
 extern void iwl4965_hw_setup_deferred_work(struct iwl_priv *priv);
 extern void iwl4965_hw_cancel_deferred_work(struct iwl_priv *priv);
-extern int iwl4965_hw_rxq_stop(struct iwl_priv *priv);
 extern int iwl4965_hw_set_hw_params(struct iwl_priv *priv);
-extern int iwl4965_hw_nic_stop_master(struct iwl_priv *priv);
-extern void iwl4965_hw_txq_ctx_stop(struct iwl_priv *priv);
-extern int iwl4965_hw_nic_reset(struct iwl_priv *priv);
-extern int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *tfd,
-                                        dma_addr_t addr, u16 len);
+extern int iwl_rxq_stop(struct iwl_priv *priv);
+extern void iwl_txq_ctx_stop(struct iwl_priv *priv);
 extern int iwl4965_hw_get_temperature(struct iwl_priv *priv);
 extern unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
-                                 struct iwl4965_frame *frame, u8 rate);
+                                 struct iwl_frame *frame, u8 rate);
 extern void iwl4965_hw_build_tx_cmd_rate(struct iwl_priv *priv,
                                      struct iwl_cmd *cmd,
-                                     struct ieee80211_tx_control *ctrl,
+                                     struct ieee80211_tx_info *info,
                                      struct ieee80211_hdr *hdr,
                                      int sta_id, int tx_id);
 extern int iwl4965_hw_reg_send_txpower(struct iwl_priv *priv);
@@ -708,6 +702,8 @@ extern void iwl4965_hw_rx_statistics(struct iwl_priv *priv,
                                  struct iwl_rx_mem_buffer *rxb);
 extern void iwl4965_disable_events(struct iwl_priv *priv);
 extern int iwl4965_get_temperature(const struct iwl_priv *priv);
+extern void iwl4965_rx_reply_rx(struct iwl_priv *priv,
+                                struct iwl_rx_mem_buffer *rxb);
 
 /**
  * iwl_find_station - Find station id for a given BSSID
@@ -720,8 +716,26 @@ extern int iwl4965_get_temperature(const struct iwl_priv *priv);
 extern u8 iwl_find_station(struct iwl_priv *priv, const u8 *bssid);
 
 extern int iwl4965_hw_channel_switch(struct iwl_priv *priv, u16 channel);
-extern int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
-extern int iwl4965_queue_space(const struct iwl4965_queue *q);
+extern int iwl_queue_space(const struct iwl_queue *q);
+static inline int iwl_queue_used(const struct iwl_queue *q, int i)
+{
+        return q->write_ptr > q->read_ptr ?
+                (i >= q->read_ptr && i < q->write_ptr) :
+                !(i < q->read_ptr && i >= q->write_ptr);
+}
+
+
+static inline u8 get_cmd_index(struct iwl_queue *q, u32 index, int is_huge)
+{
+        /* This is for scan command, the big buffer at end of command array */
+        if (is_huge)
+                return q->n_window;     /* must be power of 2 */
+
+        /* Otherwise, use normal size buffers */
+        return index & (q->n_window - 1);
+}
+
+
 struct iwl_priv;
 
 extern void iwl4965_radio_kill_sw(struct iwl_priv *priv, int disable_radio);
@@ -731,14 +745,12 @@ extern void iwl4965_radio_kill_sw(struct iwl_priv *priv, int disable_radio);
 extern int iwl4965_tx_queue_update_wr_ptr(struct iwl_priv *priv,
                                           struct iwl_tx_queue *txq,
                                           u16 byte_cnt);
-extern void iwl4965_add_station(struct iwl_priv *priv, const u8 *addr,
-                                int is_ap);
 extern int iwl4965_alive_notify(struct iwl_priv *priv);
 extern void iwl4965_update_rate_scaling(struct iwl_priv *priv, u8 mode);
 extern void iwl4965_rf_kill_ct_config(struct iwl_priv *priv);
 extern void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv,
                                          u32 rate_n_flags,
-                                         struct ieee80211_tx_control *control);
+                                         struct ieee80211_tx_info *info);
 
 #ifdef CONFIG_IWL4965_HT
 extern void iwl4965_init_ht_hw_capab(const struct iwl_priv *priv,
@@ -746,8 +758,6 @@ extern void iwl4965_init_ht_hw_capab(const struct iwl_priv *priv,
                                 enum ieee80211_band band);
 void iwl4965_set_rxon_ht(struct iwl_priv *priv,
                          struct iwl_ht_info *ht_info);
-void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index,
-                                struct ieee80211_ht_info *sta_ht_inf);
 int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
                                     enum ieee80211_ampdu_mlme_action action,
                                     const u8 *addr, u16 tid, u16 *ssn);
@@ -867,6 +877,21 @@ struct statistics_general_data {
         u32 beacon_energy_c;
 };
 
+struct iwl_calib_results {
+        void *tx_iq_res;
+        void *tx_iq_perd_res;
+        void *lo_res;
+        u32 tx_iq_res_len;
+        u32 tx_iq_perd_res_len;
+        u32 lo_res_len;
+};
+
+enum ucode_type {
+        UCODE_NONE = 0,
+        UCODE_INIT,
+        UCODE_RT
+};
+
 #ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
 /* Sensitivity calib data */
 struct iwl_sensitivity_data {
@@ -968,6 +993,9 @@ struct iwl_priv {
         s32 temperature;        /* degrees Kelvin */
         s32 last_temperature;
 
+        /* init calibration results */
+        struct iwl_calib_results calib_results;
+
         /* Scan related variables */
         unsigned long last_scan_jiffies;
         unsigned long next_scan_jiffies;
@@ -1001,6 +1029,8 @@ struct iwl_priv {
         struct fw_desc ucode_init;      /* initialization inst */
         struct fw_desc ucode_init_data; /* initialization data */
         struct fw_desc ucode_boot;      /* bootstrap inst */
+        enum ucode_type ucode_type;
+        u8 ucode_write_complete;        /* the image write is complete */
 
 
         struct iwl4965_rxon_time_cmd rxon_timing;
@@ -1009,16 +1039,16 @@ struct iwl_priv {
          * changed via explicit cast within the
          * routines that actually update the physical
          * hardware */
-        const struct iwl4965_rxon_cmd active_rxon;
-        struct iwl4965_rxon_cmd staging_rxon;
+        const struct iwl_rxon_cmd active_rxon;
+        struct iwl_rxon_cmd staging_rxon;
 
         int error_recovering;
-        struct iwl4965_rxon_cmd recovery_rxon;
+        struct iwl_rxon_cmd recovery_rxon;
 
         /* 1st responses from initialize and runtime uCode images.
          * 4965's initialize alive response contains some calibration data. */
-        struct iwl4965_init_alive_resp card_alive_init;
-        struct iwl4965_alive_resp card_alive;
+        struct iwl_init_alive_resp card_alive_init;
+        struct iwl_alive_resp card_alive;
 #ifdef CONFIG_IWLWIFI_RFKILL
         struct iwl_rfkill_mngr rfkill_mngr;
 #endif
@@ -1107,8 +1137,6 @@ struct iwl_priv {
 
         u8 mac80211_registered;
 
-        u32 notif_missed_beacons;
-
         /* Rx'd packet timing information */
         u32 last_beacon_time;
         u64 last_tsf;
@@ -1195,12 +1223,56 @@ struct iwl_priv {
 #endif /* CONFIG_IWLWIFI_DEBUG */
 
         struct work_struct txpower_work;
+#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
+        u32 disable_sens_cal;
+        u32 disable_chain_noise_cal;
+#endif /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
 #ifdef CONFIG_IWL4965_RUN_TIME_CALIB
         struct work_struct sensitivity_work;
-#endif
+#endif /* CONFIG_IWL4965_RUN_TIME_CALIB */
         struct timer_list statistics_periodic;
 }; /*iwl_priv */
 
+static inline void iwl_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
+{
+        set_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+static inline void iwl_txq_ctx_deactivate(struct iwl_priv *priv, int txq_id)
+{
+        clear_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+#ifdef CONFIG_IWLWIF_DEBUG
+const char *iwl_get_tx_fail_reason(u32 status);
+#else
+static inline const char *iwl_get_tx_fail_reason(u32 status) { return ""; }
+#endif
+
+
+#ifdef CONFIG_IWL4965_HT
+static inline int iwl_get_ra_sta_id(struct iwl_priv *priv,
+                                    struct ieee80211_hdr *hdr)
+{
+        if (priv->iw_mode == IEEE80211_IF_TYPE_STA) {
+                return IWL_AP_ID;
+        } else {
+                u8 *da = ieee80211_get_DA(hdr);
+                return iwl_find_station(priv, da);
+        }
+}
+
+static inline struct ieee80211_hdr *iwl_tx_queue_get_hdr(struct iwl_priv *priv,
+                                                         int txq_id, int idx)
+{
+        if (priv->txq[txq_id].txb[idx].skb[0])
+                return (struct ieee80211_hdr *)priv->txq[txq_id].
+                                txb[idx].skb[0]->data;
+        return NULL;
+}
+#endif
+
+
 static inline int iwl_is_associated(struct iwl_priv *priv)
 {
         return (priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ? 1 : 0;
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
index fa306601a550..11f9d9557a0e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
@@ -365,11 +365,11 @@ static void iwl_init_band_reference(const struct iwl_priv *priv,
                             ? # x " " : "")
 
 /**
- * iwl4965_set_fat_chan_info - Copy fat channel info into driver's priv.
+ * iwl_set_fat_chan_info - Copy fat channel info into driver's priv.
  *
  * Does not set up a command, or touch hardware.
  */
-static int iwl4965_set_fat_chan_info(struct iwl_priv *priv,
+static int iwl_set_fat_chan_info(struct iwl_priv *priv,
                               enum ieee80211_band band, u16 channel,
                               const struct iwl_eeprom_channel *eeprom_ch,
                               u8 fat_extension_channel)
@@ -542,16 +542,16 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                                 fat_extension_chan = HT_IE_EXT_CHANNEL_ABOVE;
 
                         /* Set up driver's info for lower half */
-                        iwl4965_set_fat_chan_info(priv, ieeeband,
-                                                  eeprom_ch_index[ch],
-                                                  &(eeprom_ch_info[ch]),
-                                                  fat_extension_chan);
+                        iwl_set_fat_chan_info(priv, ieeeband,
+                                                eeprom_ch_index[ch],
+                                                &(eeprom_ch_info[ch]),
+                                                fat_extension_chan);
 
                         /* Set up driver's info for upper half */
-                        iwl4965_set_fat_chan_info(priv, ieeeband,
-                                                  (eeprom_ch_index[ch] + 4),
-                                                  &(eeprom_ch_info[ch]),
-                                                  HT_IE_EXT_CHANNEL_BELOW);
+                        iwl_set_fat_chan_info(priv, ieeeband,
+                                                (eeprom_ch_index[ch] + 4),
+                                                &(eeprom_ch_info[ch]),
+                                                HT_IE_EXT_CHANNEL_BELOW);
                 }
         }
 
@@ -560,23 +560,21 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 EXPORT_SYMBOL(iwl_init_channel_map);
 
 /*
- * iwl_free_channel_map - undo allocations in iwl4965_init_channel_map
+ * iwl_free_channel_map - undo allocations in iwl_init_channel_map
  */
 void iwl_free_channel_map(struct iwl_priv *priv)
 {
         kfree(priv->channel_info);
         priv->channel_count = 0;
 }
-EXPORT_SYMBOL(iwl_free_channel_map);
 
 /**
  * iwl_get_channel_info - Find driver's private channel info
  *
  * Based on band and channel number.
  */
-const struct iwl_channel_info *iwl_get_channel_info(
-                const struct iwl_priv *priv,
-                enum ieee80211_band band, u16 channel)
+const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
+                                        enum ieee80211_band band, u16 channel)
 {
         int i;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
index dc1f027c66a0..d3a2a5b4ac56 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
@@ -146,6 +146,7 @@ struct iwl_eeprom_channel {
 
 /*5000 calibrations */
 #define EEPROM_5000_CALIB_ALL   (INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
+#define EEPROM_5000_XTAL        ((2*0x128) | EEPROM_5000_CALIB_ALL)
 
 /* 5000 links */
 #define EEPROM_5000_LINK_HOST             (2*0x64)
diff --git a/drivers/net/wireless/iwlwifi/iwl-hcmd.c b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
index 0412adf6ef8b..6c537360820b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-hcmd.c
+++ b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
@@ -56,6 +56,7 @@ const char *get_cmd_string(u8 cmd)
                 IWL_CMD(REPLY_RATE_SCALE);
                 IWL_CMD(REPLY_LEDS_CMD);
                 IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
+                IWL_CMD(COEX_PRIORITY_TABLE_CMD);
                 IWL_CMD(RADAR_NOTIFICATION);
                 IWL_CMD(REPLY_QUIET_CMD);
                 IWL_CMD(REPLY_CHANNEL_SWITCH);
@@ -89,6 +90,9 @@ const char *get_cmd_string(u8 cmd)
                 IWL_CMD(REPLY_RX_MPDU_CMD);
                 IWL_CMD(REPLY_RX);
                 IWL_CMD(REPLY_COMPRESSED_BA);
+                IWL_CMD(CALIBRATION_CFG_CMD);
+                IWL_CMD(CALIBRATION_RES_NOTIFICATION);
+                IWL_CMD(CALIBRATION_COMPLETE_NOTIFICATION);
         default:
                 return "UNKNOWN";
 
@@ -139,7 +143,7 @@ static int iwl_send_cmd_async(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
         if (test_bit(STATUS_EXIT_PENDING, &priv->status))
                 return -EBUSY;
 
-        ret = priv->cfg->ops->utils->enqueue_hcmd(priv, cmd);
+        ret = iwl_enqueue_hcmd(priv, cmd);
         if (ret < 0) {
                 IWL_ERROR("Error sending %s: enqueue_hcmd failed: %d\n",
                           get_cmd_string(cmd->id), ret);
@@ -170,7 +174,7 @@ int iwl_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
         if (cmd->meta.flags & CMD_WANT_SKB)
                 cmd->meta.source = &cmd->meta;
 
-        cmd_idx = priv->cfg->ops->utils->enqueue_hcmd(priv, cmd);
+        cmd_idx = iwl_enqueue_hcmd(priv, cmd);
         if (cmd_idx < 0) {
                 ret = cmd_idx;
                 IWL_ERROR("Error sending %s: enqueue_hcmd failed: %d\n",
diff --git a/drivers/net/wireless/iwlwifi/iwl-helpers.h b/drivers/net/wireless/iwlwifi/iwl-helpers.h
index a443472bea62..dedefa06ad8f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-helpers.h
+++ b/drivers/net/wireless/iwlwifi/iwl-helpers.h
@@ -136,6 +136,8 @@ static inline void iwl_set_bits16(__le16 *dst, u8 pos, u8 len, int val)
 
 #define KELVIN_TO_CELSIUS(x) ((x)-273)
 #define CELSIUS_TO_KELVIN(x) ((x)+273)
+#define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
+
 
 #define IEEE80211_CHAN_W_RADAR_DETECT 0x00000010
 
@@ -235,6 +237,25 @@ static inline int ieee80211_is_reassoc_response(u16 fc)
                ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_RESP);
 }
 
+static inline int ieee80211_is_qos_data(u16 fc)
+{
+        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
+               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_QOS_DATA);
+}
+/**
+ * ieee80211_get_qos_ctrl - get pointer to the QoS control field
+ *
+ * This function returns the pointer to 802.11 header QoS field (2 bytes)
+ * This function doesn't check whether hdr is a QoS hdr, use with care
+ * @hdr: struct ieee80211_hdr *hdr
+ * @hdr_len: header length
+ */
+
+static inline u8 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr, int hdr_len)
+{
+        return  ((u8 *) hdr + hdr_len - QOS_CONTROL_LEN);
+}
+
 static inline int iwl_check_bits(unsigned long field, unsigned long mask)
 {
         return ((field & mask) == mask) ? 1 : 0;
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index acac629386e0..70d9c7568b98 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -358,11 +358,6 @@
  *  7- 0:  Enable (1), disable (0), one bit for each channel 0-7
  */
 #define IWL49_SCD_TXFACT                   (IWL49_SCD_START_OFFSET + 0x1c)
-
-/* Mask to enable contiguous Tx DMA/FIFO channels between "lo" and "hi". */
-#define SCD_TXFACT_REG_TXFIFO_MASK(lo, hi) \
-       ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
-
 /*
  * Queue (x) Write Pointers (indexes, really!), one for each Tx queue.
  * Initialized and updated by driver as new TFDs are added to queue.
@@ -512,11 +507,39 @@
 #define IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
         ((IWL49_SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffffffc)
 
-#define IWL49_SCD_TXFIFO_POS_TID                (0)
-#define IWL49_SCD_TXFIFO_POS_RA                 (4)
-#define IWL49_SCD_QUEUE_RA_TID_MAP_RATID_MSK    (0x01FF)
+#define IWL_SCD_TXFIFO_POS_TID                  (0)
+#define IWL_SCD_TXFIFO_POS_RA                   (4)
+#define IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK      (0x01FF)
 
 /* 5000 SCD */
+#define IWL50_SCD_QUEUE_STTS_REG_POS_TXF        (0)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE     (3)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_WSL        (4)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN (19)
+#define IWL50_SCD_QUEUE_STTS_REG_MSK            (0x00FF0000)
+
+#define IWL50_SCD_QUEUE_CTX_REG1_CREDIT_POS             (8)
+#define IWL50_SCD_QUEUE_CTX_REG1_CREDIT_MSK             (0x00FFFF00)
+#define IWL50_SCD_QUEUE_CTX_REG1_SUPER_CREDIT_POS       (24)
+#define IWL50_SCD_QUEUE_CTX_REG1_SUPER_CREDIT_MSK       (0xFF000000)
+#define IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_POS           (0)
+#define IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK           (0x0000007F)
+#define IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS        (16)
+#define IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK        (0x007F0000)
+
+#define IWL50_SCD_CONTEXT_DATA_OFFSET           (0x600)
+#define IWL50_SCD_TX_STTS_BITMAP_OFFSET         (0x7B1)
+#define IWL50_SCD_TRANSLATE_TBL_OFFSET          (0x7E0)
+
+#define IWL50_SCD_CONTEXT_QUEUE_OFFSET(x)\
+        (IWL50_SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
+
+#define IWL50_SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
+        ((IWL50_SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffc)
+
+#define IWL50_SCD_QUEUECHAIN_SEL_ALL(x)         (((1<<(x)) - 1) &\
+        (~(1<<IWL_CMD_QUEUE_NUM)))
+
 #define IWL50_SCD_BASE                  (PRPH_BASE + 0xa02c00)
 
 #define IWL50_SCD_SRAM_BASE_ADDR         (IWL50_SCD_BASE + 0x0)
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
index a2eb90d40b7e..cc61c937320f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -33,6 +33,7 @@
 #include "iwl-core.h"
 #include "iwl-sta.h"
 #include "iwl-io.h"
+#include "iwl-calib.h"
 #include "iwl-helpers.h"
 /************************** RX-FUNCTIONS ****************************/
 /*
@@ -420,3 +421,50 @@ int iwl_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
         return 0;
 }
 
+int iwl_rxq_stop(struct iwl_priv *priv)
+{
+        int ret;
+        unsigned long flags;
+
+        spin_lock_irqsave(&priv->lock, flags);
+        ret = iwl_grab_nic_access(priv);
+        if (unlikely(ret)) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                return ret;
+        }
+
+        /* stop Rx DMA */
+        iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+        ret = iwl_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
+                                     (1 << 24), 1000);
+        if (ret < 0)
+                IWL_ERROR("Can't stop Rx DMA.\n");
+
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        return 0;
+}
+EXPORT_SYMBOL(iwl_rxq_stop);
+
+void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+                                struct iwl_rx_mem_buffer *rxb)
+
+{
+#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        struct iwl4965_missed_beacon_notif *missed_beacon;
+
+        missed_beacon = &pkt->u.missed_beacon;
+        if (le32_to_cpu(missed_beacon->consequtive_missed_beacons) > 5) {
+                IWL_DEBUG_CALIB("missed bcn cnsq %d totl %d rcd %d expctd %d\n",
+                    le32_to_cpu(missed_beacon->consequtive_missed_beacons),
+                    le32_to_cpu(missed_beacon->total_missed_becons),
+                    le32_to_cpu(missed_beacon->num_recvd_beacons),
+                    le32_to_cpu(missed_beacon->num_expected_beacons));
+                if (!test_bit(STATUS_SCANNING, &priv->status))
+                        iwl_init_sensitivity(priv);
+        }
+#endif /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
+}
+EXPORT_SYMBOL(iwl_rx_missed_beacon_notif);
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.c b/drivers/net/wireless/iwlwifi/iwl-sta.c
index f2267047d102..983f10760fb0 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.c
@@ -37,6 +37,10 @@
 #include "iwl-io.h"
 #include "iwl-helpers.h"
 
+
+#define IWL_STA_DRIVER_ACTIVE           0x1     /* ucode entry is active */
+#define IWL_STA_UCODE_ACTIVE            0x2     /* ucode entry is active */
+
 u8 iwl_find_station(struct iwl_priv *priv, const u8 *addr)
 {
         int i;
@@ -70,6 +74,39 @@ u8 iwl_find_station(struct iwl_priv *priv, const u8 *addr)
 }
 EXPORT_SYMBOL(iwl_find_station);
 
+static int iwl_add_sta_callback(struct iwl_priv *priv,
+                                   struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+        struct iwl_rx_packet *res = NULL;
+
+        if (!skb) {
+                IWL_ERROR("Error: Response NULL in REPLY_ADD_STA.\n");
+                return 1;
+        }
+
+        res = (struct iwl_rx_packet *)skb->data;
+        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+                IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
+                          res->hdr.flags);
+                return 1;
+        }
+
+        switch (res->u.add_sta.status) {
+        case ADD_STA_SUCCESS_MSK:
+                /* FIXME: implement iwl_sta_ucode_activate(priv, addr); */
+                /* fail through */
+        default:
+                IWL_DEBUG_HC("Received REPLY_ADD_STA:(0x%08X)\n",
+                             res->u.add_sta.status);
+                break;
+        }
+
+        /* We didn't cache the SKB; let the caller free it */
+        return 1;
+}
+
+
+
 int iwl_send_add_sta(struct iwl_priv *priv,
                      struct iwl_addsta_cmd *sta, u8 flags)
 {
@@ -82,7 +119,9 @@ int iwl_send_add_sta(struct iwl_priv *priv,
                 .data = data,
         };
 
-        if (!(flags & CMD_ASYNC))
+        if (flags & CMD_ASYNC)
+                cmd.meta.u.callback = iwl_add_sta_callback;
+        else
                 cmd.meta.flags |= CMD_WANT_SKB;
 
         cmd.len = priv->cfg->ops->utils->build_addsta_hcmd(sta, data);
@@ -117,6 +156,276 @@ int iwl_send_add_sta(struct iwl_priv *priv,
 }
 EXPORT_SYMBOL(iwl_send_add_sta);
 
+#ifdef CONFIG_IWL4965_HT
+
+static void iwl_set_ht_add_station(struct iwl_priv *priv, u8 index,
+                                   struct ieee80211_ht_info *sta_ht_inf)
+{
+        __le32 sta_flags;
+        u8 mimo_ps_mode;
+
+        if (!sta_ht_inf || !sta_ht_inf->ht_supported)
+                goto done;
+
+        mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2;
+
+        sta_flags = priv->stations[index].sta.station_flags;
+
+        sta_flags &= ~(STA_FLG_RTS_MIMO_PROT_MSK | STA_FLG_MIMO_DIS_MSK);
+
+        switch (mimo_ps_mode) {
+        case WLAN_HT_CAP_MIMO_PS_STATIC:
+                sta_flags |= STA_FLG_MIMO_DIS_MSK;
+                break;
+        case WLAN_HT_CAP_MIMO_PS_DYNAMIC:
+                sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
+                break;
+        case WLAN_HT_CAP_MIMO_PS_DISABLED:
+                break;
+        default:
+                IWL_WARNING("Invalid MIMO PS mode %d", mimo_ps_mode);
+                break;
+        }
+
+        sta_flags |= cpu_to_le32(
+              (u32)sta_ht_inf->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
+
+        sta_flags |= cpu_to_le32(
+              (u32)sta_ht_inf->ampdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
+
+        if (iwl_is_fat_tx_allowed(priv, sta_ht_inf))
+                sta_flags |= STA_FLG_FAT_EN_MSK;
+        else
+                sta_flags &= ~STA_FLG_FAT_EN_MSK;
+
+        priv->stations[index].sta.station_flags = sta_flags;
+ done:
+        return;
+}
+#else
+static inline void iwl_set_ht_add_station(struct iwl_priv *priv, u8 index,
+                                        struct ieee80211_ht_info *sta_ht_info)
+{
+}
+#endif
+
+/**
+ * iwl_add_station_flags - Add station to tables in driver and device
+ */
+u8 iwl_add_station_flags(struct iwl_priv *priv, const u8 *addr, int is_ap,
+                         u8 flags, struct ieee80211_ht_info *ht_info)
+{
+        int i;
+        int index = IWL_INVALID_STATION;
+        struct iwl_station_entry *station;
+        unsigned long flags_spin;
+        DECLARE_MAC_BUF(mac);
+
+        spin_lock_irqsave(&priv->sta_lock, flags_spin);
+        if (is_ap)
+                index = IWL_AP_ID;
+        else if (is_broadcast_ether_addr(addr))
+                index = priv->hw_params.bcast_sta_id;
+        else
+                for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++) {
+                        if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
+                                                addr)) {
+                                index = i;
+                                break;
+                        }
+
+                        if (!priv->stations[i].used &&
+                            index == IWL_INVALID_STATION)
+                                index = i;
+                }
+
+
+        /* These two conditions have the same outcome, but keep them separate
+          since they have different meanings */
+        if (unlikely(index == IWL_INVALID_STATION)) {
+                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+                return index;
+        }
+
+        if (priv->stations[index].used &&
+            !compare_ether_addr(priv->stations[index].sta.sta.addr, addr)) {
+                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+                return index;
+        }
+
+
+        IWL_DEBUG_ASSOC("Add STA ID %d: %s\n", index, print_mac(mac, addr));
+        station = &priv->stations[index];
+        station->used = 1;
+        priv->num_stations++;
+
+        /* Set up the REPLY_ADD_STA command to send to device */
+        memset(&station->sta, 0, sizeof(struct iwl_addsta_cmd));
+        memcpy(station->sta.sta.addr, addr, ETH_ALEN);
+        station->sta.mode = 0;
+        station->sta.sta.sta_id = index;
+        station->sta.station_flags = 0;
+
+        /* BCAST station and IBSS stations do not work in HT mode */
+        if (index != priv->hw_params.bcast_sta_id &&
+            priv->iw_mode != IEEE80211_IF_TYPE_IBSS)
+                iwl_set_ht_add_station(priv, index, ht_info);
+
+        spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+        /* Add station to device's station table */
+        iwl_send_add_sta(priv, &station->sta, flags);
+        return index;
+
+}
+EXPORT_SYMBOL(iwl_add_station_flags);
+
+
+static int iwl_sta_ucode_deactivate(struct iwl_priv *priv, const char *addr)
+{
+        unsigned long flags;
+        u8 sta_id;
+        DECLARE_MAC_BUF(mac);
+
+        sta_id = iwl_find_station(priv, addr);
+        if (sta_id != IWL_INVALID_STATION) {
+                IWL_DEBUG_ASSOC("Removed STA from Ucode: %s\n",
+                                print_mac(mac, addr));
+                spin_lock_irqsave(&priv->sta_lock, flags);
+                priv->stations[sta_id].used &= ~IWL_STA_UCODE_ACTIVE;
+                memset(&priv->stations[sta_id], 0,
+                        sizeof(struct iwl_station_entry));
+                spin_unlock_irqrestore(&priv->sta_lock, flags);
+                return 0;
+        }
+        return -EINVAL;
+}
+
+static int iwl_remove_sta_callback(struct iwl_priv *priv,
+                                   struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+        struct iwl_rx_packet *res = NULL;
+        const char *addr = cmd->cmd.rm_sta.addr;
+
+        if (!skb) {
+                IWL_ERROR("Error: Response NULL in REPLY_REMOVE_STA.\n");
+                return 1;
+        }
+
+        res = (struct iwl_rx_packet *)skb->data;
+        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+                IWL_ERROR("Bad return from REPLY_REMOVE_STA (0x%08X)\n",
+                res->hdr.flags);
+                return 1;
+        }
+
+        switch (res->u.rem_sta.status) {
+        case REM_STA_SUCCESS_MSK:
+                iwl_sta_ucode_deactivate(priv, addr);
+                break;
+        default:
+                break;
+        }
+
+        /* We didn't cache the SKB; let the caller free it */
+        return 1;
+}
+
+static int iwl_send_remove_station(struct iwl_priv *priv, const u8 *addr,
+                                   u8 flags)
+{
+        struct iwl_rx_packet *res = NULL;
+        int ret;
+
+        struct iwl_rem_sta_cmd rm_sta_cmd;
+
+        struct iwl_host_cmd cmd = {
+                .id = REPLY_REMOVE_STA,
+                .len = sizeof(struct iwl_rem_sta_cmd),
+                .meta.flags = flags,
+                .data = &rm_sta_cmd,
+        };
+
+        memset(&rm_sta_cmd, 0, sizeof(rm_sta_cmd));
+        rm_sta_cmd.num_sta = 1;
+        memcpy(&rm_sta_cmd.addr, addr , ETH_ALEN);
+
+        if (flags & CMD_ASYNC)
+                cmd.meta.u.callback = iwl_remove_sta_callback;
+        else
+                cmd.meta.flags |= CMD_WANT_SKB;
+        ret = iwl_send_cmd(priv, &cmd);
+
+        if (ret || (flags & CMD_ASYNC))
+                return ret;
+
+        res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+        if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+                IWL_ERROR("Bad return from REPLY_REMOVE_STA (0x%08X)\n",
+                          res->hdr.flags);
+                ret = -EIO;
+        }
+
+        if (!ret) {
+                switch (res->u.rem_sta.status) {
+                case REM_STA_SUCCESS_MSK:
+                        iwl_sta_ucode_deactivate(priv, addr);
+                        IWL_DEBUG_ASSOC("REPLY_REMOVE_STA PASSED\n");
+                        break;
+                default:
+                        ret = -EIO;
+                        IWL_ERROR("REPLY_REMOVE_STA failed\n");
+                        break;
+                }
+        }
+
+        priv->alloc_rxb_skb--;
+        dev_kfree_skb_any(cmd.meta.u.skb);
+
+        return ret;
+}
+/**
+ * iwl_remove_station - Remove driver's knowledge of station.
+ *
+ */
+u8 iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+        int index = IWL_INVALID_STATION;
+        int i;
+        unsigned long flags;
+
+        spin_lock_irqsave(&priv->sta_lock, flags);
+
+        if (is_ap)
+                index = IWL_AP_ID;
+        else if (is_broadcast_ether_addr(addr))
+                index = priv->hw_params.bcast_sta_id;
+        else
+                for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++)
+                        if (priv->stations[i].used &&
+                            !compare_ether_addr(priv->stations[i].sta.sta.addr,
+                                                addr)) {
+                                index = i;
+                                break;
+                        }
+
+        if (unlikely(index == IWL_INVALID_STATION))
+                goto out;
+
+        if (priv->stations[index].used) {
+                priv->stations[index].used = 0;
+                priv->num_stations--;
+        }
+
+        BUG_ON(priv->num_stations < 0);
+        spin_unlock_irqrestore(&priv->sta_lock, flags);
+        iwl_send_remove_station(priv, addr, CMD_ASYNC);
+        return index;
+out:
+        spin_unlock_irqrestore(&priv->sta_lock, flags);
+        return 0;
+}
+EXPORT_SYMBOL(iwl_remove_station);
 int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
 {
         int i;
@@ -200,7 +509,7 @@ int iwl_set_default_wep_key(struct iwl_priv *priv,
         unsigned long flags;
 
         keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
-        keyconf->hw_key_idx = keyconf->keyidx;
+        keyconf->hw_key_idx = HW_KEY_DEFAULT;
         priv->stations[IWL_AP_ID].keyinfo.alg = ALG_WEP;
 
         spin_lock_irqsave(&priv->sta_lock, flags);
@@ -230,7 +539,6 @@ static int iwl_set_wep_dynamic_key_info(struct iwl_priv *priv,
         int ret;
 
         keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
-        keyconf->hw_key_idx = keyconf->keyidx;
 
         key_flags |= (STA_KEY_FLG_WEP | STA_KEY_FLG_MAP_KEY_MSK);
         key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
@@ -287,7 +595,6 @@ static int iwl_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
                 key_flags |= STA_KEY_MULTICAST_MSK;
 
         keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-        keyconf->hw_key_idx = keyconf->keyidx;
 
         spin_lock_irqsave(&priv->sta_lock, flags);
         priv->stations[sta_id].keyinfo.alg = keyconf->alg;
@@ -325,12 +632,10 @@ static int iwl_set_tkip_dynamic_key_info(struct iwl_priv *priv,
 
         keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
         keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
-        keyconf->hw_key_idx = keyconf->keyidx;
 
         spin_lock_irqsave(&priv->sta_lock, flags);
 
         priv->stations[sta_id].keyinfo.alg = keyconf->alg;
-        priv->stations[sta_id].keyinfo.conf = keyconf;
         priv->stations[sta_id].keyinfo.keylen = 16;
 
         if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
@@ -359,7 +664,7 @@ int iwl_remove_dynamic_key(struct iwl_priv *priv,
         u16 key_flags;
         u8 keyidx;
 
-        priv->key_mapping_key = 0;
+        priv->key_mapping_key--;
 
         spin_lock_irqsave(&priv->sta_lock, flags);
         key_flags = le16_to_cpu(priv->stations[sta_id].sta.key.key_flags);
@@ -390,31 +695,32 @@ int iwl_remove_dynamic_key(struct iwl_priv *priv,
         priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
 
         IWL_DEBUG_INFO("hwcrypto: clear ucode station key info\n");
-        ret =  iwl_send_add_sta(priv, &priv->stations[sta_id].sta, 0);
+        ret =  iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
         spin_unlock_irqrestore(&priv->sta_lock, flags);
         return ret;
 }
 EXPORT_SYMBOL(iwl_remove_dynamic_key);
 
 int iwl_set_dynamic_key(struct iwl_priv *priv,
-                                struct ieee80211_key_conf *key, u8 sta_id)
+                                struct ieee80211_key_conf *keyconf, u8 sta_id)
 {
         int ret;
 
-        priv->key_mapping_key = 1;
+        priv->key_mapping_key++;
+        keyconf->hw_key_idx = HW_KEY_DYNAMIC;
 
-        switch (key->alg) {
+        switch (keyconf->alg) {
         case ALG_CCMP:
-                ret = iwl_set_ccmp_dynamic_key_info(priv, key, sta_id);
+                ret = iwl_set_ccmp_dynamic_key_info(priv, keyconf, sta_id);
                 break;
         case ALG_TKIP:
-                ret = iwl_set_tkip_dynamic_key_info(priv, key, sta_id);
+                ret = iwl_set_tkip_dynamic_key_info(priv, keyconf, sta_id);
                 break;
         case ALG_WEP:
-                ret = iwl_set_wep_dynamic_key_info(priv, key, sta_id);
+                ret = iwl_set_wep_dynamic_key_info(priv, keyconf, sta_id);
                 break;
         default:
-                IWL_ERROR("Unknown alg: %s alg = %d\n", __func__, key->alg);
+                IWL_ERROR("Unknown alg: %s alg = %d\n", __func__, keyconf->alg);
                 ret = -EINVAL;
         }
 
@@ -470,3 +776,168 @@ int iwl_send_lq_cmd(struct iwl_priv *priv,
 }
 EXPORT_SYMBOL(iwl_send_lq_cmd);
 
+/**
+ * iwl_sta_init_lq - Initialize a station's hardware rate table
+ *
+ * The uCode's station table contains a table of fallback rates
+ * for automatic fallback during transmission.
+ *
+ * NOTE: This sets up a default set of values.  These will be replaced later
+ *       if the driver's iwl-4965-rs rate scaling algorithm is used, instead of
+ *       rc80211_simple.
+ *
+ * NOTE: Run REPLY_ADD_STA command to set up station table entry, before
+ *       calling this function (which runs REPLY_TX_LINK_QUALITY_CMD,
+ *       which requires station table entry to exist).
+ */
+static void iwl_sta_init_lq(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+        int i, r;
+        struct iwl_link_quality_cmd link_cmd = {
+                .reserved1 = 0,
+        };
+        u16 rate_flags;
+
+        /* Set up the rate scaling to start at selected rate, fall back
+         * all the way down to 1M in IEEE order, and then spin on 1M */
+        if (is_ap)
+                r = IWL_RATE_54M_INDEX;
+        else if (priv->band == IEEE80211_BAND_5GHZ)
+                r = IWL_RATE_6M_INDEX;
+        else
+                r = IWL_RATE_1M_INDEX;
+
+        for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
+                rate_flags = 0;
+                if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
+                        rate_flags |= RATE_MCS_CCK_MSK;
+
+                /* Use Tx antenna B only */
+                rate_flags |= RATE_MCS_ANT_B_MSK; /*FIXME:RS*/
+
+                link_cmd.rs_table[i].rate_n_flags =
+                        iwl4965_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
+                r = iwl4965_get_prev_ieee_rate(r);
+        }
+
+        link_cmd.general_params.single_stream_ant_msk = 2;
+        link_cmd.general_params.dual_stream_ant_msk = 3;
+        link_cmd.agg_params.agg_dis_start_th = 3;
+        link_cmd.agg_params.agg_time_limit = cpu_to_le16(4000);
+
+        /* Update the rate scaling for control frame Tx to AP */
+        link_cmd.sta_id = is_ap ? IWL_AP_ID : priv->hw_params.bcast_sta_id;
+
+        iwl_send_cmd_pdu_async(priv, REPLY_TX_LINK_QUALITY_CMD,
+                               sizeof(link_cmd), &link_cmd, NULL);
+}
+/**
+ * iwl_rxon_add_station - add station into station table.
+ *
+ * there is only one AP station with id= IWL_AP_ID
+ * NOTE: mutex must be held before calling this fnction
+ */
+int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+        u8 sta_id;
+
+        /* Add station to device's station table */
+#ifdef CONFIG_IWL4965_HT
+        struct ieee80211_conf *conf = &priv->hw->conf;
+        struct ieee80211_ht_info *cur_ht_config = &conf->ht_conf;
+
+        if ((is_ap) &&
+            (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
+            (priv->iw_mode == IEEE80211_IF_TYPE_STA))
+                sta_id = iwl_add_station_flags(priv, addr, is_ap,
+                                                   0, cur_ht_config);
+        else
+#endif /* CONFIG_IWL4965_HT */
+                sta_id = iwl_add_station_flags(priv, addr, is_ap,
+                                                   0, NULL);
+
+        /* Set up default rate scaling table in device's station table */
+        iwl_sta_init_lq(priv, addr, is_ap);
+
+        return sta_id;
+}
+EXPORT_SYMBOL(iwl_rxon_add_station);
+
+
+/**
+ * iwl_get_sta_id - Find station's index within station table
+ *
+ * If new IBSS station, create new entry in station table
+ */
+int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
+{
+        int sta_id;
+        u16 fc = le16_to_cpu(hdr->frame_control);
+        DECLARE_MAC_BUF(mac);
+
+        /* If this frame is broadcast or management, use broadcast station id */
+        if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
+            is_multicast_ether_addr(hdr->addr1))
+                return priv->hw_params.bcast_sta_id;
+
+        switch (priv->iw_mode) {
+
+        /* If we are a client station in a BSS network, use the special
+         * AP station entry (that's the only station we communicate with) */
+        case IEEE80211_IF_TYPE_STA:
+                return IWL_AP_ID;
+
+        /* If we are an AP, then find the station, or use BCAST */
+        case IEEE80211_IF_TYPE_AP:
+                sta_id = iwl_find_station(priv, hdr->addr1);
+                if (sta_id != IWL_INVALID_STATION)
+                        return sta_id;
+                return priv->hw_params.bcast_sta_id;
+
+        /* If this frame is going out to an IBSS network, find the station,
+         * or create a new station table entry */
+        case IEEE80211_IF_TYPE_IBSS:
+                sta_id = iwl_find_station(priv, hdr->addr1);
+                if (sta_id != IWL_INVALID_STATION)
+                        return sta_id;
+
+                /* Create new station table entry */
+                sta_id = iwl_add_station_flags(priv, hdr->addr1,
+                                                   0, CMD_ASYNC, NULL);
+
+                if (sta_id != IWL_INVALID_STATION)
+                        return sta_id;
+
+                IWL_DEBUG_DROP("Station %s not in station map. "
+                               "Defaulting to broadcast...\n",
+                               print_mac(mac, hdr->addr1));
+                iwl_print_hex_dump(priv, IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
+                return priv->hw_params.bcast_sta_id;
+
+        default:
+                IWL_WARNING("Unknown mode of operation: %d", priv->iw_mode);
+                return priv->hw_params.bcast_sta_id;
+        }
+}
+EXPORT_SYMBOL(iwl_get_sta_id);
+
+
+/**
+ * iwl_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
+ */
+void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv, int sta_id, int tid)
+{
+        unsigned long flags;
+
+        /* Remove "disable" flag, to enable Tx for this TID */
+        spin_lock_irqsave(&priv->sta_lock, flags);
+        priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_TID_DISABLE_TX;
+        priv->stations[sta_id].sta.tid_disable_tx &= cpu_to_le16(~(1 << tid));
+        priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+        spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+        iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_sta_modify_enable_tid_tx);
+
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.h b/drivers/net/wireless/iwlwifi/iwl-sta.h
index 38b1b0a98845..3d55716f5301 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.h
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.h
@@ -29,6 +29,9 @@
 #ifndef __iwl_sta_h__
 #define __iwl_sta_h__
 
+#define HW_KEY_DYNAMIC 0
+#define HW_KEY_DEFAULT 1
+
 int iwl_get_free_ucode_key_index(struct iwl_priv *priv);
 int iwl_send_static_wepkey_cmd(struct iwl_priv *priv, u8 send_if_empty);
 int iwl_remove_default_wep_key(struct iwl_priv *priv,
@@ -39,4 +42,8 @@ int iwl_set_dynamic_key(struct iwl_priv *priv,
                                 struct ieee80211_key_conf *key, u8 sta_id);
 int iwl_remove_dynamic_key(struct iwl_priv *priv,
                                 struct ieee80211_key_conf *key, u8 sta_id);
+int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap);
+u8 iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap);
+int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr);
+void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv, int sta_id, int tid);
 #endif /* __iwl_sta_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index a1e03ccd5147..cfe6f4b233dd 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -27,6 +27,7 @@
  *
  *****************************************************************************/
 
+#include <linux/etherdevice.h>
 #include <net/mac80211.h>
 #include "iwl-eeprom.h"
 #include "iwl-dev.h"
@@ -35,6 +36,32 @@
 #include "iwl-io.h"
 #include "iwl-helpers.h"
 
+#ifdef CONFIG_IWL4965_HT
+
+static const u16 default_tid_to_tx_fifo[] = {
+        IWL_TX_FIFO_AC1,
+        IWL_TX_FIFO_AC0,
+        IWL_TX_FIFO_AC0,
+        IWL_TX_FIFO_AC1,
+        IWL_TX_FIFO_AC2,
+        IWL_TX_FIFO_AC2,
+        IWL_TX_FIFO_AC3,
+        IWL_TX_FIFO_AC3,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_NONE,
+        IWL_TX_FIFO_AC3
+};
+
+#endif  /*CONFIG_IWL4965_HT */
+
+
+
 /**
  * iwl_hw_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
  *
@@ -95,6 +122,89 @@ int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 }
 EXPORT_SYMBOL(iwl_hw_txq_free_tfd);
 
+
+int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *ptr,
+                                 dma_addr_t addr, u16 len)
+{
+        int index, is_odd;
+        struct iwl_tfd_frame *tfd = ptr;
+        u32 num_tbs = IWL_GET_BITS(*tfd, num_tbs);
+
+        /* Each TFD can point to a maximum 20 Tx buffers */
+        if ((num_tbs >= MAX_NUM_OF_TBS) || (num_tbs < 0)) {
+                IWL_ERROR("Error can not send more than %d chunks\n",
+                          MAX_NUM_OF_TBS);
+                return -EINVAL;
+        }
+
+        index = num_tbs / 2;
+        is_odd = num_tbs & 0x1;
+
+        if (!is_odd) {
+                tfd->pa[index].tb1_addr = cpu_to_le32(addr);
+                IWL_SET_BITS(tfd->pa[index], tb1_addr_hi,
+                             iwl_get_dma_hi_address(addr));
+                IWL_SET_BITS(tfd->pa[index], tb1_len, len);
+        } else {
+                IWL_SET_BITS(tfd->pa[index], tb2_addr_lo16,
+                             (u32) (addr & 0xffff));
+                IWL_SET_BITS(tfd->pa[index], tb2_addr_hi20, addr >> 16);
+                IWL_SET_BITS(tfd->pa[index], tb2_len, len);
+        }
+
+        IWL_SET_BITS(*tfd, num_tbs, num_tbs + 1);
+
+        return 0;
+}
+EXPORT_SYMBOL(iwl_hw_txq_attach_buf_to_tfd);
+
+/**
+ * iwl_txq_update_write_ptr - Send new write index to hardware
+ */
+int iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+        u32 reg = 0;
+        int ret = 0;
+        int txq_id = txq->q.id;
+
+        if (txq->need_update == 0)
+                return ret;
+
+        /* if we're trying to save power */
+        if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+                /* wake up nic if it's powered down ...
+                 * uCode will wake up, and interrupt us again, so next
+                 * time we'll skip this part. */
+                reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+                if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+                        IWL_DEBUG_INFO("Requesting wakeup, GP1 = 0x%x\n", reg);
+                        iwl_set_bit(priv, CSR_GP_CNTRL,
+                                    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+                        return ret;
+                }
+
+                /* restore this queue's parameters in nic hardware. */
+                ret = iwl_grab_nic_access(priv);
+                if (ret)
+                        return ret;
+                iwl_write_direct32(priv, HBUS_TARG_WRPTR,
+                                     txq->q.write_ptr | (txq_id << 8));
+                iwl_release_nic_access(priv);
+
+        /* else not in power-save mode, uCode will never sleep when we're
+         * trying to tx (during RFKILL, we're not trying to tx). */
+        } else
+                iwl_write32(priv, HBUS_TARG_WRPTR,
+                            txq->q.write_ptr | (txq_id << 8));
+
+        txq->need_update = 0;
+
+        return ret;
+}
+EXPORT_SYMBOL(iwl_txq_update_write_ptr);
+
+
 /**
  * iwl_tx_queue_free - Deallocate DMA queue.
  * @txq: Transmit queue to deallocate.
@@ -105,7 +215,7 @@ EXPORT_SYMBOL(iwl_hw_txq_free_tfd);
  */
 static void iwl_tx_queue_free(struct iwl_priv *priv, struct iwl_tx_queue *txq)
 {
-        struct iwl4965_queue *q = &txq->q;
+        struct iwl_queue *q = &txq->q;
         struct pci_dev *dev = priv->pci_dev;
         int len;
 
@@ -137,28 +247,51 @@ static void iwl_tx_queue_free(struct iwl_priv *priv, struct iwl_tx_queue *txq)
         memset(txq, 0, sizeof(*txq));
 }
 
-/**
- * iwl_hw_txq_ctx_free - Free TXQ Context
+/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
+ * DMA services
  *
- * Destroy all TX DMA queues and structures
- */
-void iwl_hw_txq_ctx_free(struct iwl_priv *priv)
+ * Theory of operation
+ *
+ * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
+ * of buffer descriptors, each of which points to one or more data buffers for
+ * the device to read from or fill.  Driver and device exchange status of each
+ * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
+ * entries in each circular buffer, to protect against confusing empty and full
+ * queue states.
+ *
+ * The device reads or writes the data in the queues via the device's several
+ * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
+ *
+ * For Tx queue, there are low mark and high mark limits. If, after queuing
+ * the packet for Tx, free space become < low mark, Tx queue stopped. When
+ * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
+ * Tx queue resumed.
+ *
+ * See more detailed info in iwl-4965-hw.h.
+ ***************************************************/
+
+int iwl_queue_space(const struct iwl_queue *q)
 {
-        int txq_id;
+        int s = q->read_ptr - q->write_ptr;
 
-        /* Tx queues */
-        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
-                iwl_tx_queue_free(priv, &priv->txq[txq_id]);
+        if (q->read_ptr > q->write_ptr)
+                s -= q->n_bd;
 
-        /* Keep-warm buffer */
-        iwl_kw_free(priv);
+        if (s <= 0)
+                s += q->n_window;
+        /* keep some reserve to not confuse empty and full situations */
+        s -= 2;
+        if (s < 0)
+                s = 0;
+        return s;
 }
-EXPORT_SYMBOL(iwl_hw_txq_ctx_free);
+EXPORT_SYMBOL(iwl_queue_space);
+
 
 /**
  * iwl_queue_init - Initialize queue's high/low-water and read/write indexes
  */
-static int iwl_queue_init(struct iwl_priv *priv, struct iwl4965_queue *q,
+static int iwl_queue_init(struct iwl_priv *priv, struct iwl_queue *q,
                           int count, int slots_num, u32 id)
 {
         q->n_bd = count;
@@ -312,6 +445,24 @@ static int iwl_tx_queue_init(struct iwl_priv *priv,
 
         return 0;
 }
+/**
+ * iwl_hw_txq_ctx_free - Free TXQ Context
+ *
+ * Destroy all TX DMA queues and structures
+ */
+void iwl_hw_txq_ctx_free(struct iwl_priv *priv)
+{
+        int txq_id;
+
+        /* Tx queues */
+        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+                iwl_tx_queue_free(priv, &priv->txq[txq_id]);
+
+        /* Keep-warm buffer */
+        iwl_kw_free(priv);
+}
+EXPORT_SYMBOL(iwl_hw_txq_ctx_free);
+
 
 /**
  * iwl_txq_ctx_reset - Reset TX queue context
@@ -324,6 +475,7 @@ int iwl_txq_ctx_reset(struct iwl_priv *priv)
 {
         int ret = 0;
         int txq_id, slots_num;
+        unsigned long flags;
 
         iwl_kw_free(priv);
 
@@ -336,11 +488,19 @@ int iwl_txq_ctx_reset(struct iwl_priv *priv)
                 IWL_ERROR("Keep Warm allocation failed");
                 goto error_kw;
         }
+        spin_lock_irqsave(&priv->lock, flags);
+        ret = iwl_grab_nic_access(priv);
+        if (unlikely(ret)) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                goto error_reset;
+        }
 
         /* Turn off all Tx DMA fifos */
-        ret = priv->cfg->ops->lib->disable_tx_fifo(priv);
-        if (unlikely(ret))
-                goto error_reset;
+        priv->cfg->ops->lib->txq_set_sched(priv, 0);
+
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
 
         /* Tell nic where to find the keep-warm buffer */
         ret = iwl_kw_init(priv);
@@ -349,8 +509,7 @@ int iwl_txq_ctx_reset(struct iwl_priv *priv)
                 goto error_reset;
         }
 
-        /* Alloc and init all (default 16) Tx queues,
-         * including the command queue (#4) */
+        /* Alloc and init all Tx queues, including the command queue (#4) */
         for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
                 slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
                                         TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
@@ -371,3 +530,864 @@ int iwl_txq_ctx_reset(struct iwl_priv *priv)
  error_kw:
         return ret;
 }
+/**
+ * iwl_txq_ctx_stop - Stop all Tx DMA channels, free Tx queue memory
+ */
+void iwl_txq_ctx_stop(struct iwl_priv *priv)
+{
+
+        int txq_id;
+        unsigned long flags;
+
+
+        /* Turn off all Tx DMA fifos */
+        spin_lock_irqsave(&priv->lock, flags);
+        if (iwl_grab_nic_access(priv)) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                return;
+        }
+
+        priv->cfg->ops->lib->txq_set_sched(priv, 0);
+
+        /* Stop each Tx DMA channel, and wait for it to be idle */
+        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
+                iwl_write_direct32(priv,
+                                   FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
+                iwl_poll_direct_bit(priv, FH_TSSR_TX_STATUS_REG,
+                                    FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
+                                    (txq_id), 200);
+        }
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        /* Deallocate memory for all Tx queues */
+        iwl_hw_txq_ctx_free(priv);
+}
+EXPORT_SYMBOL(iwl_txq_ctx_stop);
+
+/*
+ * handle build REPLY_TX command notification.
+ */
+static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
+                                  struct iwl_tx_cmd *tx_cmd,
+                                  struct ieee80211_tx_info *info,
+                                  struct ieee80211_hdr *hdr,
+                                  int is_unicast, u8 std_id)
+{
+        u16 fc = le16_to_cpu(hdr->frame_control);
+        __le32 tx_flags = tx_cmd->tx_flags;
+
+        tx_cmd->stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+                tx_flags |= TX_CMD_FLG_ACK_MSK;
+                if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
+                        tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+                if (ieee80211_is_probe_response(fc) &&
+                    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
+                        tx_flags |= TX_CMD_FLG_TSF_MSK;
+        } else {
+                tx_flags &= (~TX_CMD_FLG_ACK_MSK);
+                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+        }
+
+        if (ieee80211_is_back_request(fc))
+                tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
+
+
+        tx_cmd->sta_id = std_id;
+        if (ieee80211_get_morefrag(hdr))
+                tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
+
+        if (ieee80211_is_qos_data(fc)) {
+                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                tx_cmd->tid_tspec = qc[0] & 0xf;
+                tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
+        } else {
+                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+        }
+
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
+                tx_flags |= TX_CMD_FLG_RTS_MSK;
+                tx_flags &= ~TX_CMD_FLG_CTS_MSK;
+        } else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
+                tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+                tx_flags |= TX_CMD_FLG_CTS_MSK;
+        }
+
+        if ((tx_flags & TX_CMD_FLG_RTS_MSK) || (tx_flags & TX_CMD_FLG_CTS_MSK))
+                tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
+
+        tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
+        if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+                if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
+                    (fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ)
+                        tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(3);
+                else
+                        tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(2);
+        } else {
+                tx_cmd->timeout.pm_frame_timeout = 0;
+        }
+
+        tx_cmd->driver_txop = 0;
+        tx_cmd->tx_flags = tx_flags;
+        tx_cmd->next_frame_len = 0;
+}
+
+#define RTS_HCCA_RETRY_LIMIT            3
+#define RTS_DFAULT_RETRY_LIMIT          60
+
+static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
+                              struct iwl_tx_cmd *tx_cmd,
+                              struct ieee80211_tx_info *info,
+                              u16 fc, int sta_id,
+                              int is_hcca)
+{
+        u8 rts_retry_limit = 0;
+        u8 data_retry_limit = 0;
+        u8 rate_plcp;
+        u16 rate_flags = 0;
+        int rate_idx;
+
+        rate_idx = min(ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xffff,
+                        IWL_RATE_COUNT - 1);
+
+        rate_plcp = iwl_rates[rate_idx].plcp;
+
+        rts_retry_limit = (is_hcca) ?
+            RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
+
+        if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
+                rate_flags |= RATE_MCS_CCK_MSK;
+
+
+        if (ieee80211_is_probe_response(fc)) {
+                data_retry_limit = 3;
+                if (data_retry_limit < rts_retry_limit)
+                        rts_retry_limit = data_retry_limit;
+        } else
+                data_retry_limit = IWL_DEFAULT_TX_RETRY;
+
+        if (priv->data_retry_limit != -1)
+                data_retry_limit = priv->data_retry_limit;
+
+
+        if (ieee80211_is_data(fc)) {
+                tx_cmd->initial_rate_index = 0;
+                tx_cmd->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
+        } else {
+                switch (fc & IEEE80211_FCTL_STYPE) {
+                case IEEE80211_STYPE_AUTH:
+                case IEEE80211_STYPE_DEAUTH:
+                case IEEE80211_STYPE_ASSOC_REQ:
+                case IEEE80211_STYPE_REASSOC_REQ:
+                        if (tx_cmd->tx_flags & TX_CMD_FLG_RTS_MSK) {
+                                tx_cmd->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+                                tx_cmd->tx_flags |= TX_CMD_FLG_CTS_MSK;
+                        }
+                        break;
+                default:
+                        break;
+                }
+
+                /* Alternate between antenna A and B for successive frames */
+                if (priv->use_ant_b_for_management_frame) {
+                        priv->use_ant_b_for_management_frame = 0;
+                        rate_flags |= RATE_MCS_ANT_B_MSK;
+                } else {
+                        priv->use_ant_b_for_management_frame = 1;
+                        rate_flags |= RATE_MCS_ANT_A_MSK;
+                }
+        }
+
+        tx_cmd->rts_retry_limit = rts_retry_limit;
+        tx_cmd->data_retry_limit = data_retry_limit;
+        tx_cmd->rate_n_flags = iwl4965_hw_set_rate_n_flags(rate_plcp, rate_flags);
+}
+
+static void iwl_tx_cmd_build_hwcrypto(struct iwl_priv *priv,
+                                      struct ieee80211_tx_info *info,
+                                      struct iwl_tx_cmd *tx_cmd,
+                                      struct sk_buff *skb_frag,
+                                      int sta_id)
+{
+        struct ieee80211_key_conf *keyconf = info->control.hw_key;
+
+        switch (keyconf->alg) {
+        case ALG_CCMP:
+                tx_cmd->sec_ctl = TX_CMD_SEC_CCM;
+                memcpy(tx_cmd->key, keyconf->key, keyconf->keylen);
+                if (info->flags & IEEE80211_TX_CTL_AMPDU)
+                        tx_cmd->tx_flags |= TX_CMD_FLG_AGG_CCMP_MSK;
+                IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
+                break;
+
+        case ALG_TKIP:
+                tx_cmd->sec_ctl = TX_CMD_SEC_TKIP;
+                ieee80211_get_tkip_key(keyconf, skb_frag,
+                        IEEE80211_TKIP_P2_KEY, tx_cmd->key);
+                IWL_DEBUG_TX("tx_cmd with tkip hwcrypto\n");
+                break;
+
+        case ALG_WEP:
+                tx_cmd->sec_ctl |= (TX_CMD_SEC_WEP |
+                        (keyconf->keyidx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT);
+
+                if (keyconf->keylen == WEP_KEY_LEN_128)
+                        tx_cmd->sec_ctl |= TX_CMD_SEC_KEY128;
+
+                memcpy(&tx_cmd->key[3], keyconf->key, keyconf->keylen);
+
+                IWL_DEBUG_TX("Configuring packet for WEP encryption "
+                             "with key %d\n", keyconf->keyidx);
+                break;
+
+        default:
+                printk(KERN_ERR "Unknown encode alg %d\n", keyconf->alg);
+                break;
+        }
+}
+
+static void iwl_update_tx_stats(struct iwl_priv *priv, u16 fc, u16 len)
+{
+        /* 0 - mgmt, 1 - cnt, 2 - data */
+        int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
+        priv->tx_stats[idx].cnt++;
+        priv->tx_stats[idx].bytes += len;
+}
+
+/*
+ * start REPLY_TX command process
+ */
+int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
+{
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct iwl_tfd_frame *tfd;
+        u32 *control_flags;
+        int txq_id = skb_get_queue_mapping(skb);
+        struct iwl_tx_queue *txq = NULL;
+        struct iwl_queue *q = NULL;
+        dma_addr_t phys_addr;
+        dma_addr_t txcmd_phys;
+        dma_addr_t scratch_phys;
+        struct iwl_cmd *out_cmd = NULL;
+        struct iwl_tx_cmd *tx_cmd;
+        u16 len, idx, len_org;
+        u16 seq_number = 0;
+        u8 id, hdr_len, unicast;
+        u8 sta_id;
+        u16 fc;
+        u8 wait_write_ptr = 0;
+        u8 tid = 0;
+        u8 *qc = NULL;
+        unsigned long flags;
+        int ret;
+
+        spin_lock_irqsave(&priv->lock, flags);
+        if (iwl_is_rfkill(priv)) {
+                IWL_DEBUG_DROP("Dropping - RF KILL\n");
+                goto drop_unlock;
+        }
+
+        if (!priv->vif) {
+                IWL_DEBUG_DROP("Dropping - !priv->vif\n");
+                goto drop_unlock;
+        }
+
+        if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xFF) ==
+             IWL_INVALID_RATE) {
+                IWL_ERROR("ERROR: No TX rate available.\n");
+                goto drop_unlock;
+        }
+
+        unicast = !is_multicast_ether_addr(hdr->addr1);
+        id = 0;
+
+        fc = le16_to_cpu(hdr->frame_control);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+        if (ieee80211_is_auth(fc))
+                IWL_DEBUG_TX("Sending AUTH frame\n");
+        else if (ieee80211_is_assoc_request(fc))
+                IWL_DEBUG_TX("Sending ASSOC frame\n");
+        else if (ieee80211_is_reassoc_request(fc))
+                IWL_DEBUG_TX("Sending REASSOC frame\n");
+#endif
+
+        /* drop all data frame if we are not associated */
+        if (((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
+           (!iwl_is_associated(priv) ||
+            ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id) ||
+            !priv->assoc_station_added)) {
+                IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
+                goto drop_unlock;
+        }
+
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        hdr_len = ieee80211_get_hdrlen(fc);
+
+        /* Find (or create) index into station table for destination station */
+        sta_id = iwl_get_sta_id(priv, hdr);
+        if (sta_id == IWL_INVALID_STATION) {
+                DECLARE_MAC_BUF(mac);
+
+                IWL_DEBUG_DROP("Dropping - INVALID STATION: %s\n",
+                               print_mac(mac, hdr->addr1));
+                goto drop;
+        }
+
+        IWL_DEBUG_TX("station Id %d\n", sta_id);
+
+        if (ieee80211_is_qos_data(fc)) {
+                qc = ieee80211_get_qos_ctrl(hdr, hdr_len);
+                tid = qc[0] & 0xf;
+                seq_number = priv->stations[sta_id].tid[tid].seq_number &
+                                IEEE80211_SCTL_SEQ;
+                hdr->seq_ctrl = cpu_to_le16(seq_number) |
+                        (hdr->seq_ctrl &
+                                __constant_cpu_to_le16(IEEE80211_SCTL_FRAG));
+                seq_number += 0x10;
+#ifdef CONFIG_IWL4965_HT
+                /* aggregation is on for this <sta,tid> */
+                if (info->flags & IEEE80211_TX_CTL_AMPDU)
+                        txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
+                priv->stations[sta_id].tid[tid].tfds_in_queue++;
+#endif /* CONFIG_IWL4965_HT */
+        }
+
+        /* Descriptor for chosen Tx queue */
+        txq = &priv->txq[txq_id];
+        q = &txq->q;
+
+        spin_lock_irqsave(&priv->lock, flags);
+
+        /* Set up first empty TFD within this queue's circular TFD buffer */
+        tfd = &txq->bd[q->write_ptr];
+        memset(tfd, 0, sizeof(*tfd));
+        control_flags = (u32 *) tfd;
+        idx = get_cmd_index(q, q->write_ptr, 0);
+
+        /* Set up driver data for this TFD */
+        memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl_tx_info));
+        txq->txb[q->write_ptr].skb[0] = skb;
+
+        /* Set up first empty entry in queue's array of Tx/cmd buffers */
+        out_cmd = &txq->cmd[idx];
+        tx_cmd = &out_cmd->cmd.tx;
+        memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
+        memset(tx_cmd, 0, sizeof(struct iwl_tx_cmd));
+
+        /*
+         * Set up the Tx-command (not MAC!) header.
+         * Store the chosen Tx queue and TFD index within the sequence field;
+         * after Tx, uCode's Tx response will return this value so driver can
+         * locate the frame within the tx queue and do post-tx processing.
+         */
+        out_cmd->hdr.cmd = REPLY_TX;
+        out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+                                INDEX_TO_SEQ(q->write_ptr)));
+
+        /* Copy MAC header from skb into command buffer */
+        memcpy(tx_cmd->hdr, hdr, hdr_len);
+
+        /*
+         * Use the first empty entry in this queue's command buffer array
+         * to contain the Tx command and MAC header concatenated together
+         * (payload data will be in another buffer).
+         * Size of this varies, due to varying MAC header length.
+         * If end is not dword aligned, we'll have 2 extra bytes at the end
+         * of the MAC header (device reads on dword boundaries).
+         * We'll tell device about this padding later.
+         */
+        len = sizeof(struct iwl_tx_cmd) +
+                sizeof(struct iwl_cmd_header) + hdr_len;
+
+        len_org = len;
+        len = (len + 3) & ~3;
+
+        if (len_org != len)
+                len_org = 1;
+        else
+                len_org = 0;
+
+        /* Physical address of this Tx command's header (not MAC header!),
+         * within command buffer array. */
+        txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
+                     offsetof(struct iwl_cmd, hdr);
+
+        /* Add buffer containing Tx command and MAC(!) header to TFD's
+         * first entry */
+        iwl_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
+
+        if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT))
+                iwl_tx_cmd_build_hwcrypto(priv, info, tx_cmd, skb, sta_id);
+
+        /* Set up TFD's 2nd entry to point directly to remainder of skb,
+         * if any (802.11 null frames have no payload). */
+        len = skb->len - hdr_len;
+        if (len) {
+                phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
+                                           len, PCI_DMA_TODEVICE);
+                iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
+        }
+
+        /* Tell NIC about any 2-byte padding after MAC header */
+        if (len_org)
+                tx_cmd->tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
+
+        /* Total # bytes to be transmitted */
+        len = (u16)skb->len;
+        tx_cmd->len = cpu_to_le16(len);
+        /* TODO need this for burst mode later on */
+        iwl_tx_cmd_build_basic(priv, tx_cmd, info, hdr, unicast, sta_id);
+
+        /* set is_hcca to 0; it probably will never be implemented */
+        iwl_tx_cmd_build_rate(priv, tx_cmd, info, fc, sta_id, 0);
+
+        iwl_update_tx_stats(priv, fc, len);
+
+        scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
+                offsetof(struct iwl_tx_cmd, scratch);
+        tx_cmd->dram_lsb_ptr = cpu_to_le32(scratch_phys);
+        tx_cmd->dram_msb_ptr = iwl_get_dma_hi_address(scratch_phys);
+
+        if (!ieee80211_get_morefrag(hdr)) {
+                txq->need_update = 1;
+                if (qc)
+                        priv->stations[sta_id].tid[tid].seq_number = seq_number;
+        } else {
+                wait_write_ptr = 1;
+                txq->need_update = 0;
+        }
+
+        iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd, sizeof(*tx_cmd));
+
+        iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd->hdr, hdr_len);
+
+        /* Set up entry for this TFD in Tx byte-count array */
+        priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, len);
+
+        /* Tell device the write index *just past* this latest filled TFD */
+        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
+        ret = iwl_txq_update_write_ptr(priv, txq);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        if (ret)
+                return ret;
+
+        if ((iwl_queue_space(q) < q->high_mark)
+            && priv->mac80211_registered) {
+                if (wait_write_ptr) {
+                        spin_lock_irqsave(&priv->lock, flags);
+                        txq->need_update = 1;
+                        iwl_txq_update_write_ptr(priv, txq);
+                        spin_unlock_irqrestore(&priv->lock, flags);
+                }
+
+                ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
+        }
+
+        return 0;
+
+drop_unlock:
+        spin_unlock_irqrestore(&priv->lock, flags);
+drop:
+        return -1;
+}
+EXPORT_SYMBOL(iwl_tx_skb);
+
+/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
+
+/**
+ * iwl_enqueue_hcmd - enqueue a uCode command
+ * @priv: device private data point
+ * @cmd: a point to the ucode command structure
+ *
+ * The function returns < 0 values to indicate the operation is
+ * failed. On success, it turns the index (> 0) of command in the
+ * command queue.
+ */
+int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+        struct iwl_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
+        struct iwl_queue *q = &txq->q;
+        struct iwl_tfd_frame *tfd;
+        u32 *control_flags;
+        struct iwl_cmd *out_cmd;
+        u32 idx;
+        u16 fix_size;
+        dma_addr_t phys_addr;
+        int ret;
+        unsigned long flags;
+
+        cmd->len = priv->cfg->ops->utils->get_hcmd_size(cmd->id, cmd->len);
+        fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
+
+        /* If any of the command structures end up being larger than
+         * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
+         * we will need to increase the size of the TFD entries */
+        BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
+               !(cmd->meta.flags & CMD_SIZE_HUGE));
+
+        if (iwl_is_rfkill(priv)) {
+                IWL_DEBUG_INFO("Not sending command - RF KILL");
+                return -EIO;
+        }
+
+        if (iwl_queue_space(q) < ((cmd->meta.flags & CMD_ASYNC) ? 2 : 1)) {
+                IWL_ERROR("No space for Tx\n");
+                return -ENOSPC;
+        }
+
+        spin_lock_irqsave(&priv->hcmd_lock, flags);
+
+        tfd = &txq->bd[q->write_ptr];
+        memset(tfd, 0, sizeof(*tfd));
+
+        control_flags = (u32 *) tfd;
+
+        idx = get_cmd_index(q, q->write_ptr, cmd->meta.flags & CMD_SIZE_HUGE);
+        out_cmd = &txq->cmd[idx];
+
+        out_cmd->hdr.cmd = cmd->id;
+        memcpy(&out_cmd->meta, &cmd->meta, sizeof(cmd->meta));
+        memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);
+
+        /* At this point, the out_cmd now has all of the incoming cmd
+         * information */
+
+        out_cmd->hdr.flags = 0;
+        out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(IWL_CMD_QUEUE_NUM) |
+                        INDEX_TO_SEQ(q->write_ptr));
+        if (out_cmd->meta.flags & CMD_SIZE_HUGE)
+                out_cmd->hdr.sequence |= cpu_to_le16(SEQ_HUGE_FRAME);
+
+        phys_addr = txq->dma_addr_cmd + sizeof(txq->cmd[0]) * idx +
+                        offsetof(struct iwl_cmd, hdr);
+        iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, fix_size);
+
+        IWL_DEBUG_HC("Sending command %s (#%x), seq: 0x%04X, "
+                     "%d bytes at %d[%d]:%d\n",
+                     get_cmd_string(out_cmd->hdr.cmd),
+                     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
+                     fix_size, q->write_ptr, idx, IWL_CMD_QUEUE_NUM);
+
+        txq->need_update = 1;
+
+        /* Set up entry in queue's byte count circular buffer */
+        priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, 0);
+
+        /* Increment and update queue's write index */
+        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
+        ret = iwl_txq_update_write_ptr(priv, txq);
+
+        spin_unlock_irqrestore(&priv->hcmd_lock, flags);
+        return ret ? ret : idx;
+}
+
+int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
+{
+        struct iwl_tx_queue *txq = &priv->txq[txq_id];
+        struct iwl_queue *q = &txq->q;
+        struct iwl_tx_info *tx_info;
+        int nfreed = 0;
+
+        if ((index >= q->n_bd) || (iwl_queue_used(q, index) == 0)) {
+                IWL_ERROR("Read index for DMA queue txq id (%d), index %d, "
+                          "is out of range [0-%d] %d %d.\n", txq_id,
+                          index, q->n_bd, q->write_ptr, q->read_ptr);
+                return 0;
+        }
+
+        for (index = iwl_queue_inc_wrap(index, q->n_bd); q->read_ptr != index;
+                q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+                tx_info = &txq->txb[txq->q.read_ptr];
+                ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0]);
+                tx_info->skb[0] = NULL;
+
+                if (priv->cfg->ops->lib->txq_inval_byte_cnt_tbl)
+                        priv->cfg->ops->lib->txq_inval_byte_cnt_tbl(priv, txq);
+
+                iwl_hw_txq_free_tfd(priv, txq);
+                nfreed++;
+        }
+        return nfreed;
+}
+EXPORT_SYMBOL(iwl_tx_queue_reclaim);
+
+
+/**
+ * iwl_hcmd_queue_reclaim - Reclaim TX command queue entries already Tx'd
+ *
+ * When FW advances 'R' index, all entries between old and new 'R' index
+ * need to be reclaimed. As result, some free space forms.  If there is
+ * enough free space (> low mark), wake the stack that feeds us.
+ */
+static void iwl_hcmd_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
+{
+        struct iwl_tx_queue *txq = &priv->txq[txq_id];
+        struct iwl_queue *q = &txq->q;
+        int nfreed = 0;
+
+        if ((index >= q->n_bd) || (iwl_queue_used(q, index) == 0)) {
+                IWL_ERROR("Read index for DMA queue txq id (%d), index %d, "
+                          "is out of range [0-%d] %d %d.\n", txq_id,
+                          index, q->n_bd, q->write_ptr, q->read_ptr);
+                return;
+        }
+
+        for (index = iwl_queue_inc_wrap(index, q->n_bd); q->read_ptr != index;
+                q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+                if (nfreed > 1) {
+                        IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
+                                        q->write_ptr, q->read_ptr);
+                        queue_work(priv->workqueue, &priv->restart);
+                }
+                nfreed++;
+        }
+}
+
+/**
+ * iwl_tx_cmd_complete - Pull unused buffers off the queue and reclaim them
+ * @rxb: Rx buffer to reclaim
+ *
+ * If an Rx buffer has an async callback associated with it the callback
+ * will be executed.  The attached skb (if present) will only be freed
+ * if the callback returns 1
+ */
+void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+        int txq_id = SEQ_TO_QUEUE(sequence);
+        int index = SEQ_TO_INDEX(sequence);
+        int huge = sequence & SEQ_HUGE_FRAME;
+        int cmd_index;
+        struct iwl_cmd *cmd;
+
+        /* If a Tx command is being handled and it isn't in the actual
+         * command queue then there a command routing bug has been introduced
+         * in the queue management code. */
+        if (txq_id != IWL_CMD_QUEUE_NUM)
+                IWL_ERROR("Error wrong command queue %d command id 0x%X\n",
+                          txq_id, pkt->hdr.cmd);
+        BUG_ON(txq_id != IWL_CMD_QUEUE_NUM);
+
+        cmd_index = get_cmd_index(&priv->txq[IWL_CMD_QUEUE_NUM].q, index, huge);
+        cmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_index];
+
+        /* Input error checking is done when commands are added to queue. */
+        if (cmd->meta.flags & CMD_WANT_SKB) {
+                cmd->meta.source->u.skb = rxb->skb;
+                rxb->skb = NULL;
+        } else if (cmd->meta.u.callback &&
+                   !cmd->meta.u.callback(priv, cmd, rxb->skb))
+                rxb->skb = NULL;
+
+        iwl_hcmd_queue_reclaim(priv, txq_id, index);
+
+        if (!(cmd->meta.flags & CMD_ASYNC)) {
+                clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+                wake_up_interruptible(&priv->wait_command_queue);
+        }
+}
+EXPORT_SYMBOL(iwl_tx_cmd_complete);
+
+
+#ifdef CONFIG_IWL4965_HT
+/*
+ * Find first available (lowest unused) Tx Queue, mark it "active".
+ * Called only when finding queue for aggregation.
+ * Should never return anything < 7, because they should already
+ * be in use as EDCA AC (0-3), Command (4), HCCA (5, 6).
+ */
+static int iwl_txq_ctx_activate_free(struct iwl_priv *priv)
+{
+        int txq_id;
+
+        for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+                if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
+                        return txq_id;
+        return -1;
+}
+
+int iwl_tx_agg_start(struct iwl_priv *priv, const u8 *ra, u16 tid, u16 *ssn)
+{
+        int sta_id;
+        int tx_fifo;
+        int txq_id;
+        int ret;
+        unsigned long flags;
+        struct iwl_tid_data *tid_data;
+        DECLARE_MAC_BUF(mac);
+
+        if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
+                tx_fifo = default_tid_to_tx_fifo[tid];
+        else
+                return -EINVAL;
+
+        IWL_WARNING("%s on ra = %s tid = %d\n",
+                        __func__, print_mac(mac, ra), tid);
+
+        sta_id = iwl_find_station(priv, ra);
+        if (sta_id == IWL_INVALID_STATION)
+                return -ENXIO;
+
+        if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
+                IWL_ERROR("Start AGG when state is not IWL_AGG_OFF !\n");
+                return -ENXIO;
+        }
+
+        txq_id = iwl_txq_ctx_activate_free(priv);
+        if (txq_id == -1)
+                return -ENXIO;
+
+        spin_lock_irqsave(&priv->sta_lock, flags);
+        tid_data = &priv->stations[sta_id].tid[tid];
+        *ssn = SEQ_TO_SN(tid_data->seq_number);
+        tid_data->agg.txq_id = txq_id;
+        spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+        ret = priv->cfg->ops->lib->txq_agg_enable(priv, txq_id, tx_fifo,
+                                                  sta_id, tid, *ssn);
+        if (ret)
+                return ret;
+
+        if (tid_data->tfds_in_queue == 0) {
+                printk(KERN_ERR "HW queue is empty\n");
+                tid_data->agg.state = IWL_AGG_ON;
+                ieee80211_start_tx_ba_cb_irqsafe(priv->hw, ra, tid);
+        } else {
+                IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
+                             tid_data->tfds_in_queue);
+                tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
+        }
+        return ret;
+}
+EXPORT_SYMBOL(iwl_tx_agg_start);
+
+int iwl_tx_agg_stop(struct iwl_priv *priv , const u8 *ra, u16 tid)
+{
+        int tx_fifo_id, txq_id, sta_id, ssn = -1;
+        struct iwl_tid_data *tid_data;
+        int ret, write_ptr, read_ptr;
+        unsigned long flags;
+        DECLARE_MAC_BUF(mac);
+
+        if (!ra) {
+                IWL_ERROR("ra = NULL\n");
+                return -EINVAL;
+        }
+
+        if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
+                tx_fifo_id = default_tid_to_tx_fifo[tid];
+        else
+                return -EINVAL;
+
+        sta_id = iwl_find_station(priv, ra);
+
+        if (sta_id == IWL_INVALID_STATION)
+                return -ENXIO;
+
+        if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_ON)
+                IWL_WARNING("Stopping AGG while state not IWL_AGG_ON\n");
+
+        tid_data = &priv->stations[sta_id].tid[tid];
+        ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
+        txq_id = tid_data->agg.txq_id;
+        write_ptr = priv->txq[txq_id].q.write_ptr;
+        read_ptr = priv->txq[txq_id].q.read_ptr;
+
+        /* The queue is not empty */
+        if (write_ptr != read_ptr) {
+                IWL_DEBUG_HT("Stopping a non empty AGG HW QUEUE\n");
+                priv->stations[sta_id].tid[tid].agg.state =
+                                IWL_EMPTYING_HW_QUEUE_DELBA;
+                return 0;
+        }
+
+        IWL_DEBUG_HT("HW queue is empty\n");
+        priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
+
+        spin_lock_irqsave(&priv->lock, flags);
+        ret = priv->cfg->ops->lib->txq_agg_disable(priv, txq_id, ssn,
+                                                   tx_fifo_id);
+        spin_unlock_irqrestore(&priv->lock, flags);
+
+        if (ret)
+                return ret;
+
+        ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, ra, tid);
+
+        return 0;
+}
+EXPORT_SYMBOL(iwl_tx_agg_stop);
+
+int iwl_txq_check_empty(struct iwl_priv *priv, int sta_id, u8 tid, int txq_id)
+{
+        struct iwl_queue *q = &priv->txq[txq_id].q;
+        u8 *addr = priv->stations[sta_id].sta.sta.addr;
+        struct iwl_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
+
+        switch (priv->stations[sta_id].tid[tid].agg.state) {
+        case IWL_EMPTYING_HW_QUEUE_DELBA:
+                /* We are reclaiming the last packet of the */
+                /* aggregated HW queue */
+                if (txq_id  == tid_data->agg.txq_id &&
+                    q->read_ptr == q->write_ptr) {
+                        u16 ssn = SEQ_TO_SN(tid_data->seq_number);
+                        int tx_fifo = default_tid_to_tx_fifo[tid];
+                        IWL_DEBUG_HT("HW queue empty: continue DELBA flow\n");
+                        priv->cfg->ops->lib->txq_agg_disable(priv, txq_id,
+                                                             ssn, tx_fifo);
+                        tid_data->agg.state = IWL_AGG_OFF;
+                        ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+                }
+                break;
+        case IWL_EMPTYING_HW_QUEUE_ADDBA:
+                /* We are reclaiming the last packet of the queue */
+                if (tid_data->tfds_in_queue == 0) {
+                        IWL_DEBUG_HT("HW queue empty: continue ADDBA flow\n");
+                        tid_data->agg.state = IWL_AGG_ON;
+                        ieee80211_start_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+                }
+                break;
+        }
+        return 0;
+}
+EXPORT_SYMBOL(iwl_txq_check_empty);
+#endif /* CONFIG_IWL4965_HT */
+
+#ifdef CONFIG_IWLWIF_DEBUG
+#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
+
+const char *iwl_get_tx_fail_reason(u32 status)
+{
+        switch (status & TX_STATUS_MSK) {
+        case TX_STATUS_SUCCESS:
+                return "SUCCESS";
+                TX_STATUS_ENTRY(SHORT_LIMIT);
+                TX_STATUS_ENTRY(LONG_LIMIT);
+                TX_STATUS_ENTRY(FIFO_UNDERRUN);
+                TX_STATUS_ENTRY(MGMNT_ABORT);
+                TX_STATUS_ENTRY(NEXT_FRAG);
+                TX_STATUS_ENTRY(LIFE_EXPIRE);
+                TX_STATUS_ENTRY(DEST_PS);
+                TX_STATUS_ENTRY(ABORTED);
+                TX_STATUS_ENTRY(BT_RETRY);
+                TX_STATUS_ENTRY(STA_INVALID);
+                TX_STATUS_ENTRY(FRAG_DROPPED);
+                TX_STATUS_ENTRY(TID_DISABLE);
+                TX_STATUS_ENTRY(FRAME_FLUSHED);
+                TX_STATUS_ENTRY(INSUFFICIENT_CF_POLL);
+                TX_STATUS_ENTRY(TX_LOCKED);
+                TX_STATUS_ENTRY(NO_BEACON_ON_RADAR);
+        }
+
+        return "UNKNOWN";
+}
+EXPORT_SYMBOL(iwl_get_tx_fail_reason);
+#endif /* CONFIG_IWLWIFI_DEBUG */
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index c1234ff4fc98..72279e07fe32 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -102,16 +102,6 @@ MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT);
 MODULE_LICENSE("GPL");
 
-static __le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr)
-{
-        u16 fc = le16_to_cpu(hdr->frame_control);
-        int hdr_len = ieee80211_get_hdrlen(fc);
-
-        if ((fc & 0x00cc) == (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA))
-                return (__le16 *) ((u8 *) hdr + hdr_len - QOS_CONTROL_LEN);
-        return NULL;
-}
-
 static const struct ieee80211_supported_band *iwl3945_get_band(
                 struct iwl3945_priv *priv, enum ieee80211_band band)
 {
@@ -2386,13 +2376,13 @@ static int iwl3945_set_mode(struct iwl3945_priv *priv, int mode)
 }
 
 static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
-                                      struct ieee80211_tx_control *ctl,
+                                      struct ieee80211_tx_info *info,
                                       struct iwl3945_cmd *cmd,
                                       struct sk_buff *skb_frag,
                                       int last_frag)
 {
         struct iwl3945_hw_key *keyinfo =
-            &priv->stations[ctl->hw_key->hw_key_idx].keyinfo;
+            &priv->stations[info->control.hw_key->hw_key_idx].keyinfo;
 
         switch (keyinfo->alg) {
         case ALG_CCMP:
@@ -2415,7 +2405,7 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
 
         case ALG_WEP:
                 cmd->cmd.tx.sec_ctl = TX_CMD_SEC_WEP |
-                    (ctl->hw_key->hw_key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
+                    (info->control.hw_key->hw_key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
 
                 if (keyinfo->keylen == 13)
                         cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
@@ -2423,7 +2413,7 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
                 memcpy(&cmd->cmd.tx.key[3], keyinfo->key, keyinfo->keylen);
 
                 IWL_DEBUG_TX("Configuring packet for WEP encryption "
-                             "with key %d\n", ctl->hw_key->hw_key_idx);
+                             "with key %d\n", info->control.hw_key->hw_key_idx);
                 break;
 
         default:
@@ -2437,16 +2427,15 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
  */
 static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
                                   struct iwl3945_cmd *cmd,
-                                  struct ieee80211_tx_control *ctrl,
+                                  struct ieee80211_tx_info *info,
                                   struct ieee80211_hdr *hdr,
                                   int is_unicast, u8 std_id)
 {
-        __le16 *qc;
         u16 fc = le16_to_cpu(hdr->frame_control);
         __le32 tx_flags = cmd->cmd.tx.tx_flags;
 
         cmd->cmd.tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
-        if (!(ctrl->flags & IEEE80211_TXCTL_NO_ACK)) {
+        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                 tx_flags |= TX_CMD_FLG_ACK_MSK;
                 if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
                         tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
@@ -2462,17 +2451,18 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
         if (ieee80211_get_morefrag(hdr))
                 tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
 
-        qc = ieee80211_get_qos_ctrl(hdr);
-        if (qc) {
-                cmd->cmd.tx.tid_tspec = (u8) (le16_to_cpu(*qc) & 0xf);
+        if (ieee80211_is_qos_data(fc)) {
+                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                cmd->cmd.tx.tid_tspec = qc[0] & 0xf;
                 tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
-        } else
+        } else {
                 tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+        }
 
-        if (ctrl->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
                 tx_flags |= TX_CMD_FLG_RTS_MSK;
                 tx_flags &= ~TX_CMD_FLG_CTS_MSK;
-        } else if (ctrl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+        } else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                 tx_flags &= ~TX_CMD_FLG_RTS_MSK;
                 tx_flags |= TX_CMD_FLG_CTS_MSK;
         }
@@ -2556,25 +2546,27 @@ static int iwl3945_get_sta_id(struct iwl3945_priv *priv, struct ieee80211_hdr *h
 /*
  * start REPLY_TX command process
  */
-static int iwl3945_tx_skb(struct iwl3945_priv *priv,
-                      struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
 {
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct iwl3945_tfd_frame *tfd;
         u32 *control_flags;
-        int txq_id = ctl->queue;
+        int txq_id = skb_get_queue_mapping(skb);
         struct iwl3945_tx_queue *txq = NULL;
         struct iwl3945_queue *q = NULL;
         dma_addr_t phys_addr;
         dma_addr_t txcmd_phys;
         struct iwl3945_cmd *out_cmd = NULL;
-        u16 len, idx, len_org;
-        u8 id, hdr_len, unicast;
+        u16 len, idx, len_org, hdr_len;
+        u8 id;
+        u8 unicast;
         u8 sta_id;
+        u8 tid = 0;
         u16 seq_number = 0;
         u16 fc;
-        __le16 *qc;
         u8 wait_write_ptr = 0;
+        u8 *qc = NULL;
         unsigned long flags;
         int rc;
 
@@ -2589,7 +2581,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
                 goto drop_unlock;
         }
 
-        if ((ctl->tx_rate->hw_value & 0xFF) == IWL_INVALID_RATE) {
+        if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xFF) == IWL_INVALID_RATE) {
                 IWL_ERROR("ERROR: No TX rate available.\n");
                 goto drop_unlock;
         }
@@ -2632,9 +2624,9 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 
         IWL_DEBUG_RATE("station Id %d\n", sta_id);
 
-        qc = ieee80211_get_qos_ctrl(hdr);
-        if (qc) {
-                u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
+        if (ieee80211_is_qos_data(fc)) {
+                qc = ieee80211_get_qos_ctrl(hdr, hdr_len);
+                tid = qc[0] & 0xf;
                 seq_number = priv->stations[sta_id].tid[tid].seq_number &
                                 IEEE80211_SCTL_SEQ;
                 hdr->seq_ctrl = cpu_to_le16(seq_number) |
@@ -2658,8 +2650,6 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
         /* Set up driver data for this TFD */
         memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl3945_tx_info));
         txq->txb[q->write_ptr].skb[0] = skb;
-        memcpy(&(txq->txb[q->write_ptr].status.control),
-               ctl, sizeof(struct ieee80211_tx_control));
 
         /* Init first empty entry in queue's array of Tx/cmd buffers */
         out_cmd = &txq->cmd[idx];
@@ -2708,8 +2698,8 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
          * first entry */
         iwl3945_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
 
-        if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
-                iwl3945_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, 0);
+        if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT))
+                iwl3945_build_tx_cmd_hwcrypto(priv, info, out_cmd, skb, 0);
 
         /* Set up TFD's 2nd entry to point directly to remainder of skb,
          * if any (802.11 null frames have no payload). */
@@ -2734,10 +2724,10 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
         out_cmd->cmd.tx.len = cpu_to_le16(len);
 
         /* TODO need this for burst mode later on */
-        iwl3945_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
+        iwl3945_build_tx_cmd_basic(priv, out_cmd, info, hdr, unicast, sta_id);
 
         /* set is_hcca to 0; it probably will never be implemented */
-        iwl3945_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
+        iwl3945_hw_build_tx_cmd_rate(priv, out_cmd, info, hdr, sta_id, 0);
 
         out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_A_MSK;
         out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_B_MSK;
@@ -2745,7 +2735,6 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
         if (!ieee80211_get_morefrag(hdr)) {
                 txq->need_update = 1;
                 if (qc) {
-                        u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
                         priv->stations[sta_id].tid[tid].seq_number = seq_number;
                 }
         } else {
@@ -2776,7 +2765,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
                         spin_unlock_irqrestore(&priv->lock, flags);
                 }
 
-                ieee80211_stop_queue(priv->hw, ctl->queue);
+                ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
         }
 
         return 0;
@@ -3239,7 +3228,7 @@ static void iwl3945_bg_beacon_update(struct work_struct *work)
         struct sk_buff *beacon;
 
         /* Pull updated AP beacon from mac80211. will fail if not in AP mode */
-        beacon = ieee80211_beacon_get(priv->hw, priv->vif, NULL);
+        beacon = ieee80211_beacon_get(priv->hw, priv->vif);
 
         if (!beacon) {
                 IWL_ERROR("update beacon failed\n");
@@ -5832,7 +5821,7 @@ static void iwl3945_alive_start(struct iwl3945_priv *priv)
         if (iwl3945_is_rfkill(priv))
                 return;
 
-        ieee80211_start_queues(priv->hw);
+        ieee80211_wake_queues(priv->hw);
 
         priv->active_rate = priv->rates_mask;
         priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
@@ -5858,9 +5847,6 @@ static void iwl3945_alive_start(struct iwl3945_priv *priv)
         /* Configure the adapter for unassociated operation */
         iwl3945_commit_rxon(priv);
 
-        /* At this point, the NIC is initialized and operational */
-        priv->notif_missed_beacons = 0;
-
         iwl3945_reg_txpower_periodic(priv);
 
         iwl3945_led_register(priv);
@@ -6690,8 +6676,7 @@ static void iwl3945_mac_stop(struct ieee80211_hw *hw)
         IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                      struct ieee80211_tx_control *ctl)
+static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct iwl3945_priv *priv = hw->priv;
 
@@ -6703,9 +6688,9 @@ static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
         }
 
         IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
-                     ctl->tx_rate->bitrate);
+                     ieee80211_get_tx_rate(hw, IEEE80211_SKB_CB(skb))->bitrate);
 
-        if (iwl3945_tx_skb(priv, skb, ctl))
+        if (iwl3945_tx_skb(priv, skb))
                 dev_kfree_skb_any(skb);
 
         IWL_DEBUG_MAC80211("leave\n");
@@ -7342,8 +7327,7 @@ static void iwl3945_mac_reset_tsf(struct ieee80211_hw *hw)
 
 }
 
-static int iwl3945_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                                 struct ieee80211_tx_control *control)
+static int iwl3945_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct iwl3945_priv *priv = hw->priv;
         unsigned long flags;
@@ -8273,7 +8257,7 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 
         iwl3945_free_channel_map(priv);
         iwl3945_free_geos(priv);
-
+        kfree(priv->scan);
         if (priv->ibss_beacon)
                 dev_kfree_skb(priv->ibss_beacon);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl4965-base.c b/drivers/net/wireless/iwlwifi/iwl4965-base.c
index 55ca752ae9e6..c71daec8c746 100644
--- a/drivers/net/wireless/iwlwifi/iwl4965-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl4965-base.c
@@ -53,8 +53,6 @@
 #include "iwl-sta.h"
 #include "iwl-calib.h"
 
-static int iwl4965_tx_queue_update_write_ptr(struct iwl_priv *priv,
-                                  struct iwl_tx_queue *txq);
 
 /******************************************************************************
  *
@@ -89,22 +87,6 @@ MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT);
 MODULE_LICENSE("GPL");
 
-__le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr)
-{
-        u16 fc = le16_to_cpu(hdr->frame_control);
-        int hdr_len = ieee80211_get_hdrlen(fc);
-
-        if ((fc & 0x00cc) == (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA))
-                return (__le16 *) ((u8 *) hdr + hdr_len - QOS_CONTROL_LEN);
-        return NULL;
-}
-
-static const struct ieee80211_supported_band *iwl_get_hw_mode(
-                struct iwl_priv *priv, enum ieee80211_band band)
-{
-        return priv->hw->wiphy->bands[band];
-}
-
 static int iwl4965_is_empty_essid(const char *essid, int essid_len)
 {
         /* Single white space is for Linksys APs */
@@ -145,70 +127,6 @@ static const char *iwl4965_escape_essid(const char *essid, u8 essid_len)
         return escaped;
 }
 
-
-/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
- * DMA services
- *
- * Theory of operation
- *
- * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
- * of buffer descriptors, each of which points to one or more data buffers for
- * the device to read from or fill.  Driver and device exchange status of each
- * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
- * entries in each circular buffer, to protect against confusing empty and full
- * queue states.
- *
- * The device reads or writes the data in the queues via the device's several
- * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
- *
- * For Tx queue, there are low mark and high mark limits. If, after queuing
- * the packet for Tx, free space become < low mark, Tx queue stopped. When
- * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
- * Tx queue resumed.
- *
- * The 4965 operates with up to 17 queues:  One receive queue, one transmit
- * queue (#4) for sending commands to the device firmware, and 15 other
- * Tx queues that may be mapped to prioritized Tx DMA/FIFO channels.
- *
- * See more detailed info in iwl-4965-hw.h.
- ***************************************************/
-
-int iwl4965_queue_space(const struct iwl4965_queue *q)
-{
-        int s = q->read_ptr - q->write_ptr;
-
-        if (q->read_ptr > q->write_ptr)
-                s -= q->n_bd;
-
-        if (s <= 0)
-                s += q->n_window;
-        /* keep some reserve to not confuse empty and full situations */
-        s -= 2;
-        if (s < 0)
-                s = 0;
-        return s;
-}
-
-
-static inline int x2_queue_used(const struct iwl4965_queue *q, int i)
-{
-        return q->write_ptr > q->read_ptr ?
-                (i >= q->read_ptr && i < q->write_ptr) :
-                !(i < q->read_ptr && i >= q->write_ptr);
-}
-
-static inline u8 get_cmd_index(struct iwl4965_queue *q, u32 index, int is_huge)
-{
-        /* This is for scan command, the big buffer at end of command array */
-        if (is_huge)
-                return q->n_window;     /* must be power of 2 */
-
-        /* Otherwise, use normal size buffers */
-        return index & (q->n_window - 1);
-}
-
-const u8 iwl4965_broadcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-
 /*************** STATION TABLE MANAGEMENT ****
  * mac80211 should be examined to determine if sta_info is duplicating
  * the functionality provided here
@@ -216,213 +134,11 @@ const u8 iwl4965_broadcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
 
 /**************************************************************/
 
-#if 0 /* temporary disable till we add real remove station */
-/**
- * iwl4965_remove_station - Remove driver's knowledge of station.
- *
- * NOTE:  This does not remove station from device's station table.
- */
-static u8 iwl4965_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
-{
-        int index = IWL_INVALID_STATION;
-        int i;
-        unsigned long flags;
-
-        spin_lock_irqsave(&priv->sta_lock, flags);
 
-        if (is_ap)
-                index = IWL_AP_ID;
-        else if (is_broadcast_ether_addr(addr))
-                index = priv->hw_params.bcast_sta_id;
-        else
-                for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++)
-                        if (priv->stations[i].used &&
-                            !compare_ether_addr(priv->stations[i].sta.sta.addr,
-                                                addr)) {
-                                index = i;
-                                break;
-                        }
-
-        if (unlikely(index == IWL_INVALID_STATION))
-                goto out;
-
-        if (priv->stations[index].used) {
-                priv->stations[index].used = 0;
-                priv->num_stations--;
-        }
-
-        BUG_ON(priv->num_stations < 0);
-
-out:
-        spin_unlock_irqrestore(&priv->sta_lock, flags);
-        return 0;
-}
-#endif
-
-/**
- * iwl4965_add_station_flags - Add station to tables in driver and device
- */
-u8 iwl4965_add_station_flags(struct iwl_priv *priv, const u8 *addr,
-                                int is_ap, u8 flags, void *ht_data)
-{
-        int i;
-        int index = IWL_INVALID_STATION;
-        struct iwl_station_entry *station;
-        unsigned long flags_spin;
-        DECLARE_MAC_BUF(mac);
-
-        spin_lock_irqsave(&priv->sta_lock, flags_spin);
-        if (is_ap)
-                index = IWL_AP_ID;
-        else if (is_broadcast_ether_addr(addr))
-                index = priv->hw_params.bcast_sta_id;
-        else
-                for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++) {
-                        if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
-                                                addr)) {
-                                index = i;
-                                break;
-                        }
-
-                        if (!priv->stations[i].used &&
-                            index == IWL_INVALID_STATION)
-                                index = i;
-                }
-
-
-        /* These two conditions have the same outcome, but keep them separate
-          since they have different meanings */
-        if (unlikely(index == IWL_INVALID_STATION)) {
-                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
-                return index;
-        }
-
-        if (priv->stations[index].used &&
-            !compare_ether_addr(priv->stations[index].sta.sta.addr, addr)) {
-                spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
-                return index;
-        }
-
-
-        IWL_DEBUG_ASSOC("Add STA ID %d: %s\n", index, print_mac(mac, addr));
-        station = &priv->stations[index];
-        station->used = 1;
-        priv->num_stations++;
-
-        /* Set up the REPLY_ADD_STA command to send to device */
-        memset(&station->sta, 0, sizeof(struct iwl_addsta_cmd));
-        memcpy(station->sta.sta.addr, addr, ETH_ALEN);
-        station->sta.mode = 0;
-        station->sta.sta.sta_id = index;
-        station->sta.station_flags = 0;
-
-#ifdef CONFIG_IWL4965_HT
-        /* BCAST station and IBSS stations do not work in HT mode */
-        if (index != priv->hw_params.bcast_sta_id &&
-            priv->iw_mode != IEEE80211_IF_TYPE_IBSS)
-                iwl4965_set_ht_add_station(priv, index,
-                                 (struct ieee80211_ht_info *) ht_data);
-#endif /*CONFIG_IWL4965_HT*/
-
-        spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
-
-        /* Add station to device's station table */
-        iwl_send_add_sta(priv, &station->sta, flags);
-        return index;
-
-}
-
-
-
-/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
-
-/**
- * iwl4965_enqueue_hcmd - enqueue a uCode command
- * @priv: device private data point
- * @cmd: a point to the ucode command structure
- *
- * The function returns < 0 values to indicate the operation is
- * failed. On success, it turns the index (> 0) of command in the
- * command queue.
- */
-int iwl4965_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
-{
-        struct iwl_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
-        struct iwl4965_queue *q = &txq->q;
-        struct iwl_tfd_frame *tfd;
-        u32 *control_flags;
-        struct iwl_cmd *out_cmd;
-        u32 idx;
-        u16 fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
-        dma_addr_t phys_addr;
-        int ret;
-        unsigned long flags;
-
-        /* If any of the command structures end up being larger than
-         * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
-         * we will need to increase the size of the TFD entries */
-        BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
-               !(cmd->meta.flags & CMD_SIZE_HUGE));
-
-        if (iwl_is_rfkill(priv)) {
-                IWL_DEBUG_INFO("Not sending command - RF KILL");
-                return -EIO;
-        }
-
-        if (iwl4965_queue_space(q) < ((cmd->meta.flags & CMD_ASYNC) ? 2 : 1)) {
-                IWL_ERROR("No space for Tx\n");
-                return -ENOSPC;
-        }
-
-        spin_lock_irqsave(&priv->hcmd_lock, flags);
-
-        tfd = &txq->bd[q->write_ptr];
-        memset(tfd, 0, sizeof(*tfd));
-
-        control_flags = (u32 *) tfd;
-
-        idx = get_cmd_index(q, q->write_ptr, cmd->meta.flags & CMD_SIZE_HUGE);
-        out_cmd = &txq->cmd[idx];
-
-        out_cmd->hdr.cmd = cmd->id;
-        memcpy(&out_cmd->meta, &cmd->meta, sizeof(cmd->meta));
-        memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);
-
-        /* At this point, the out_cmd now has all of the incoming cmd
-         * information */
-
-        out_cmd->hdr.flags = 0;
-        out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(IWL_CMD_QUEUE_NUM) |
-                        INDEX_TO_SEQ(q->write_ptr));
-        if (out_cmd->meta.flags & CMD_SIZE_HUGE)
-                out_cmd->hdr.sequence |= cpu_to_le16(SEQ_HUGE_FRAME);
-
-        phys_addr = txq->dma_addr_cmd + sizeof(txq->cmd[0]) * idx +
-                        offsetof(struct iwl_cmd, hdr);
-        iwl4965_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, fix_size);
-
-        IWL_DEBUG_HC("Sending command %s (#%x), seq: 0x%04X, "
-                     "%d bytes at %d[%d]:%d\n",
-                     get_cmd_string(out_cmd->hdr.cmd),
-                     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
-                     fix_size, q->write_ptr, idx, IWL_CMD_QUEUE_NUM);
-
-        txq->need_update = 1;
-
-        /* Set up entry in queue's byte count circular buffer */
-        priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, 0);
-
-        /* Increment and update queue's write index */
-        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
-        ret = iwl4965_tx_queue_update_write_ptr(priv, txq);
-
-        spin_unlock_irqrestore(&priv->hcmd_lock, flags);
-        return ret ? ret : idx;
-}
 
 static void iwl4965_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
 {
-        struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
+        struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
 
         if (hw_decrypt)
                 rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
@@ -432,45 +148,13 @@ static void iwl4965_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
 }
 
 /**
- * iwl4965_rxon_add_station - add station into station table.
- *
- * there is only one AP station with id= IWL_AP_ID
- * NOTE: mutex must be held before calling this fnction
- */
-static int iwl4965_rxon_add_station(struct iwl_priv *priv,
-                                const u8 *addr, int is_ap)
-{
-        u8 sta_id;
-
-        /* Add station to device's station table */
-#ifdef CONFIG_IWL4965_HT
-        struct ieee80211_conf *conf = &priv->hw->conf;
-        struct ieee80211_ht_info *cur_ht_config = &conf->ht_conf;
-
-        if ((is_ap) &&
-            (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
-            (priv->iw_mode == IEEE80211_IF_TYPE_STA))
-                sta_id = iwl4965_add_station_flags(priv, addr, is_ap,
-                                                   0, cur_ht_config);
-        else
-#endif /* CONFIG_IWL4965_HT */
-                sta_id = iwl4965_add_station_flags(priv, addr, is_ap,
-                                                   0, NULL);
-
-        /* Set up default rate scaling table in device's station table */
-        iwl4965_add_station(priv, addr, is_ap);
-
-        return sta_id;
-}
-
-/**
  * iwl4965_check_rxon_cmd - validate RXON structure is valid
  *
  * NOTE:  This is really only useful during development and can eventually
  * be #ifdef'd out once the driver is stable and folks aren't actively
  * making changes
  */
-static int iwl4965_check_rxon_cmd(struct iwl4965_rxon_cmd *rxon)
+static int iwl4965_check_rxon_cmd(struct iwl_rxon_cmd *rxon)
 {
         int error = 0;
         int counter = 1;
@@ -595,7 +279,7 @@ static int iwl4965_full_rxon_required(struct iwl_priv *priv)
 static int iwl4965_commit_rxon(struct iwl_priv *priv)
 {
         /* cast away the const for active_rxon in this function */
-        struct iwl4965_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
+        struct iwl_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
         DECLARE_MAC_BUF(mac);
         int rc = 0;
 
@@ -640,7 +324,7 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
                 active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 
                 rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
-                                      sizeof(struct iwl4965_rxon_cmd),
+                                      sizeof(struct iwl_rxon_cmd),
                                       &priv->active_rxon);
 
                 /* If the mask clearing failed then we set
@@ -665,12 +349,13 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
         iwl4965_set_rxon_hwcrypto(priv, !priv->hw_params.sw_crypto);
         /* Apply the new configuration */
         rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
-                              sizeof(struct iwl4965_rxon_cmd), &priv->staging_rxon);
+                              sizeof(struct iwl_rxon_cmd), &priv->staging_rxon);
         if (rc) {
                 IWL_ERROR("Error setting new configuration (%d).\n", rc);
                 return rc;
         }
 
+        iwl_remove_station(priv, iwl_bcast_addr, 0);
         iwlcore_clear_stations_table(priv);
 
         if (!priv->error_recovering)
@@ -689,7 +374,7 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
         }
 
         /* Add the broadcast address so we can send broadcast frames */
-        if (iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0) ==
+        if (iwl_rxon_add_station(priv, iwl_bcast_addr, 0) ==
             IWL_INVALID_STATION) {
                 IWL_ERROR("Error adding BROADCAST address for transmit.\n");
                 return -EIO;
@@ -699,7 +384,7 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
          * add the IWL_AP_ID to the station rate table */
         if (iwl_is_associated(priv) &&
             (priv->iw_mode == IEEE80211_IF_TYPE_STA)) {
-                if (iwl4965_rxon_add_station(priv, priv->active_rxon.bssid_addr, 1)
+                if (iwl_rxon_add_station(priv, priv->active_rxon.bssid_addr, 1)
                     == IWL_INVALID_STATION) {
                         IWL_ERROR("Error adding AP address for transmit.\n");
                         return -EIO;
@@ -797,7 +482,7 @@ static int iwl4965_send_card_state(struct iwl_priv *priv, u32 flags, u8 meta_fla
         return iwl_send_cmd(priv, &cmd);
 }
 
-static void iwl4965_clear_free_frames(struct iwl_priv *priv)
+static void iwl_clear_free_frames(struct iwl_priv *priv)
 {
         struct list_head *element;
 
@@ -807,7 +492,7 @@ static void iwl4965_clear_free_frames(struct iwl_priv *priv)
         while (!list_empty(&priv->free_frames)) {
                 element = priv->free_frames.next;
                 list_del(element);
-                kfree(list_entry(element, struct iwl4965_frame, list));
+                kfree(list_entry(element, struct iwl_frame, list));
                 priv->frames_count--;
         }
 
@@ -818,9 +503,9 @@ static void iwl4965_clear_free_frames(struct iwl_priv *priv)
         }
 }
 
-static struct iwl4965_frame *iwl4965_get_free_frame(struct iwl_priv *priv)
+static struct iwl_frame *iwl_get_free_frame(struct iwl_priv *priv)
 {
-        struct iwl4965_frame *frame;
+        struct iwl_frame *frame;
         struct list_head *element;
         if (list_empty(&priv->free_frames)) {
                 frame = kzalloc(sizeof(*frame), GFP_KERNEL);
@@ -835,10 +520,10 @@ static struct iwl4965_frame *iwl4965_get_free_frame(struct iwl_priv *priv)
 
         element = priv->free_frames.next;
         list_del(element);
-        return list_entry(element, struct iwl4965_frame, list);
+        return list_entry(element, struct iwl_frame, list);
 }
 
-static void iwl4965_free_frame(struct iwl_priv *priv, struct iwl4965_frame *frame)
+static void iwl_free_frame(struct iwl_priv *priv, struct iwl_frame *frame)
 {
         memset(frame, 0, sizeof(*frame));
         list_add(&frame->list, &priv->free_frames);
@@ -875,9 +560,9 @@ static u8 iwl4965_rate_get_lowest_plcp(struct iwl_priv *priv)
 
         /* Find lowest valid rate */
         for (i = IWL_RATE_1M_INDEX; i != IWL_RATE_INVALID;
-                                        i = iwl4965_rates[i].next_ieee) {
+                                        i = iwl_rates[i].next_ieee) {
                 if (rate_mask & (1 << i))
-                        return iwl4965_rates[i].plcp;
+                        return iwl_rates[i].plcp;
         }
 
         /* No valid rate was found. Assign the lowest one */
@@ -889,12 +574,12 @@ static u8 iwl4965_rate_get_lowest_plcp(struct iwl_priv *priv)
 
 static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
 {
-        struct iwl4965_frame *frame;
+        struct iwl_frame *frame;
         unsigned int frame_size;
         int rc;
         u8 rate;
 
-        frame = iwl4965_get_free_frame(priv);
+        frame = iwl_get_free_frame(priv);
 
         if (!frame) {
                 IWL_ERROR("Could not obtain free frame buffer for beacon "
@@ -909,7 +594,7 @@ static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
         rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
                               &frame->u.cmd[0]);
 
-        iwl4965_free_frame(priv, frame);
+        iwl_free_frame(priv, frame);
 
         return rc;
 }
@@ -936,7 +621,7 @@ static u16 iwl4965_supported_rate_to_ie(u8 *ie, u16 supported_rate,
         for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
                 if (bit & supported_rate) {
                         ret_rates |= bit;
-                        rates[*cnt] = iwl4965_rates[i].ieee |
+                        rates[*cnt] = iwl_rates[i].ieee |
                                 ((bit & basic_rate) ? 0x80 : 0x00);
                         (*cnt)++;
                         (*left)--;
@@ -967,9 +652,9 @@ static void iwl4965_ht_conf(struct iwl_priv *priv,
         priv->ps_mode = (u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
 
         if (ht_conf->cap & IEEE80211_HT_CAP_SGI_20)
-                iwl_conf->sgf |= 0x1;
+                iwl_conf->sgf |= HT_SHORT_GI_20MHZ;
         if (ht_conf->cap & IEEE80211_HT_CAP_SGI_40)
-                iwl_conf->sgf |= 0x2;
+                iwl_conf->sgf |= HT_SHORT_GI_40MHZ;
 
         iwl_conf->is_green_field = !!(ht_conf->cap & IEEE80211_HT_CAP_GRN_FLD);
         iwl_conf->max_amsdu_size =
@@ -1056,9 +741,9 @@ static u16 iwl4965_fill_probe_req(struct iwl_priv *priv,
         len += 24;
 
         frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-        memcpy(frame->da, iwl4965_broadcast_addr, ETH_ALEN);
+        memcpy(frame->da, iwl_bcast_addr, ETH_ALEN);
         memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
-        memcpy(frame->bssid, iwl4965_broadcast_addr, ETH_ALEN);
+        memcpy(frame->bssid, iwl_bcast_addr, ETH_ALEN);
         frame->seq_ctrl = 0;
 
         /* fill in our indirect SSID IE */
@@ -1224,33 +909,7 @@ int iwl4965_is_network_packet(struct iwl_priv *priv, struct ieee80211_hdr *heade
         return 1;
 }
 
-#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
-
-static const char *iwl4965_get_tx_fail_reason(u32 status)
-{
-        switch (status & TX_STATUS_MSK) {
-        case TX_STATUS_SUCCESS:
-                return "SUCCESS";
-                TX_STATUS_ENTRY(SHORT_LIMIT);
-                TX_STATUS_ENTRY(LONG_LIMIT);
-                TX_STATUS_ENTRY(FIFO_UNDERRUN);
-                TX_STATUS_ENTRY(MGMNT_ABORT);
-                TX_STATUS_ENTRY(NEXT_FRAG);
-                TX_STATUS_ENTRY(LIFE_EXPIRE);
-                TX_STATUS_ENTRY(DEST_PS);
-                TX_STATUS_ENTRY(ABORTED);
-                TX_STATUS_ENTRY(BT_RETRY);
-                TX_STATUS_ENTRY(STA_INVALID);
-                TX_STATUS_ENTRY(FRAG_DROPPED);
-                TX_STATUS_ENTRY(TID_DISABLE);
-                TX_STATUS_ENTRY(FRAME_FLUSHED);
-                TX_STATUS_ENTRY(INSUFFICIENT_CF_POLL);
-                TX_STATUS_ENTRY(TX_LOCKED);
-                TX_STATUS_ENTRY(NO_BEACON_ON_RADAR);
-        }
 
-        return "UNKNOWN";
-}
 
 /**
  * iwl4965_scan_cancel - Cancel any currently executing HW scan
@@ -1425,8 +1084,8 @@ static int iwl4965_scan_initiate(struct iwl_priv *priv)
 }
 
 
-static void iwl4965_set_flags_for_phymode(struct iwl_priv *priv,
-                                          enum ieee80211_band band)
+static void iwl_set_flags_for_band(struct iwl_priv *priv,
+                                   enum ieee80211_band band)
 {
         if (band == IEEE80211_BAND_5GHZ) {
                 priv->staging_rxon.flags &=
@@ -1511,7 +1170,7 @@ static void iwl4965_connection_init_rx_config(struct iwl_priv *priv)
         priv->staging_rxon.channel = cpu_to_le16(ch_info->channel);
         priv->band = ch_info->band;
 
-        iwl4965_set_flags_for_phymode(priv, priv->band);
+        iwl_set_flags_for_band(priv, priv->band);
 
         priv->staging_rxon.ofdm_basic_rates =
             (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
@@ -1566,441 +1225,6 @@ static int iwl4965_set_mode(struct iwl_priv *priv, int mode)
         return 0;
 }
 
-static void iwl4965_build_tx_cmd_hwcrypto(struct iwl_priv *priv,
-                                      struct ieee80211_tx_control *ctl,
-                                      struct iwl_cmd *cmd,
-                                      struct sk_buff *skb_frag,
-                                      int sta_id)
-{
-        struct iwl_hw_key *keyinfo = &priv->stations[sta_id].keyinfo;
-        struct iwl_wep_key *wepkey;
-        int keyidx = 0;
-
-        BUG_ON(ctl->hw_key->hw_key_idx > 3);
-
-        switch (keyinfo->alg) {
-        case ALG_CCMP:
-                cmd->cmd.tx.sec_ctl = TX_CMD_SEC_CCM;
-                memcpy(cmd->cmd.tx.key, keyinfo->key, keyinfo->keylen);
-                if (ctl->flags & IEEE80211_TXCTL_AMPDU)
-                        cmd->cmd.tx.tx_flags |= TX_CMD_FLG_AGG_CCMP_MSK;
-                IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
-                break;
-
-        case ALG_TKIP:
-                cmd->cmd.tx.sec_ctl = TX_CMD_SEC_TKIP;
-                ieee80211_get_tkip_key(keyinfo->conf, skb_frag,
-                        IEEE80211_TKIP_P2_KEY, cmd->cmd.tx.key);
-                IWL_DEBUG_TX("tx_cmd with tkip hwcrypto\n");
-                break;
-
-        case ALG_WEP:
-                wepkey = &priv->wep_keys[ctl->hw_key->hw_key_idx];
-                cmd->cmd.tx.sec_ctl = 0;
-                if (priv->default_wep_key) {
-                        /* the WEP key was sent as static */
-                        keyidx = ctl->hw_key->hw_key_idx;
-                        memcpy(&cmd->cmd.tx.key[3], wepkey->key,
-                                                        wepkey->key_size);
-                        if (wepkey->key_size == WEP_KEY_LEN_128)
-                                cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
-                } else {
-                        /* the WEP key was sent as dynamic */
-                        keyidx = keyinfo->keyidx;
-                        memcpy(&cmd->cmd.tx.key[3], keyinfo->key,
-                                                        keyinfo->keylen);
-                        if (keyinfo->keylen == WEP_KEY_LEN_128)
-                                cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
-                }
-
-                cmd->cmd.tx.sec_ctl |= (TX_CMD_SEC_WEP |
-                        (keyidx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT);
-
-                IWL_DEBUG_TX("Configuring packet for WEP encryption "
-                             "with key %d\n", keyidx);
-                break;
-
-        default:
-                printk(KERN_ERR "Unknown encode alg %d\n", keyinfo->alg);
-                break;
-        }
-}
-
-/*
- * handle build REPLY_TX command notification.
- */
-static void iwl4965_build_tx_cmd_basic(struct iwl_priv *priv,
-                                  struct iwl_cmd *cmd,
-                                  struct ieee80211_tx_control *ctrl,
-                                  struct ieee80211_hdr *hdr,
-                                  int is_unicast, u8 std_id)
-{
-        __le16 *qc;
-        u16 fc = le16_to_cpu(hdr->frame_control);
-        __le32 tx_flags = cmd->cmd.tx.tx_flags;
-
-        cmd->cmd.tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
-        if (!(ctrl->flags & IEEE80211_TXCTL_NO_ACK)) {
-                tx_flags |= TX_CMD_FLG_ACK_MSK;
-                if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
-                        tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-                if (ieee80211_is_probe_response(fc) &&
-                    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
-                        tx_flags |= TX_CMD_FLG_TSF_MSK;
-        } else {
-                tx_flags &= (~TX_CMD_FLG_ACK_MSK);
-                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-        }
-
-        if (ieee80211_is_back_request(fc))
-                tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
-
-
-        cmd->cmd.tx.sta_id = std_id;
-        if (ieee80211_get_morefrag(hdr))
-                tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
-
-        qc = ieee80211_get_qos_ctrl(hdr);
-        if (qc) {
-                cmd->cmd.tx.tid_tspec = (u8) (le16_to_cpu(*qc) & 0xf);
-                tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
-        } else
-                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-
-        if (ctrl->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-                tx_flags |= TX_CMD_FLG_RTS_MSK;
-                tx_flags &= ~TX_CMD_FLG_CTS_MSK;
-        } else if (ctrl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
-                tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-                tx_flags |= TX_CMD_FLG_CTS_MSK;
-        }
-
-        if ((tx_flags & TX_CMD_FLG_RTS_MSK) || (tx_flags & TX_CMD_FLG_CTS_MSK))
-                tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
-
-        tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
-        if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
-                if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
-                    (fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ)
-                        cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(3);
-                else
-                        cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(2);
-        } else {
-                cmd->cmd.tx.timeout.pm_frame_timeout = 0;
-        }
-
-        cmd->cmd.tx.driver_txop = 0;
-        cmd->cmd.tx.tx_flags = tx_flags;
-        cmd->cmd.tx.next_frame_len = 0;
-}
-static void iwl_update_tx_stats(struct iwl_priv *priv, u16 fc, u16 len)
-{
-        /* 0 - mgmt, 1 - cnt, 2 - data */
-        int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
-        priv->tx_stats[idx].cnt++;
-        priv->tx_stats[idx].bytes += len;
-}
-/**
- * iwl4965_get_sta_id - Find station's index within station table
- *
- * If new IBSS station, create new entry in station table
- */
-static int iwl4965_get_sta_id(struct iwl_priv *priv,
-                                struct ieee80211_hdr *hdr)
-{
-        int sta_id;
-        u16 fc = le16_to_cpu(hdr->frame_control);
-        DECLARE_MAC_BUF(mac);
-
-        /* If this frame is broadcast or management, use broadcast station id */
-        if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
-            is_multicast_ether_addr(hdr->addr1))
-                return priv->hw_params.bcast_sta_id;
-
-        switch (priv->iw_mode) {
-
-        /* If we are a client station in a BSS network, use the special
-         * AP station entry (that's the only station we communicate with) */
-        case IEEE80211_IF_TYPE_STA:
-                return IWL_AP_ID;
-
-        /* If we are an AP, then find the station, or use BCAST */
-        case IEEE80211_IF_TYPE_AP:
-                sta_id = iwl_find_station(priv, hdr->addr1);
-                if (sta_id != IWL_INVALID_STATION)
-                        return sta_id;
-                return priv->hw_params.bcast_sta_id;
-
-        /* If this frame is going out to an IBSS network, find the station,
-         * or create a new station table entry */
-        case IEEE80211_IF_TYPE_IBSS:
-                sta_id = iwl_find_station(priv, hdr->addr1);
-                if (sta_id != IWL_INVALID_STATION)
-                        return sta_id;
-
-                /* Create new station table entry */
-                sta_id = iwl4965_add_station_flags(priv, hdr->addr1,
-                                                   0, CMD_ASYNC, NULL);
-
-                if (sta_id != IWL_INVALID_STATION)
-                        return sta_id;
-
-                IWL_DEBUG_DROP("Station %s not in station map. "
-                               "Defaulting to broadcast...\n",
-                               print_mac(mac, hdr->addr1));
-                iwl_print_hex_dump(priv, IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
-                return priv->hw_params.bcast_sta_id;
-
-        default:
-                IWL_WARNING("Unknown mode of operation: %d", priv->iw_mode);
-                return priv->hw_params.bcast_sta_id;
-        }
-}
-
-/*
- * start REPLY_TX command process
- */
-static int iwl4965_tx_skb(struct iwl_priv *priv,
-                      struct sk_buff *skb, struct ieee80211_tx_control *ctl)
-{
-        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-        struct iwl_tfd_frame *tfd;
-        u32 *control_flags;
-        int txq_id = ctl->queue;
-        struct iwl_tx_queue *txq = NULL;
-        struct iwl4965_queue *q = NULL;
-        dma_addr_t phys_addr;
-        dma_addr_t txcmd_phys;
-        dma_addr_t scratch_phys;
-        struct iwl_cmd *out_cmd = NULL;
-        u16 len, idx, len_org;
-        u8 id, hdr_len, unicast;
-        u8 sta_id;
-        u16 seq_number = 0;
-        u16 fc;
-        __le16 *qc;
-        u8 wait_write_ptr = 0;
-        unsigned long flags;
-        int rc;
-
-        spin_lock_irqsave(&priv->lock, flags);
-        if (iwl_is_rfkill(priv)) {
-                IWL_DEBUG_DROP("Dropping - RF KILL\n");
-                goto drop_unlock;
-        }
-
-        if (!priv->vif) {
-                IWL_DEBUG_DROP("Dropping - !priv->vif\n");
-                goto drop_unlock;
-        }
-
-        if ((ctl->tx_rate->hw_value & 0xFF) == IWL_INVALID_RATE) {
-                IWL_ERROR("ERROR: No TX rate available.\n");
-                goto drop_unlock;
-        }
-
-        unicast = !is_multicast_ether_addr(hdr->addr1);
-        id = 0;
-
-        fc = le16_to_cpu(hdr->frame_control);
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-        if (ieee80211_is_auth(fc))
-                IWL_DEBUG_TX("Sending AUTH frame\n");
-        else if (ieee80211_is_assoc_request(fc))
-                IWL_DEBUG_TX("Sending ASSOC frame\n");
-        else if (ieee80211_is_reassoc_request(fc))
-                IWL_DEBUG_TX("Sending REASSOC frame\n");
-#endif
-
-        /* drop all data frame if we are not associated */
-        if (((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
-           (!iwl_is_associated(priv) ||
-            ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id) ||
-            !priv->assoc_station_added)) {
-                IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
-                goto drop_unlock;
-        }
-
-        spin_unlock_irqrestore(&priv->lock, flags);
-
-        hdr_len = ieee80211_get_hdrlen(fc);
-
-        /* Find (or create) index into station table for destination station */
-        sta_id = iwl4965_get_sta_id(priv, hdr);
-        if (sta_id == IWL_INVALID_STATION) {
-                DECLARE_MAC_BUF(mac);
-
-                IWL_DEBUG_DROP("Dropping - INVALID STATION: %s\n",
-                               print_mac(mac, hdr->addr1));
-                goto drop;
-        }
-
-        IWL_DEBUG_TX("station Id %d\n", sta_id);
-
-        qc = ieee80211_get_qos_ctrl(hdr);
-        if (qc) {
-                u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
-                seq_number = priv->stations[sta_id].tid[tid].seq_number &
-                                IEEE80211_SCTL_SEQ;
-                hdr->seq_ctrl = cpu_to_le16(seq_number) |
-                        (hdr->seq_ctrl &
-                                __constant_cpu_to_le16(IEEE80211_SCTL_FRAG));
-                seq_number += 0x10;
-#ifdef CONFIG_IWL4965_HT
-                /* aggregation is on for this <sta,tid> */
-                if (ctl->flags & IEEE80211_TXCTL_AMPDU)
-                        txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
-                priv->stations[sta_id].tid[tid].tfds_in_queue++;
-#endif /* CONFIG_IWL4965_HT */
-        }
-
-        /* Descriptor for chosen Tx queue */
-        txq = &priv->txq[txq_id];
-        q = &txq->q;
-
-        spin_lock_irqsave(&priv->lock, flags);
-
-        /* Set up first empty TFD within this queue's circular TFD buffer */
-        tfd = &txq->bd[q->write_ptr];
-        memset(tfd, 0, sizeof(*tfd));
-        control_flags = (u32 *) tfd;
-        idx = get_cmd_index(q, q->write_ptr, 0);
-
-        /* Set up driver data for this TFD */
-        memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl4965_tx_info));
-        txq->txb[q->write_ptr].skb[0] = skb;
-        memcpy(&(txq->txb[q->write_ptr].status.control),
-               ctl, sizeof(struct ieee80211_tx_control));
-
-        /* Set up first empty entry in queue's array of Tx/cmd buffers */
-        out_cmd = &txq->cmd[idx];
-        memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
-        memset(&out_cmd->cmd.tx, 0, sizeof(out_cmd->cmd.tx));
-
-        /*
-         * Set up the Tx-command (not MAC!) header.
-         * Store the chosen Tx queue and TFD index within the sequence field;
-         * after Tx, uCode's Tx response will return this value so driver can
-         * locate the frame within the tx queue and do post-tx processing.
-         */
-        out_cmd->hdr.cmd = REPLY_TX;
-        out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
-                                INDEX_TO_SEQ(q->write_ptr)));
-
-        /* Copy MAC header from skb into command buffer */
-        memcpy(out_cmd->cmd.tx.hdr, hdr, hdr_len);
-
-        /*
-         * Use the first empty entry in this queue's command buffer array
-         * to contain the Tx command and MAC header concatenated together
-         * (payload data will be in another buffer).
-         * Size of this varies, due to varying MAC header length.
-         * If end is not dword aligned, we'll have 2 extra bytes at the end
-         * of the MAC header (device reads on dword boundaries).
-         * We'll tell device about this padding later.
-         */
-        len = priv->hw_params.tx_cmd_len +
-                sizeof(struct iwl_cmd_header) + hdr_len;
-
-        len_org = len;
-        len = (len + 3) & ~3;
-
-        if (len_org != len)
-                len_org = 1;
-        else
-                len_org = 0;
-
-        /* Physical address of this Tx command's header (not MAC header!),
-         * within command buffer array. */
-        txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
-                     offsetof(struct iwl_cmd, hdr);
-
-        /* Add buffer containing Tx command and MAC(!) header to TFD's
-         * first entry */
-        iwl4965_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
-
-        if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
-                iwl4965_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, sta_id);
-
-        /* Set up TFD's 2nd entry to point directly to remainder of skb,
-         * if any (802.11 null frames have no payload). */
-        len = skb->len - hdr_len;
-        if (len) {
-                phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
-                                           len, PCI_DMA_TODEVICE);
-                iwl4965_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
-        }
-
-        /* Tell 4965 about any 2-byte padding after MAC header */
-        if (len_org)
-                out_cmd->cmd.tx.tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
-
-        /* Total # bytes to be transmitted */
-        len = (u16)skb->len;
-        out_cmd->cmd.tx.len = cpu_to_le16(len);
-
-        /* TODO need this for burst mode later on */
-        iwl4965_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
-
-        /* set is_hcca to 0; it probably will never be implemented */
-        iwl4965_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
-
-        iwl_update_tx_stats(priv, fc, len);
-
-        scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
-                offsetof(struct iwl4965_tx_cmd, scratch);
-        out_cmd->cmd.tx.dram_lsb_ptr = cpu_to_le32(scratch_phys);
-        out_cmd->cmd.tx.dram_msb_ptr = iwl_get_dma_hi_address(scratch_phys);
-
-        if (!ieee80211_get_morefrag(hdr)) {
-                txq->need_update = 1;
-                if (qc) {
-                        u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
-                        priv->stations[sta_id].tid[tid].seq_number = seq_number;
-                }
-        } else {
-                wait_write_ptr = 1;
-                txq->need_update = 0;
-        }
-
-        iwl_print_hex_dump(priv, IWL_DL_TX, out_cmd->cmd.payload,
-                           sizeof(out_cmd->cmd.tx));
-
-        iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
-                           ieee80211_get_hdrlen(fc));
-
-        /* Set up entry for this TFD in Tx byte-count array */
-        priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, len);
-
-        /* Tell device the write index *just past* this latest filled TFD */
-        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
-        rc = iwl4965_tx_queue_update_write_ptr(priv, txq);
-        spin_unlock_irqrestore(&priv->lock, flags);
-
-        if (rc)
-                return rc;
-
-        if ((iwl4965_queue_space(q) < q->high_mark)
-            && priv->mac80211_registered) {
-                if (wait_write_ptr) {
-                        spin_lock_irqsave(&priv->lock, flags);
-                        txq->need_update = 1;
-                        iwl4965_tx_queue_update_write_ptr(priv, txq);
-                        spin_unlock_irqrestore(&priv->lock, flags);
-                }
-
-                ieee80211_stop_queue(priv->hw, ctl->queue);
-        }
-
-        return 0;
-
-drop_unlock:
-        spin_unlock_irqrestore(&priv->lock, flags);
-drop:
-        return -1;
-}
-
 static void iwl4965_set_rate(struct iwl_priv *priv)
 {
         const struct ieee80211_supported_band *hw = NULL;
@@ -2305,341 +1529,16 @@ static int iwl4965_get_measurement(struct iwl_priv *priv,
 }
 #endif
 
-static void iwl4965_txstatus_to_ieee(struct iwl_priv *priv,
-                                 struct iwl4965_tx_info *tx_sta)
-{
-
-        tx_sta->status.ack_signal = 0;
-        tx_sta->status.excessive_retries = 0;
-
-        if (in_interrupt())
-                ieee80211_tx_status_irqsafe(priv->hw,
-                                            tx_sta->skb[0], &(tx_sta->status));
-        else
-                ieee80211_tx_status(priv->hw,
-                                    tx_sta->skb[0], &(tx_sta->status));
-
-        tx_sta->skb[0] = NULL;
-}
-
-/**
- * iwl4965_tx_queue_reclaim - Reclaim Tx queue entries already Tx'd
- *
- * When FW advances 'R' index, all entries between old and new 'R' index
- * need to be reclaimed. As result, some free space forms.  If there is
- * enough free space (> low mark), wake the stack that feeds us.
- */
-int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
-{
-        struct iwl_tx_queue *txq = &priv->txq[txq_id];
-        struct iwl4965_queue *q = &txq->q;
-        int nfreed = 0;
-
-        if ((index >= q->n_bd) || (x2_queue_used(q, index) == 0)) {
-                IWL_ERROR("Read index for DMA queue txq id (%d), index %d, "
-                          "is out of range [0-%d] %d %d.\n", txq_id,
-                          index, q->n_bd, q->write_ptr, q->read_ptr);
-                return 0;
-        }
-
-        for (index = iwl_queue_inc_wrap(index, q->n_bd);
-                q->read_ptr != index;
-                q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
-                if (txq_id != IWL_CMD_QUEUE_NUM) {
-                        iwl4965_txstatus_to_ieee(priv,
-                                        &(txq->txb[txq->q.read_ptr]));
-                        iwl_hw_txq_free_tfd(priv, txq);
-                } else if (nfreed > 1) {
-                        IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
-                                        q->write_ptr, q->read_ptr);
-                        queue_work(priv->workqueue, &priv->restart);
-                }
-                nfreed++;
-        }
-
-        return nfreed;
-}
-
-static int iwl4965_is_tx_success(u32 status)
-{
-        status &= TX_STATUS_MSK;
-        return (status == TX_STATUS_SUCCESS)
-            || (status == TX_STATUS_DIRECT_DONE);
-}
-
 /******************************************************************************
  *
  * Generic RX handler implementations
  *
  ******************************************************************************/
-#ifdef CONFIG_IWL4965_HT
-
-static inline int iwl4965_get_ra_sta_id(struct iwl_priv *priv,
-                                    struct ieee80211_hdr *hdr)
-{
-        if (priv->iw_mode == IEEE80211_IF_TYPE_STA)
-                return IWL_AP_ID;
-        else {
-                u8 *da = ieee80211_get_DA(hdr);
-                return iwl_find_station(priv, da);
-        }
-}
-
-static struct ieee80211_hdr *iwl4965_tx_queue_get_hdr(
-        struct iwl_priv *priv, int txq_id, int idx)
-{
-        if (priv->txq[txq_id].txb[idx].skb[0])
-                return (struct ieee80211_hdr *)priv->txq[txq_id].
-                                txb[idx].skb[0]->data;
-        return NULL;
-}
-
-static inline u32 iwl4965_get_scd_ssn(struct iwl4965_tx_resp *tx_resp)
-{
-        __le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
-                                tx_resp->frame_count);
-        return le32_to_cpu(*scd_ssn) & MAX_SN;
-
-}
-
-/**
- * iwl4965_tx_status_reply_tx - Handle Tx rspnse for frames in aggregation queue
- */
-static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
-                                      struct iwl_ht_agg *agg,
-                                      struct iwl4965_tx_resp_agg *tx_resp,
-                                      u16 start_idx)
-{
-        u16 status;
-        struct agg_tx_status *frame_status = &tx_resp->status;
-        struct ieee80211_tx_status *tx_status = NULL;
-        struct ieee80211_hdr *hdr = NULL;
-        int i, sh;
-        int txq_id, idx;
-        u16 seq;
-
-        if (agg->wait_for_ba)
-                IWL_DEBUG_TX_REPLY("got tx response w/o block-ack\n");
-
-        agg->frame_count = tx_resp->frame_count;
-        agg->start_idx = start_idx;
-        agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
-        agg->bitmap = 0;
-
-        /* # frames attempted by Tx command */
-        if (agg->frame_count == 1) {
-                /* Only one frame was attempted; no block-ack will arrive */
-                status = le16_to_cpu(frame_status[0].status);
-                seq  = le16_to_cpu(frame_status[0].sequence);
-                idx = SEQ_TO_INDEX(seq);
-                txq_id = SEQ_TO_QUEUE(seq);
-
-                /* FIXME: code repetition */
-                IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
-                                   agg->frame_count, agg->start_idx, idx);
-
-                tx_status = &(priv->txq[txq_id].txb[idx].status);
-                tx_status->retry_count = tx_resp->failure_frame;
-                tx_status->control.flags &= ~IEEE80211_TXCTL_AMPDU;
-                tx_status->flags = iwl4965_is_tx_success(status)?
-                        IEEE80211_TX_STATUS_ACK : 0;
-                iwl4965_hwrate_to_tx_control(priv,
-                                             le32_to_cpu(tx_resp->rate_n_flags),
-                                             &tx_status->control);
-                /* FIXME: code repetition end */
-
-                IWL_DEBUG_TX_REPLY("1 Frame 0x%x failure :%d\n",
-                                    status & 0xff, tx_resp->failure_frame);
-                IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n",
-                                iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
-
-                agg->wait_for_ba = 0;
-        } else {
-                /* Two or more frames were attempted; expect block-ack */
-                u64 bitmap = 0;
-                int start = agg->start_idx;
-
-                /* Construct bit-map of pending frames within Tx window */
-                for (i = 0; i < agg->frame_count; i++) {
-                        u16 sc;
-                        status = le16_to_cpu(frame_status[i].status);
-                        seq  = le16_to_cpu(frame_status[i].sequence);
-                        idx = SEQ_TO_INDEX(seq);
-                        txq_id = SEQ_TO_QUEUE(seq);
-
-                        if (status & (AGG_TX_STATE_FEW_BYTES_MSK |
-                                      AGG_TX_STATE_ABORT_MSK))
-                                continue;
-
-                        IWL_DEBUG_TX_REPLY("FrameCnt = %d, txq_id=%d idx=%d\n",
-                                           agg->frame_count, txq_id, idx);
-
-                        hdr = iwl4965_tx_queue_get_hdr(priv, txq_id, idx);
-
-                        sc = le16_to_cpu(hdr->seq_ctrl);
-                        if (idx != (SEQ_TO_SN(sc) & 0xff)) {
-                                IWL_ERROR("BUG_ON idx doesn't match seq control"
-                                          " idx=%d, seq_idx=%d, seq=%d\n",
-                                          idx, SEQ_TO_SN(sc),
-                                          hdr->seq_ctrl);
-                                return -1;
-                        }
-
-                        IWL_DEBUG_TX_REPLY("AGG Frame i=%d idx %d seq=%d\n",
-                                           i, idx, SEQ_TO_SN(sc));
-
-                        sh = idx - start;
-                        if (sh > 64) {
-                                sh = (start - idx) + 0xff;
-                                bitmap = bitmap << sh;
-                                sh = 0;
-                                start = idx;
-                        } else if (sh < -64)
-                                sh  = 0xff - (start - idx);
-                        else if (sh < 0) {
-                                sh = start - idx;
-                                start = idx;
-                                bitmap = bitmap << sh;
-                                sh = 0;
-                        }
-                        bitmap |= (1 << sh);
-                        IWL_DEBUG_TX_REPLY("start=%d bitmap=0x%x\n",
-                                           start, (u32)(bitmap & 0xFFFFFFFF));
-                }
-
-                agg->bitmap = bitmap;
-                agg->start_idx = start;
-                agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
-                IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%llx\n",
-                                   agg->frame_count, agg->start_idx,
-                                   (unsigned long long)agg->bitmap);
-
-                if (bitmap)
-                        agg->wait_for_ba = 1;
-        }
-        return 0;
-}
-#endif
-
-/**
- * iwl4965_rx_reply_tx - Handle standard (non-aggregation) Tx response
- */
-static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
+static void iwl_rx_reply_alive(struct iwl_priv *priv,
                                 struct iwl_rx_mem_buffer *rxb)
 {
         struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        u16 sequence = le16_to_cpu(pkt->hdr.sequence);
-        int txq_id = SEQ_TO_QUEUE(sequence);
-        int index = SEQ_TO_INDEX(sequence);
-        struct iwl_tx_queue *txq = &priv->txq[txq_id];
-        struct ieee80211_tx_status *tx_status;
-        struct iwl4965_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
-        u32  status = le32_to_cpu(tx_resp->status);
-#ifdef CONFIG_IWL4965_HT
-        int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
-        struct ieee80211_hdr *hdr;
-        __le16 *qc;
-#endif
-
-        if ((index >= txq->q.n_bd) || (x2_queue_used(&txq->q, index) == 0)) {
-                IWL_ERROR("Read index for DMA queue txq_id (%d) index %d "
-                          "is out of range [0-%d] %d %d\n", txq_id,
-                          index, txq->q.n_bd, txq->q.write_ptr,
-                          txq->q.read_ptr);
-                return;
-        }
-
-#ifdef CONFIG_IWL4965_HT
-        hdr = iwl4965_tx_queue_get_hdr(priv, txq_id, index);
-        qc = ieee80211_get_qos_ctrl(hdr);
-
-        if (qc)
-                tid = le16_to_cpu(*qc) & 0xf;
-
-        sta_id = iwl4965_get_ra_sta_id(priv, hdr);
-        if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
-                IWL_ERROR("Station not known\n");
-                return;
-        }
-
-        if (txq->sched_retry) {
-                const u32 scd_ssn = iwl4965_get_scd_ssn(tx_resp);
-                struct iwl_ht_agg *agg = NULL;
-
-                if (!qc)
-                        return;
-
-                agg = &priv->stations[sta_id].tid[tid].agg;
-
-                iwl4965_tx_status_reply_tx(priv, agg,
-                                (struct iwl4965_tx_resp_agg *)tx_resp, index);
-
-                if ((tx_resp->frame_count == 1) &&
-                    !iwl4965_is_tx_success(status)) {
-                        /* TODO: send BAR */
-                }
-
-                if (txq->q.read_ptr != (scd_ssn & 0xff)) {
-                        int freed, ampdu_q;
-                        index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
-                        IWL_DEBUG_TX_REPLY("Retry scheduler reclaim scd_ssn "
-                                           "%d index %d\n", scd_ssn , index);
-                        freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
-                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
-
-                        if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
-                            txq_id >= 0 && priv->mac80211_registered &&
-                            agg->state != IWL_EMPTYING_HW_QUEUE_DELBA) {
-                                /* calculate mac80211 ampdu sw queue to wake */
-                                ampdu_q = txq_id - IWL_BACK_QUEUE_FIRST_ID +
-                                          priv->hw->queues;
-                                if (agg->state == IWL_AGG_OFF)
-                                        ieee80211_wake_queue(priv->hw, txq_id);
-                                else
-                                        ieee80211_wake_queue(priv->hw, ampdu_q);
-                        }
-                        iwl4965_check_empty_hw_queue(priv, sta_id, tid, txq_id);
-                }
-        } else {
-#endif /* CONFIG_IWL4965_HT */
-        tx_status = &(txq->txb[txq->q.read_ptr].status);
-
-        tx_status->retry_count = tx_resp->failure_frame;
-        tx_status->flags =
-            iwl4965_is_tx_success(status) ? IEEE80211_TX_STATUS_ACK : 0;
-        iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
-                                     &tx_status->control);
-
-        IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
-                     "retries %d\n", txq_id, iwl4965_get_tx_fail_reason(status),
-                     status, le32_to_cpu(tx_resp->rate_n_flags),
-                     tx_resp->failure_frame);
-
-        IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
-#ifdef CONFIG_IWL4965_HT
-        if (index != -1) {
-                int freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
-                if (tid != MAX_TID_COUNT)
-                        priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
-                if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
-                        (txq_id >= 0) && priv->mac80211_registered)
-                        ieee80211_wake_queue(priv->hw, txq_id);
-                if (tid != MAX_TID_COUNT)
-                        iwl4965_check_empty_hw_queue(priv, sta_id, tid, txq_id);
-        }
-        }
-#endif /* CONFIG_IWL4965_HT */
-
-        if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
-                IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
-}
-
-
-static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
-                                   struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_alive_resp *palive;
+        struct iwl_alive_resp *palive;
         struct delayed_work *pwork;
 
         palive = &pkt->u.alive_frame;
@@ -2653,12 +1552,12 @@ static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
                 IWL_DEBUG_INFO("Initialization Alive received.\n");
                 memcpy(&priv->card_alive_init,
                        &pkt->u.alive_frame,
-                       sizeof(struct iwl4965_init_alive_resp));
+                       sizeof(struct iwl_init_alive_resp));
                 pwork = &priv->init_alive_start;
         } else {
                 IWL_DEBUG_INFO("Runtime Alive received.\n");
                 memcpy(&priv->card_alive, &pkt->u.alive_frame,
-                       sizeof(struct iwl4965_alive_resp));
+                       sizeof(struct iwl_alive_resp));
                 pwork = &priv->alive_start;
         }
 
@@ -2671,15 +1570,6 @@ static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
                 IWL_WARNING("uCode did not respond OK.\n");
 }
 
-static void iwl4965_rx_reply_add_sta(struct iwl_priv *priv,
-                                     struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-
-        IWL_DEBUG_RX("Received REPLY_ADD_STA: 0x%02X\n", pkt->u.status);
-        return;
-}
-
 static void iwl4965_rx_reply_error(struct iwl_priv *priv,
                                    struct iwl_rx_mem_buffer *rxb)
 {
@@ -2699,7 +1589,7 @@ static void iwl4965_rx_reply_error(struct iwl_priv *priv,
 static void iwl4965_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
 {
         struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_rxon_cmd *rxon = (void *)&priv->active_rxon;
+        struct iwl_rxon_cmd *rxon = (void *)&priv->active_rxon;
         struct iwl4965_csa_notification *csa = &(pkt->u.csa_notif);
         IWL_DEBUG_11H("CSA notif: channel %d, status %d\n",
                       le16_to_cpu(csa->channel), le32_to_cpu(csa->status));
@@ -2753,7 +1643,7 @@ static void iwl4965_bg_beacon_update(struct work_struct *work)
         struct sk_buff *beacon;
 
         /* Pull updated AP beacon from mac80211. will fail if not in AP mode */
-        beacon = ieee80211_beacon_get(priv->hw, priv->vif, NULL);
+        beacon = ieee80211_beacon_get(priv->hw, priv->vif);
 
         if (!beacon) {
                 IWL_ERROR("update beacon failed\n");
@@ -2976,6 +1866,17 @@ static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
                 wake_up_interruptible(&priv->wait_command_queue);
 }
 
+/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
+ * This will be used later in iwl4965_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
+static void iwl4965_rx_reply_rx_phy(struct iwl_priv *priv,
+                                    struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        priv->last_phy_res[0] = 1;
+        memcpy(&priv->last_phy_res[1], &(pkt->u.raw[0]),
+               sizeof(struct iwl4965_rx_phy_res));
+}
+
 /**
  * iwl4965_setup_rx_handlers - Initialize Rx handler callbacks
  *
@@ -2987,8 +1888,7 @@ static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
  */
 static void iwl4965_setup_rx_handlers(struct iwl_priv *priv)
 {
-        priv->rx_handlers[REPLY_ALIVE] = iwl4965_rx_reply_alive;
-        priv->rx_handlers[REPLY_ADD_STA] = iwl4965_rx_reply_add_sta;
+        priv->rx_handlers[REPLY_ALIVE] = iwl_rx_reply_alive;
         priv->rx_handlers[REPLY_ERROR] = iwl4965_rx_reply_error;
         priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl4965_rx_csa;
         priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
@@ -3005,66 +1905,25 @@ static void iwl4965_setup_rx_handlers(struct iwl_priv *priv)
          */
         priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl4965_hw_rx_statistics;
         priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl4965_hw_rx_statistics;
-
+        /* scan handlers */
         priv->rx_handlers[REPLY_SCAN_CMD] = iwl4965_rx_reply_scan;
         priv->rx_handlers[SCAN_START_NOTIFICATION] = iwl4965_rx_scan_start_notif;
         priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
             iwl4965_rx_scan_results_notif;
         priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
             iwl4965_rx_scan_complete_notif;
+        /* status change handler */
         priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl4965_rx_card_state_notif;
-        priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
 
+        priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
+            iwl_rx_missed_beacon_notif;
+        /* Rx handlers */
+        priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl4965_rx_reply_rx_phy;
+        priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl4965_rx_reply_rx;
         /* Set up hardware specific Rx handlers */
         priv->cfg->ops->lib->rx_handler_setup(priv);
 }
 
-/**
- * iwl4965_tx_cmd_complete - Pull unused buffers off the queue and reclaim them
- * @rxb: Rx buffer to reclaim
- *
- * If an Rx buffer has an async callback associated with it the callback
- * will be executed.  The attached skb (if present) will only be freed
- * if the callback returns 1
- */
-static void iwl4965_tx_cmd_complete(struct iwl_priv *priv,
-                                    struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        u16 sequence = le16_to_cpu(pkt->hdr.sequence);
-        int txq_id = SEQ_TO_QUEUE(sequence);
-        int index = SEQ_TO_INDEX(sequence);
-        int huge = sequence & SEQ_HUGE_FRAME;
-        int cmd_index;
-        struct iwl_cmd *cmd;
-
-        /* If a Tx command is being handled and it isn't in the actual
-         * command queue then there a command routing bug has been introduced
-         * in the queue management code. */
-        if (txq_id != IWL_CMD_QUEUE_NUM)
-                IWL_ERROR("Error wrong command queue %d command id 0x%X\n",
-                          txq_id, pkt->hdr.cmd);
-        BUG_ON(txq_id != IWL_CMD_QUEUE_NUM);
-
-        cmd_index = get_cmd_index(&priv->txq[IWL_CMD_QUEUE_NUM].q, index, huge);
-        cmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_index];
-
-        /* Input error checking is done when commands are added to queue. */
-        if (cmd->meta.flags & CMD_WANT_SKB) {
-                cmd->meta.source->u.skb = rxb->skb;
-                rxb->skb = NULL;
-        } else if (cmd->meta.u.callback &&
-                   !cmd->meta.u.callback(priv, cmd, rxb->skb))
-                rxb->skb = NULL;
-
-        iwl4965_tx_queue_reclaim(priv, txq_id, index);
-
-        if (!(cmd->meta.flags & CMD_ASYNC)) {
-                clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
-                wake_up_interruptible(&priv->wait_command_queue);
-        }
-}
-
 /*
  * this should be called while priv->lock is locked
 */
@@ -3153,7 +2012,7 @@ void iwl_rx_handle(struct iwl_priv *priv)
                          * fire off the (possibly) blocking iwl_send_cmd()
                          * as we reclaim the driver command queue */
                         if (rxb && rxb->skb)
-                                iwl4965_tx_cmd_complete(priv, rxb);
+                                iwl_tx_cmd_complete(priv, rxb);
                         else
                                 IWL_WARNING("Claim null rxb?\n");
                 }
@@ -3268,56 +2127,10 @@ int iwl4965_calc_sig_qual(int rssi_dbm, int noise_dbm)
         return sig_qual;
 }
 
-/**
- * iwl4965_tx_queue_update_write_ptr - Send new write index to hardware
- */
-static int iwl4965_tx_queue_update_write_ptr(struct iwl_priv *priv,
-                                  struct iwl_tx_queue *txq)
-{
-        u32 reg = 0;
-        int rc = 0;
-        int txq_id = txq->q.id;
-
-        if (txq->need_update == 0)
-                return rc;
-
-        /* if we're trying to save power */
-        if (test_bit(STATUS_POWER_PMI, &priv->status)) {
-                /* wake up nic if it's powered down ...
-                 * uCode will wake up, and interrupt us again, so next
-                 * time we'll skip this part. */
-                reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
-
-                if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
-                        IWL_DEBUG_INFO("Requesting wakeup, GP1 = 0x%x\n", reg);
-                        iwl_set_bit(priv, CSR_GP_CNTRL,
-                                    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
-                        return rc;
-                }
-
-                /* restore this queue's parameters in nic hardware. */
-                rc = iwl_grab_nic_access(priv);
-                if (rc)
-                        return rc;
-                iwl_write_direct32(priv, HBUS_TARG_WRPTR,
-                                     txq->q.write_ptr | (txq_id << 8));
-                iwl_release_nic_access(priv);
-
-        /* else not in power-save mode, uCode will never sleep when we're
-         * trying to tx (during RFKILL, we're not trying to tx). */
-        } else
-                iwl_write32(priv, HBUS_TARG_WRPTR,
-                            txq->q.write_ptr | (txq_id << 8));
-
-        txq->need_update = 0;
-
-        return rc;
-}
-
 #ifdef CONFIG_IWLWIFI_DEBUG
 static void iwl4965_print_rx_config_cmd(struct iwl_priv *priv)
 {
-        struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
+        struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
         DECLARE_MAC_BUF(mac);
 
         IWL_DEBUG_RADIO("RX CONFIG:\n");
@@ -3367,173 +2180,6 @@ static inline void iwl4965_disable_interrupts(struct iwl_priv *priv)
         IWL_DEBUG_ISR("Disabled interrupts\n");
 }
 
-static const char *desc_lookup(int i)
-{
-        switch (i) {
-        case 1:
-                return "FAIL";
-        case 2:
-                return "BAD_PARAM";
-        case 3:
-                return "BAD_CHECKSUM";
-        case 4:
-                return "NMI_INTERRUPT";
-        case 5:
-                return "SYSASSERT";
-        case 6:
-                return "FATAL_ERROR";
-        }
-
-        return "UNKNOWN";
-}
-
-#define ERROR_START_OFFSET  (1 * sizeof(u32))
-#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
-
-static void iwl4965_dump_nic_error_log(struct iwl_priv *priv)
-{
-        u32 data2, line;
-        u32 desc, time, count, base, data1;
-        u32 blink1, blink2, ilink1, ilink2;
-        int rc;
-
-        base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
-
-        if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
-                IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
-                return;
-        }
-
-        rc = iwl_grab_nic_access(priv);
-        if (rc) {
-                IWL_WARNING("Can not read from adapter at this time.\n");
-                return;
-        }
-
-        count = iwl_read_targ_mem(priv, base);
-
-        if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
-                IWL_ERROR("Start IWL Error Log Dump:\n");
-                IWL_ERROR("Status: 0x%08lX, count: %d\n", priv->status, count);
-        }
-
-        desc = iwl_read_targ_mem(priv, base + 1 * sizeof(u32));
-        blink1 = iwl_read_targ_mem(priv, base + 3 * sizeof(u32));
-        blink2 = iwl_read_targ_mem(priv, base + 4 * sizeof(u32));
-        ilink1 = iwl_read_targ_mem(priv, base + 5 * sizeof(u32));
-        ilink2 = iwl_read_targ_mem(priv, base + 6 * sizeof(u32));
-        data1 = iwl_read_targ_mem(priv, base + 7 * sizeof(u32));
-        data2 = iwl_read_targ_mem(priv, base + 8 * sizeof(u32));
-        line = iwl_read_targ_mem(priv, base + 9 * sizeof(u32));
-        time = iwl_read_targ_mem(priv, base + 11 * sizeof(u32));
-
-        IWL_ERROR("Desc               Time       "
-                  "data1      data2      line\n");
-        IWL_ERROR("%-13s (#%d) %010u 0x%08X 0x%08X %u\n",
-                  desc_lookup(desc), desc, time, data1, data2, line);
-        IWL_ERROR("blink1  blink2  ilink1  ilink2\n");
-        IWL_ERROR("0x%05X 0x%05X 0x%05X 0x%05X\n", blink1, blink2,
-                  ilink1, ilink2);
-
-        iwl_release_nic_access(priv);
-}
-
-#define EVENT_START_OFFSET  (4 * sizeof(u32))
-
-/**
- * iwl4965_print_event_log - Dump error event log to syslog
- *
- * NOTE: Must be called with iwl_grab_nic_access() already obtained!
- */
-static void iwl4965_print_event_log(struct iwl_priv *priv, u32 start_idx,
-                                u32 num_events, u32 mode)
-{
-        u32 i;
-        u32 base;       /* SRAM byte address of event log header */
-        u32 event_size; /* 2 u32s, or 3 u32s if timestamp recorded */
-        u32 ptr;        /* SRAM byte address of log data */
-        u32 ev, time, data; /* event log data */
-
-        if (num_events == 0)
-                return;
-
-        base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
-
-        if (mode == 0)
-                event_size = 2 * sizeof(u32);
-        else
-                event_size = 3 * sizeof(u32);
-
-        ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
-
-        /* "time" is actually "data" for mode 0 (no timestamp).
-         * place event id # at far right for easier visual parsing. */
-        for (i = 0; i < num_events; i++) {
-                ev = iwl_read_targ_mem(priv, ptr);
-                ptr += sizeof(u32);
-                time = iwl_read_targ_mem(priv, ptr);
-                ptr += sizeof(u32);
-                if (mode == 0)
-                        IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
-                else {
-                        data = iwl_read_targ_mem(priv, ptr);
-                        ptr += sizeof(u32);
-                        IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
-                }
-        }
-}
-
-static void iwl4965_dump_nic_event_log(struct iwl_priv *priv)
-{
-        int rc;
-        u32 base;       /* SRAM byte address of event log header */
-        u32 capacity;   /* event log capacity in # entries */
-        u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
-        u32 num_wraps;  /* # times uCode wrapped to top of log */
-        u32 next_entry; /* index of next entry to be written by uCode */
-        u32 size;       /* # entries that we'll print */
-
-        base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
-        if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
-                IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
-                return;
-        }
-
-        rc = iwl_grab_nic_access(priv);
-        if (rc) {
-                IWL_WARNING("Can not read from adapter at this time.\n");
-                return;
-        }
-
-        /* event log header */
-        capacity = iwl_read_targ_mem(priv, base);
-        mode = iwl_read_targ_mem(priv, base + (1 * sizeof(u32)));
-        num_wraps = iwl_read_targ_mem(priv, base + (2 * sizeof(u32)));
-        next_entry = iwl_read_targ_mem(priv, base + (3 * sizeof(u32)));
-
-        size = num_wraps ? capacity : next_entry;
-
-        /* bail out if nothing in log */
-        if (size == 0) {
-                IWL_ERROR("Start IWL Event Log Dump: nothing in log\n");
-                iwl_release_nic_access(priv);
-                return;
-        }
-
-        IWL_ERROR("Start IWL Event Log Dump: display count %d, wraps %d\n",
-                  size, num_wraps);
-
-        /* if uCode has wrapped back to top of log, start at the oldest entry,
-         * i.e the next one that uCode would fill. */
-        if (num_wraps)
-                iwl4965_print_event_log(priv, next_entry,
-                                    capacity - next_entry, mode);
-
-        /* (then/else) start at top of log */
-        iwl4965_print_event_log(priv, 0, next_entry, mode);
-
-        iwl_release_nic_access(priv);
-}
 
 /**
  * iwl4965_irq_handle_error - called for HW or SW error interrupt from card
@@ -3548,8 +2194,8 @@ static void iwl4965_irq_handle_error(struct iwl_priv *priv)
 
 #ifdef CONFIG_IWLWIFI_DEBUG
         if (priv->debug_level & IWL_DL_FW_ERRORS) {
-                iwl4965_dump_nic_error_log(priv);
-                iwl4965_dump_nic_event_log(priv);
+                iwl_dump_nic_error_log(priv);
+                iwl_dump_nic_event_log(priv);
                 iwl4965_print_rx_config_cmd(priv);
         }
 #endif
@@ -3583,7 +2229,7 @@ static void iwl4965_error_recovery(struct iwl_priv *priv)
         priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
         iwl4965_commit_rxon(priv);
 
-        iwl4965_rxon_add_station(priv, priv->bssid, 1);
+        iwl_rxon_add_station(priv, priv->bssid, 1);
 
         spin_lock_irqsave(&priv->lock, flags);
         priv->assoc_id = le16_to_cpu(priv->staging_rxon.assoc_id);
@@ -3703,12 +2349,12 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
         if (inta & CSR_INT_BIT_WAKEUP) {
                 IWL_DEBUG_ISR("Wakeup interrupt\n");
                 iwl_rx_queue_update_write_ptr(priv, &priv->rxq);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[0]);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[1]);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[2]);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[3]);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[4]);
-                iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[5]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[0]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[1]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[2]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[3]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[4]);
+                iwl_txq_update_write_ptr(priv, &priv->txq[5]);
 
                 handled |= CSR_INT_BIT_WAKEUP;
         }
@@ -3724,6 +2370,9 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
         if (inta & CSR_INT_BIT_FH_TX) {
                 IWL_DEBUG_ISR("Tx interrupt\n");
                 handled |= CSR_INT_BIT_FH_TX;
+                /* FH finished to write, send event */
+                priv->ucode_write_complete = 1;
+                wake_up_interruptible(&priv->wait_command_queue);
         }
 
         if (inta & ~handled)
@@ -4162,11 +2811,11 @@ static int iwl4965_read_ucode(struct iwl_priv *priv)
 }
 
 /**
- * iwl4965_alive_start - called after REPLY_ALIVE notification received
+ * iwl_alive_start - called after REPLY_ALIVE notification received
  *                   from protocol/runtime uCode (initialization uCode's
- *                   Alive gets handled by iwl4965_init_alive_start()).
+ *                   Alive gets handled by iwl_init_alive_start()).
  */
-static void iwl4965_alive_start(struct iwl_priv *priv)
+static void iwl_alive_start(struct iwl_priv *priv)
 {
         int ret = 0;
 
@@ -4190,7 +2839,6 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
         }
 
         iwlcore_clear_stations_table(priv);
-
         ret = priv->cfg->ops->lib->alive_notify(priv);
         if (ret) {
                 IWL_WARNING("Could not complete ALIVE transition [ntf]: %d\n",
@@ -4207,14 +2855,14 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
         if (iwl_is_rfkill(priv))
                 return;
 
-        ieee80211_start_queues(priv->hw);
+        ieee80211_wake_queues(priv->hw);
 
         priv->active_rate = priv->rates_mask;
         priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
 
         if (iwl_is_associated(priv)) {
-                struct iwl4965_rxon_cmd *active_rxon =
-                                (struct iwl4965_rxon_cmd *)(&priv->active_rxon);
+                struct iwl_rxon_cmd *active_rxon =
+                                (struct iwl_rxon_cmd *)&priv->active_rxon;
 
                 memcpy(&priv->staging_rxon, &priv->active_rxon,
                        sizeof(priv->staging_rxon));
@@ -4228,12 +2876,12 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
         /* Configure Bluetooth device coexistence support */
         iwl4965_send_bt_config(priv);
 
+        iwl_reset_run_time_calib(priv);
+
         /* Configure the adapter for unassociated operation */
         iwl4965_commit_rxon(priv);
 
         /* At this point, the NIC is initialized and operational */
-        priv->notif_missed_beacons = 0;
-
         iwl4965_rf_kill_ct_config(priv);
 
         iwl_leds_register(priv);
@@ -4259,12 +2907,9 @@ static void __iwl4965_down(struct iwl_priv *priv)
 {
         unsigned long flags;
         int exit_pending = test_bit(STATUS_EXIT_PENDING, &priv->status);
-        struct ieee80211_conf *conf = NULL;
 
         IWL_DEBUG_INFO(DRV_NAME " is going down\n");
 
-        conf = ieee80211_get_hw_conf(priv->hw);
-
         if (!exit_pending)
                 set_bit(STATUS_EXIT_PENDING, &priv->status);
 
@@ -4326,8 +2971,8 @@ static void __iwl4965_down(struct iwl_priv *priv)
                          CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
         spin_unlock_irqrestore(&priv->lock, flags);
 
-        iwl4965_hw_txq_ctx_stop(priv);
-        iwl4965_hw_rxq_stop(priv);
+        iwl_txq_ctx_stop(priv);
+        iwl_rxq_stop(priv);
 
         spin_lock_irqsave(&priv->lock, flags);
         if (!iwl_grab_nic_access(priv)) {
@@ -4339,20 +2984,19 @@ static void __iwl4965_down(struct iwl_priv *priv)
 
         udelay(5);
 
-        iwl4965_hw_nic_stop_master(priv);
-        iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-        iwl4965_hw_nic_reset(priv);
+        /* FIXME: apm_ops.suspend(priv) */
+        priv->cfg->ops->lib->apm_ops.reset(priv);
         priv->cfg->ops->lib->free_shared_mem(priv);
 
  exit:
-        memset(&priv->card_alive, 0, sizeof(struct iwl4965_alive_resp));
+        memset(&priv->card_alive, 0, sizeof(struct iwl_alive_resp));
 
         if (priv->ibss_beacon)
                 dev_kfree_skb(priv->ibss_beacon);
         priv->ibss_beacon = NULL;
 
         /* clear out any free frames */
-        iwl4965_clear_free_frames(priv);
+        iwl_clear_free_frames(priv);
 }
 
 static void iwl4965_down(struct iwl_priv *priv)
@@ -4477,7 +3121,7 @@ static int __iwl4965_up(struct iwl_priv *priv)
  *
  *****************************************************************************/
 
-static void iwl4965_bg_init_alive_start(struct work_struct *data)
+static void iwl_bg_init_alive_start(struct work_struct *data)
 {
         struct iwl_priv *priv =
             container_of(data, struct iwl_priv, init_alive_start.work);
@@ -4490,7 +3134,7 @@ static void iwl4965_bg_init_alive_start(struct work_struct *data)
         mutex_unlock(&priv->mutex);
 }
 
-static void iwl4965_bg_alive_start(struct work_struct *data)
+static void iwl_bg_alive_start(struct work_struct *data)
 {
         struct iwl_priv *priv =
             container_of(data, struct iwl_priv, alive_start.work);
@@ -4499,7 +3143,7 @@ static void iwl4965_bg_alive_start(struct work_struct *data)
                 return;
 
         mutex_lock(&priv->mutex);
-        iwl4965_alive_start(priv);
+        iwl_alive_start(priv);
         mutex_unlock(&priv->mutex);
 }
 
@@ -4909,8 +3553,8 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
                 /* clear out the station table */
                 iwlcore_clear_stations_table(priv);
 
-                iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0);
-                iwl4965_rxon_add_station(priv, priv->bssid, 0);
+                iwl_rxon_add_station(priv, iwl_bcast_addr, 0);
+                iwl_rxon_add_station(priv, priv->bssid, 0);
                 iwl4965_rate_scale_init(priv->hw, IWL_STA_ID);
                 iwl4965_send_beacon_cmd(priv);
 
@@ -5021,7 +3665,7 @@ static int iwl4965_mac_start(struct ieee80211_hw *hw)
         /* we should be verifying the device is ready to be opened */
         mutex_lock(&priv->mutex);
 
-        memset(&priv->staging_rxon, 0, sizeof(struct iwl4965_rxon_cmd));
+        memset(&priv->staging_rxon, 0, sizeof(struct iwl_rxon_cmd));
         /* fetch ucode file from disk, alloc and copy to bus-master buffers ...
          * ucode filename and max sizes are card-specific. */
 
@@ -5046,21 +3690,23 @@ static int iwl4965_mac_start(struct ieee80211_hw *hw)
         if (test_bit(STATUS_IN_SUSPEND, &priv->status))
                 return 0;
 
-        /* Wait for START_ALIVE from ucode. Otherwise callbacks from
+        /* Wait for START_ALIVE from Run Time ucode. Otherwise callbacks from
          * mac80211 will not be run successfully. */
-        ret = wait_event_interruptible_timeout(priv->wait_command_queue,
-                        test_bit(STATUS_READY, &priv->status),
-                        UCODE_READY_TIMEOUT);
-        if (!ret) {
-                if (!test_bit(STATUS_READY, &priv->status)) {
-                        IWL_ERROR("Wait for START_ALIVE timeout after %dms.\n",
-                                  jiffies_to_msecs(UCODE_READY_TIMEOUT));
-                        ret = -ETIMEDOUT;
-                        goto out_release_irq;
+        if (priv->ucode_type == UCODE_RT) {
+                ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+                                test_bit(STATUS_READY, &priv->status),
+                                UCODE_READY_TIMEOUT);
+                if (!ret) {
+                        if (!test_bit(STATUS_READY, &priv->status)) {
+                                IWL_ERROR("START_ALIVE timeout after %dms.\n",
+                                        jiffies_to_msecs(UCODE_READY_TIMEOUT));
+                                ret = -ETIMEDOUT;
+                                goto out_release_irq;
+                        }
                 }
-        }
 
-        priv->is_open = 1;
+                priv->is_open = 1;
+        }
         IWL_DEBUG_MAC80211("leave\n");
         return 0;
 
@@ -5108,8 +3754,7 @@ static void iwl4965_mac_stop(struct ieee80211_hw *hw)
         IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                      struct ieee80211_tx_control *ctl)
+static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct iwl_priv *priv = hw->priv;
 
@@ -5121,9 +3766,9 @@ static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
         }
 
         IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
-                     ctl->tx_rate->bitrate);
+                     ieee80211_get_tx_rate(hw, IEEE80211_SKB_CB(skb))->bitrate);
 
-        if (iwl4965_tx_skb(priv, skb, ctl))
+        if (iwl_tx_skb(priv, skb))
                 dev_kfree_skb_any(skb);
 
         IWL_DEBUG_MAC80211("leave\n");
@@ -5178,6 +3823,7 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
         const struct iwl_channel_info *ch_info;
         unsigned long flags;
         int ret = 0;
+        u16 channel;
 
         mutex_lock(&priv->mutex);
         IWL_DEBUG_MAC80211("enter to channel %d\n", conf->channel->hw_value);
@@ -5198,22 +3844,21 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
                 return 0;
         }
 
-        spin_lock_irqsave(&priv->lock, flags);
-
-        ch_info = iwl_get_channel_info(priv, conf->channel->band,
-                        ieee80211_frequency_to_channel(conf->channel->center_freq));
+        channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+        ch_info = iwl_get_channel_info(priv, conf->channel->band, channel);
         if (!is_channel_valid(ch_info)) {
                 IWL_DEBUG_MAC80211("leave - invalid channel\n");
-                spin_unlock_irqrestore(&priv->lock, flags);
                 ret = -EINVAL;
                 goto out;
         }
 
+        spin_lock_irqsave(&priv->lock, flags);
+
 #ifdef CONFIG_IWL4965_HT
         /* if we are switching from ht to 2.4 clear flags
          * from any ht related info since 2.4 does not
          * support ht */
-        if ((le16_to_cpu(priv->staging_rxon.channel) != conf->channel->hw_value)
+        if ((le16_to_cpu(priv->staging_rxon.channel) != channel)
 #ifdef IEEE80211_CONF_CHANNEL_SWITCH
             && !(conf->flags & IEEE80211_CONF_CHANNEL_SWITCH)
 #endif
@@ -5221,10 +3866,9 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
                 priv->staging_rxon.flags = 0;
 #endif /* CONFIG_IWL4965_HT */
 
-        iwl_set_rxon_channel(priv, conf->channel->band,
-                ieee80211_frequency_to_channel(conf->channel->center_freq));
+        iwl_set_rxon_channel(priv, conf->channel->band, channel);
 
-        iwl4965_set_flags_for_phymode(priv, conf->channel->band);
+        iwl_set_flags_for_band(priv, conf->channel->band);
 
         /* The list of supported rates and rate mask can be different
          * for each band; since the band may have changed, reset
@@ -5321,7 +3965,7 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
                 priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
                 iwl4965_commit_rxon(priv);
                 iwl4965_activate_qos(priv, 1);
-                iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0);
+                iwl_rxon_add_station(priv, iwl_bcast_addr, 0);
         }
         iwl4965_send_beacon_cmd(priv);
 
@@ -5410,7 +4054,7 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
                 else {
                         rc = iwl4965_commit_rxon(priv);
                         if ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && rc)
-                                iwl4965_rxon_add_station(
+                                iwl_rxon_add_station(
                                         priv, priv->active_rxon.bssid_addr, 1);
                 }
 
@@ -5697,7 +4341,8 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
                 if (cmd == SET_KEY)
                         is_default_wep_key = !priv->key_mapping_key;
                 else
-                        is_default_wep_key = priv->default_wep_key;
+                        is_default_wep_key =
+                                        (key->hw_key_idx == HW_KEY_DEFAULT);
         }
 
         switch (cmd) {
@@ -5783,7 +4428,7 @@ static int iwl4965_mac_get_tx_stats(struct ieee80211_hw *hw,
         struct iwl_priv *priv = hw->priv;
         int i, avail;
         struct iwl_tx_queue *txq;
-        struct iwl4965_queue *q;
+        struct iwl_queue *q;
         unsigned long flags;
 
         IWL_DEBUG_MAC80211("enter\n");
@@ -5798,7 +4443,7 @@ static int iwl4965_mac_get_tx_stats(struct ieee80211_hw *hw,
         for (i = 0; i < AC_NUM; i++) {
                 txq = &priv->txq[i];
                 q = &txq->q;
-                avail = iwl4965_queue_space(q);
+                avail = iwl_queue_space(q);
 
                 stats[i].len = q->n_window - avail;
                 stats[i].limit = q->n_window - q->high_mark;
@@ -5904,8 +4549,7 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
         IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                                 struct ieee80211_tx_control *control)
+static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct iwl_priv *priv = hw->priv;
         unsigned long flags;
@@ -5998,7 +4642,7 @@ static ssize_t show_version(struct device *d,
                                 struct device_attribute *attr, char *buf)
 {
         struct iwl_priv *priv = d->driver_data;
-        struct iwl4965_alive_resp *palive = &priv->card_alive;
+        struct iwl_alive_resp *palive = &priv->card_alive;
 
         if (palive->is_valid)
                 return sprintf(buf, "fw version: 0x%01X.0x%01X.0x%01X.0x%01X\n"
@@ -6368,34 +5012,6 @@ static ssize_t show_status(struct device *d,
 
 static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
 
-static ssize_t dump_error_log(struct device *d,
-                              struct device_attribute *attr,
-                              const char *buf, size_t count)
-{
-        char *p = (char *)buf;
-
-        if (p[0] == '1')
-                iwl4965_dump_nic_error_log((struct iwl_priv *)d->driver_data);
-
-        return strnlen(buf, count);
-}
-
-static DEVICE_ATTR(dump_errors, S_IWUSR, NULL, dump_error_log);
-
-static ssize_t dump_event_log(struct device *d,
-                              struct device_attribute *attr,
-                              const char *buf, size_t count)
-{
-        char *p = (char *)buf;
-
-        if (p[0] == '1')
-                iwl4965_dump_nic_event_log((struct iwl_priv *)d->driver_data);
-
-        return strnlen(buf, count);
-}
-
-static DEVICE_ATTR(dump_events, S_IWUSR, NULL, dump_event_log);
-
 /*****************************************************************************
  *
  * driver setup and teardown
@@ -6418,8 +5034,8 @@ static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
         INIT_WORK(&priv->beacon_update, iwl4965_bg_beacon_update);
         INIT_WORK(&priv->set_monitor, iwl4965_bg_set_monitor);
         INIT_DELAYED_WORK(&priv->post_associate, iwl4965_bg_post_associate);
-        INIT_DELAYED_WORK(&priv->init_alive_start, iwl4965_bg_init_alive_start);
-        INIT_DELAYED_WORK(&priv->alive_start, iwl4965_bg_alive_start);
+        INIT_DELAYED_WORK(&priv->init_alive_start, iwl_bg_init_alive_start);
+        INIT_DELAYED_WORK(&priv->alive_start, iwl_bg_alive_start);
         INIT_DELAYED_WORK(&priv->scan_check, iwl4965_bg_scan_check);
 
         iwl4965_hw_setup_deferred_work(priv);
@@ -6441,8 +5057,6 @@ static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
 
 static struct attribute *iwl4965_sysfs_entries[] = {
         &dev_attr_channels.attr,
-        &dev_attr_dump_errors.attr,
-        &dev_attr_dump_events.attr,
         &dev_attr_flags.attr,
         &dev_attr_filter_flags.attr,
 #ifdef CONFIG_IWL4965_SPECTRUM_MEASUREMENT
@@ -6605,7 +5219,7 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
         if (err)
                 goto out_iounmap;
 
-        /* MAC Address location in EEPROM same for 3945/4965 */
+        /* extract MAC Address */
         iwl_eeprom_get_mac(priv, priv->mac_addr);
         IWL_DEBUG_INFO("MAC address: %s\n", print_mac(mac, priv->mac_addr));
         SET_IEEE80211_PERM_ADDR(priv->hw, priv->mac_addr);
@@ -6620,10 +5234,10 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
         }
 
         /*******************
-         * 6. Setup hw/priv
+         * 6. Setup priv
          *******************/
 
-        err = iwl_setup(priv);
+        err = iwl_init_drv(priv);
         if (err)
                 goto out_free_eeprom;
         /* At this point both hw and priv are initialized. */
@@ -6638,9 +5252,6 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
                 IWL_DEBUG_INFO("Radio disabled.\n");
         }
 
-        if (priv->cfg->mod_params->enable_qos)
-                priv->qos_data.qos_enable = 1;
-
         /********************
          * 8. Setup services
          ********************/
@@ -6651,14 +5262,9 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
         err = sysfs_create_group(&pdev->dev.kobj, &iwl4965_attribute_group);
         if (err) {
                 IWL_ERROR("failed to create sysfs device attributes\n");
-                goto out_free_eeprom;
+                goto out_uninit_drv;
         }
 
-        err = iwl_dbgfs_register(priv, DRV_NAME);
-        if (err) {
-                IWL_ERROR("failed to create debugfs files\n");
-                goto out_remove_sysfs;
-        }
 
         iwl4965_setup_deferred_work(priv);
         iwl4965_setup_rx_handlers(priv);
@@ -6669,12 +5275,26 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
         pci_save_state(pdev);
         pci_disable_device(pdev);
 
+        /**********************************
+         * 10. Setup and register mac80211
+         **********************************/
+
+        err = iwl_setup_mac(priv);
+        if (err)
+                goto out_remove_sysfs;
+
+        err = iwl_dbgfs_register(priv, DRV_NAME);
+        if (err)
+                IWL_ERROR("failed to create debugfs files\n");
+
         /* notify iwlcore to init */
         iwlcore_low_level_notify(priv, IWLCORE_INIT_EVT);
         return 0;
 
  out_remove_sysfs:
         sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
+ out_uninit_drv:
+        iwl_uninit_drv(priv);
  out_free_eeprom:
         iwl_eeprom_free(priv);
  out_iounmap:
@@ -6702,6 +5322,9 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
 
         IWL_DEBUG_INFO("*** UNLOAD DRIVER ***\n");
 
+        iwl_dbgfs_unregister(priv);
+        sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
+
         if (priv->mac80211_registered) {
                 ieee80211_unregister_hw(priv->hw);
                 priv->mac80211_registered = 0;
@@ -6729,8 +5352,6 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
         }
 
         iwlcore_low_level_notify(priv, IWLCORE_REMOVE_EVT);
-        iwl_dbgfs_unregister(priv);
-        sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
 
         iwl4965_dealloc_ucode_pci(priv);
 
@@ -6756,8 +5377,7 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
         pci_disable_device(pdev);
         pci_set_drvdata(pdev, NULL);
 
-        iwl_free_channel_map(priv);
-        iwlcore_free_geos(priv);
+        iwl_uninit_drv(priv);
 
         if (priv->ibss_beacon)
                 dev_kfree_skb(priv->ibss_beacon);
@@ -6848,10 +5468,6 @@ static int __init iwl4965_init(void)
 
         return ret;
 
-
-#ifdef CONFIG_IWLWIFI_DEBUG
-        pci_unregister_driver(&iwl_driver);
-#endif
 error_register:
         iwl4965_rate_control_unregister();
         return ret;
diff --git a/drivers/net/wireless/libertas/Makefile b/drivers/net/wireless/libertas/Makefile
index f0724e31adfd..02080a3682a9 100644
--- a/drivers/net/wireless/libertas/Makefile
+++ b/drivers/net/wireless/libertas/Makefile
@@ -1,9 +1,5 @@
-libertas-objs := main.o wext.o \
-                rx.o tx.o cmd.o           \
-                cmdresp.o scan.o          \
-                11d.o             \
-                debugfs.o         \
-                ethtool.o assoc.o
+libertas-objs := main.o wext.o rx.o tx.o cmd.o cmdresp.o scan.o 11d.o   \
+                 debugfs.o persistcfg.o ethtool.o assoc.o
 
 usb8xxx-objs += if_usb.o
 libertas_cs-objs += if_cs.o
diff --git a/drivers/net/wireless/libertas/assoc.c b/drivers/net/wireless/libertas/assoc.c
index c9c3640ce9fb..a267d6e65f03 100644
--- a/drivers/net/wireless/libertas/assoc.c
+++ b/drivers/net/wireless/libertas/assoc.c
@@ -603,7 +603,8 @@ static int assoc_helper_channel(struct lbs_private *priv,
                 /* Change mesh channel first; 21.p21 firmware won't let
                    you change channel otherwise (even though it'll return
                    an error to this */
-                lbs_mesh_config(priv, 0, assoc_req->channel);
+                lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_STOP,
+                                assoc_req->channel);
         }
 
         lbs_deb_assoc("ASSOC: channel: %d -> %d\n",
@@ -642,7 +643,8 @@ static int assoc_helper_channel(struct lbs_private *priv,
 
  restore_mesh:
         if (priv->mesh_dev)
-                lbs_mesh_config(priv, 1, priv->curbssparams.channel);
+                lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+                                priv->curbssparams.channel);
 
  done:
         lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
@@ -1248,7 +1250,7 @@ static int get_common_rates(struct lbs_private *priv,
         lbs_deb_hex(LBS_DEB_JOIN, "common rates", tmp, tmp_size);
         lbs_deb_join("TX data rate 0x%02x\n", priv->cur_rate);
 
-        if (!priv->auto_rate) {
+        if (!priv->enablehwauto) {
                 for (i = 0; i < tmp_size; i++) {
                         if (tmp[i] == priv->cur_rate)
                                 goto done;
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index c2dd43ece069..cf261d3487fd 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -4,6 +4,7 @@
   */
 
 #include <net/iw_handler.h>
+#include <net/ieee80211.h>
 #include <linux/kfifo.h>
 #include "host.h"
 #include "hostcmd.h"
@@ -109,7 +110,7 @@ int lbs_update_hw_spec(struct lbs_private *priv)
          * CF card    firmware 5.0.16p0:   cap 0x00000303
          * USB dongle firmware 5.110.17p2: cap 0x00000303
          */
-        printk("libertas: %s, fw %u.%u.%up%u, cap 0x%08x\n",
+        lbs_pr_info("%s, fw %u.%u.%up%u, cap 0x%08x\n",
                 print_mac(mac, cmd.permanentaddr),
                 priv->fwrelease >> 24 & 0xff,
                 priv->fwrelease >> 16 & 0xff,
@@ -675,26 +676,60 @@ static int lbs_cmd_802_11_monitor_mode(struct cmd_ds_command *cmd,
         return 0;
 }
 
-static int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
-                                              struct cmd_ds_command *cmd,
-                                              u16 cmd_action)
+static __le16 lbs_rate_to_fw_bitmap(int rate, int lower_rates_ok)
 {
-        struct cmd_ds_802_11_rate_adapt_rateset
-        *rateadapt = &cmd->params.rateset;
+/*              Bit     Rate
+*               15:13 Reserved
+*               12    54 Mbps
+*               11    48 Mbps
+*               10    36 Mbps
+*               9     24 Mbps
+*               8     18 Mbps
+*               7     12 Mbps
+*               6     9 Mbps
+*               5     6 Mbps
+*               4     Reserved
+*               3     11 Mbps
+*               2     5.5 Mbps
+*               1     2 Mbps
+*               0     1 Mbps
+**/
+
+        uint16_t ratemask;
+        int i = lbs_data_rate_to_fw_index(rate);
+        if (lower_rates_ok)
+                ratemask = (0x1fef >> (12 - i));
+        else
+                ratemask = (1 << i);
+        return cpu_to_le16(ratemask);
+}
+
+int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
+                                      uint16_t cmd_action)
+{
+        struct cmd_ds_802_11_rate_adapt_rateset cmd;
+        int ret;
 
         lbs_deb_enter(LBS_DEB_CMD);
-        cmd->size =
-            cpu_to_le16(sizeof(struct cmd_ds_802_11_rate_adapt_rateset)
-                             + S_DS_GEN);
-        cmd->command = cpu_to_le16(CMD_802_11_RATE_ADAPT_RATESET);
 
-        rateadapt->action = cpu_to_le16(cmd_action);
-        rateadapt->enablehwauto = cpu_to_le16(priv->enablehwauto);
-        rateadapt->bitmap = cpu_to_le16(priv->ratebitmap);
+        if (!priv->cur_rate && !priv->enablehwauto)
+                return -EINVAL;
 
-        lbs_deb_leave(LBS_DEB_CMD);
-        return 0;
+        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+
+        cmd.action = cpu_to_le16(cmd_action);
+        cmd.enablehwauto = cpu_to_le16(priv->enablehwauto);
+        cmd.bitmap = lbs_rate_to_fw_bitmap(priv->cur_rate, priv->enablehwauto);
+        ret = lbs_cmd_with_response(priv, CMD_802_11_RATE_ADAPT_RATESET, &cmd);
+        if (!ret && cmd_action == CMD_ACT_GET) {
+                priv->ratebitmap = le16_to_cpu(cmd.bitmap);
+                priv->enablehwauto = le16_to_cpu(cmd.enablehwauto);
+        }
+
+        lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+        return ret;
 }
+EXPORT_SYMBOL_GPL(lbs_cmd_802_11_rate_adapt_rateset);
 
 /**
  *  @brief Set the data rate
@@ -746,28 +781,6 @@ out:
         return ret;
 }
 
-static int lbs_cmd_mac_multicast_adr(struct lbs_private *priv,
-                                      struct cmd_ds_command *cmd,
-                                      u16 cmd_action)
-{
-        struct cmd_ds_mac_multicast_adr *pMCastAdr = &cmd->params.madr;
-
-        lbs_deb_enter(LBS_DEB_CMD);
-        cmd->size = cpu_to_le16(sizeof(struct cmd_ds_mac_multicast_adr) +
-                             S_DS_GEN);
-        cmd->command = cpu_to_le16(CMD_MAC_MULTICAST_ADR);
-
-        lbs_deb_cmd("MULTICAST_ADR: setting %d addresses\n", pMCastAdr->nr_of_adrs);
-        pMCastAdr->action = cpu_to_le16(cmd_action);
-        pMCastAdr->nr_of_adrs =
-            cpu_to_le16((u16) priv->nr_of_multicastmacaddr);
-        memcpy(pMCastAdr->maclist, priv->multicastlist,
-               priv->nr_of_multicastmacaddr * ETH_ALEN);
-
-        lbs_deb_leave(LBS_DEB_CMD);
-        return 0;
-}
-
 /**
  *  @brief Get the radio channel
  *
@@ -1020,24 +1033,69 @@ int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
         return ret;
 }
 
-int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan)
+int lbs_mesh_config_send(struct lbs_private *priv,
+                         struct cmd_ds_mesh_config *cmd,
+                         uint16_t action, uint16_t type)
+{
+        int ret;
+
+        lbs_deb_enter(LBS_DEB_CMD);
+
+        cmd->hdr.command = cpu_to_le16(CMD_MESH_CONFIG);
+        cmd->hdr.size = cpu_to_le16(sizeof(struct cmd_ds_mesh_config));
+        cmd->hdr.result = 0;
+
+        cmd->type = cpu_to_le16(type);
+        cmd->action = cpu_to_le16(action);
+
+        ret = lbs_cmd_with_response(priv, CMD_MESH_CONFIG, cmd);
+
+        lbs_deb_leave(LBS_DEB_CMD);
+        return ret;
+}
+
+/* This function is the CMD_MESH_CONFIG legacy function.  It only handles the
+ * START and STOP actions.  The extended actions supported by CMD_MESH_CONFIG
+ * are all handled by preparing a struct cmd_ds_mesh_config and passing it to
+ * lbs_mesh_config_send.
+ */
+int lbs_mesh_config(struct lbs_private *priv, uint16_t action, uint16_t chan)
 {
         struct cmd_ds_mesh_config cmd;
+        struct mrvl_meshie *ie;
 
         memset(&cmd, 0, sizeof(cmd));
-        cmd.action = cpu_to_le16(enable);
         cmd.channel = cpu_to_le16(chan);
-        cmd.type = cpu_to_le16(priv->mesh_tlv);
-        cmd.hdr.size = cpu_to_le16(sizeof(cmd));
-
-        if (enable) {
-                cmd.length = cpu_to_le16(priv->mesh_ssid_len);
-                memcpy(cmd.data, priv->mesh_ssid, priv->mesh_ssid_len);
+        ie = (struct mrvl_meshie *)cmd.data;
+
+        switch (action) {
+        case CMD_ACT_MESH_CONFIG_START:
+                ie->hdr.id = MFIE_TYPE_GENERIC;
+                ie->val.oui[0] = 0x00;
+                ie->val.oui[1] = 0x50;
+                ie->val.oui[2] = 0x43;
+                ie->val.type = MARVELL_MESH_IE_TYPE;
+                ie->val.subtype = MARVELL_MESH_IE_SUBTYPE;
+                ie->val.version = MARVELL_MESH_IE_VERSION;
+                ie->val.active_protocol_id = MARVELL_MESH_PROTO_ID_HWMP;
+                ie->val.active_metric_id = MARVELL_MESH_METRIC_ID;
+                ie->val.mesh_capability = MARVELL_MESH_CAPABILITY;
+                ie->val.mesh_id_len = priv->mesh_ssid_len;
+                memcpy(ie->val.mesh_id, priv->mesh_ssid, priv->mesh_ssid_len);
+                ie->hdr.len = sizeof(struct mrvl_meshie_val) -
+                        IW_ESSID_MAX_SIZE + priv->mesh_ssid_len;
+                cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie_val));
+                break;
+        case CMD_ACT_MESH_CONFIG_STOP:
+                break;
+        default:
+                return -1;
         }
-        lbs_deb_cmd("mesh config enable %d TLV %x channel %d SSID %s\n",
-                    enable, priv->mesh_tlv, chan,
+        lbs_deb_cmd("mesh config action %d type %x channel %d SSID %s\n",
+                    action, priv->mesh_tlv, chan,
                     escape_essid(priv->mesh_ssid, priv->mesh_ssid_len));
-        return lbs_cmd_with_response(priv, CMD_MESH_CONFIG, &cmd);
+
+        return lbs_mesh_config_send(priv, &cmd, action, priv->mesh_tlv);
 }
 
 static int lbs_cmd_bcn_ctrl(struct lbs_private * priv,
@@ -1112,7 +1170,7 @@ static void lbs_submit_command(struct lbs_private *priv,
         struct cmd_header *cmd;
         uint16_t cmdsize;
         uint16_t command;
-        int timeo = 5 * HZ;
+        int timeo = 3 * HZ;
         int ret;
 
         lbs_deb_enter(LBS_DEB_HOST);
@@ -1130,7 +1188,7 @@ static void lbs_submit_command(struct lbs_private *priv,
         /* These commands take longer */
         if (command == CMD_802_11_SCAN || command == CMD_802_11_ASSOCIATE ||
             command == CMD_802_11_AUTHENTICATE)
-                timeo = 10 * HZ;
+                timeo = 5 * HZ;
 
         lbs_deb_cmd("DNLD_CMD: command 0x%04x, seq %d, size %d\n",
                      command, le16_to_cpu(cmd->seqnum), cmdsize);
@@ -1142,7 +1200,7 @@ static void lbs_submit_command(struct lbs_private *priv,
                 lbs_pr_info("DNLD_CMD: hw_host_to_card failed: %d\n", ret);
                 /* Let the timer kick in and retry, and potentially reset
                    the whole thing if the condition persists */
-                timeo = HZ;
+                timeo = HZ/4;
         }
 
         /* Setup the timer after transmit command */
@@ -1247,8 +1305,7 @@ void lbs_set_mac_control(struct lbs_private *priv)
         cmd.action = cpu_to_le16(priv->mac_control);
         cmd.reserved = 0;
 
-        lbs_cmd_async(priv, CMD_MAC_CONTROL,
-                &cmd.hdr, sizeof(cmd));
+        lbs_cmd_async(priv, CMD_MAC_CONTROL, &cmd.hdr, sizeof(cmd));
 
         lbs_deb_leave(LBS_DEB_CMD);
 }
@@ -1355,15 +1412,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
                                                  cmd_action, pdata_buf);
                 break;
 
-        case CMD_802_11_RATE_ADAPT_RATESET:
-                ret = lbs_cmd_802_11_rate_adapt_rateset(priv,
-                                                         cmdptr, cmd_action);
-                break;
-
-        case CMD_MAC_MULTICAST_ADR:
-                ret = lbs_cmd_mac_multicast_adr(priv, cmdptr, cmd_action);
-                break;
-
         case CMD_802_11_MONITOR_MODE:
                 ret = lbs_cmd_802_11_monitor_mode(cmdptr,
                                           cmd_action, pdata_buf);
@@ -1452,7 +1500,7 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
                 ret = lbs_cmd_bcn_ctrl(priv, cmdptr, cmd_action);
                 break;
         default:
-                lbs_deb_host("PREP_CMD: unknown command 0x%04x\n", cmd_no);
+                lbs_pr_err("PREP_CMD: unknown command 0x%04x\n", cmd_no);
                 ret = -1;
                 break;
         }
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index f4019c22adfc..a53b51f8bdb4 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -39,12 +39,17 @@ int lbs_set_data_rate(struct lbs_private *priv, u8 rate);
 int lbs_get_channel(struct lbs_private *priv);
 int lbs_set_channel(struct lbs_private *priv, u8 channel);
 
+int lbs_mesh_config_send(struct lbs_private *priv,
+                         struct cmd_ds_mesh_config *cmd,
+                         uint16_t action, uint16_t type);
 int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan);
 
 int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria);
 int lbs_suspend(struct lbs_private *priv);
 void lbs_resume(struct lbs_private *priv);
 
+int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
+                                      uint16_t cmd_action);
 int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
                                       uint16_t cmd_action, uint16_t *timeout);
 int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 5abecb7673e6..24de3c3cf877 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -203,22 +203,6 @@ static int lbs_ret_802_11_rf_tx_power(struct lbs_private *priv,
         return 0;
 }
 
-static int lbs_ret_802_11_rate_adapt_rateset(struct lbs_private *priv,
-                                              struct cmd_ds_command *resp)
-{
-        struct cmd_ds_802_11_rate_adapt_rateset *rates = &resp->params.rateset;
-
-        lbs_deb_enter(LBS_DEB_CMD);
-
-        if (rates->action == CMD_ACT_GET) {
-                priv->enablehwauto = le16_to_cpu(rates->enablehwauto);
-                priv->ratebitmap = le16_to_cpu(rates->bitmap);
-        }
-
-        lbs_deb_leave(LBS_DEB_CMD);
-        return 0;
-}
-
 static int lbs_ret_802_11_rssi(struct lbs_private *priv,
                                 struct cmd_ds_command *resp)
 {
@@ -316,16 +300,11 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
                 break;
 
-        case CMD_RET(CMD_MAC_MULTICAST_ADR):
         case CMD_RET(CMD_802_11_RESET):
         case CMD_RET(CMD_802_11_AUTHENTICATE):
         case CMD_RET(CMD_802_11_BEACON_STOP):
                 break;
 
-        case CMD_RET(CMD_802_11_RATE_ADAPT_RATESET):
-                ret = lbs_ret_802_11_rate_adapt_rateset(priv, resp);
-                break;
-
         case CMD_RET(CMD_802_11_RSSI):
                 ret = lbs_ret_802_11_rssi(priv, resp);
                 break;
@@ -376,8 +355,8 @@ static inline int handle_cmd_response(struct lbs_private *priv,
                 break;
 
         default:
-                lbs_deb_host("CMD_RESP: unknown cmd response 0x%04x\n",
-                             le16_to_cpu(resp->command));
+                lbs_pr_err("CMD_RESP: unknown cmd response 0x%04x\n",
+                           le16_to_cpu(resp->command));
                 break;
         }
         lbs_deb_leave(LBS_DEB_HOST);
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index 0632b09655d2..a8ac974dacac 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -60,6 +60,10 @@ void lbs_mac_event_disconnected(struct lbs_private *priv);
 
 void lbs_send_iwevcustom_event(struct lbs_private *priv, s8 *str);
 
+/* persistcfg.c */
+void lbs_persist_config_init(struct net_device *net);
+void lbs_persist_config_remove(struct net_device *net);
+
 /* main.c */
 struct chan_freq_power *lbs_get_region_cfp_table(u8 region,
         int *cfp_no);
diff --git a/drivers/net/wireless/libertas/defs.h b/drivers/net/wireless/libertas/defs.h
index d39520111062..12e687550bce 100644
--- a/drivers/net/wireless/libertas/defs.h
+++ b/drivers/net/wireless/libertas/defs.h
@@ -40,6 +40,7 @@
 #define LBS_DEB_THREAD  0x00100000
 #define LBS_DEB_HEX     0x00200000
 #define LBS_DEB_SDIO    0x00400000
+#define LBS_DEB_SYSFS   0x00800000
 
 extern unsigned int lbs_debug;
 
@@ -81,7 +82,8 @@ do { if ((lbs_debug & (grp)) == (grp)) \
 #define lbs_deb_usbd(dev, fmt, args...) LBS_DEB_LL(LBS_DEB_USB, " usbd", "%s:" fmt, (dev)->bus_id, ##args)
 #define lbs_deb_cs(fmt, args...)        LBS_DEB_LL(LBS_DEB_CS, " cs", fmt, ##args)
 #define lbs_deb_thread(fmt, args...)    LBS_DEB_LL(LBS_DEB_THREAD, " thread", fmt, ##args)
-#define lbs_deb_sdio(fmt, args...)      LBS_DEB_LL(LBS_DEB_SDIO, " thread", fmt, ##args)
+#define lbs_deb_sdio(fmt, args...)      LBS_DEB_LL(LBS_DEB_SDIO, " sdio", fmt, ##args)
+#define lbs_deb_sysfs(fmt, args...)     LBS_DEB_LL(LBS_DEB_SYSFS, " sysfs", fmt, ##args)
 
 #define lbs_pr_info(format, args...) \
         printk(KERN_INFO DRV_NAME": " format, ## args)
@@ -170,6 +172,16 @@ static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, in
 
 #define MARVELL_MESH_IE_LENGTH          9
 
+/* Values used to populate the struct mrvl_mesh_ie.  The only time you need this
+ * is when enabling the mesh using CMD_MESH_CONFIG.
+ */
+#define MARVELL_MESH_IE_TYPE            4
+#define MARVELL_MESH_IE_SUBTYPE         0
+#define MARVELL_MESH_IE_VERSION         0
+#define MARVELL_MESH_PROTO_ID_HWMP      0
+#define MARVELL_MESH_METRIC_ID          0
+#define MARVELL_MESH_CAPABILITY         0
+
 /** INT status Bit Definition*/
 #define MRVDRV_TX_DNLD_RDY              0x0001
 #define MRVDRV_RX_UPLD_RDY              0x0002
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 0d9edb9b11f5..f5bb40c54d85 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -140,6 +140,8 @@ struct lbs_private {
         wait_queue_head_t waitq;
         struct workqueue_struct *work_thread;
 
+        struct work_struct mcast_work;
+
         /** Scanning */
         struct delayed_work scan_work;
         struct delayed_work assoc_work;
@@ -151,6 +153,7 @@ struct lbs_private {
 
         /** Hardware access */
         int (*hw_host_to_card) (struct lbs_private *priv, u8 type, u8 *payload, u16 nb);
+        void (*reset_card) (struct lbs_private *priv);
 
         /* Wake On LAN */
         uint32_t wol_criteria;
@@ -234,8 +237,8 @@ struct lbs_private {
         /** 802.11 statistics */
 //      struct cmd_DS_802_11_GET_STAT wlan802_11Stat;
 
-        u16 enablehwauto;
-        u16 ratebitmap;
+        uint16_t enablehwauto;
+        uint16_t ratebitmap;
 
         u32 fragthsd;
         u32 rtsthsd;
@@ -293,7 +296,6 @@ struct lbs_private {
 
         /** data rate stuff */
         u8 cur_rate;
-        u8 auto_rate;
 
         /** RF calibration data */
 
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index 3915c3144fad..c92e41b4faf4 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -256,6 +256,23 @@ enum cmd_mesh_access_opts {
         CMD_ACT_MESH_GET_AUTOSTART_ENABLED,
 };
 
+/* Define actions and types for CMD_MESH_CONFIG */
+enum cmd_mesh_config_actions {
+        CMD_ACT_MESH_CONFIG_STOP = 0,
+        CMD_ACT_MESH_CONFIG_START,
+        CMD_ACT_MESH_CONFIG_SET,
+        CMD_ACT_MESH_CONFIG_GET,
+};
+
+enum cmd_mesh_config_types {
+        CMD_TYPE_MESH_SET_BOOTFLAG = 1,
+        CMD_TYPE_MESH_SET_BOOTTIME,
+        CMD_TYPE_MESH_SET_DEF_CHANNEL,
+        CMD_TYPE_MESH_SET_MESH_IE,
+        CMD_TYPE_MESH_GET_DEFAULTS,
+        CMD_TYPE_MESH_GET_MESH_IE, /* GET_DEFAULTS is superset of GET_MESHIE */
+};
+
 /** Card Event definition */
 #define MACREG_INT_CODE_TX_PPA_FREE             0
 #define MACREG_INT_CODE_TX_DMA_DONE             1
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index f29bc5bbda3e..913b480211a9 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -219,6 +219,7 @@ struct cmd_ds_mac_control {
 };
 
 struct cmd_ds_mac_multicast_adr {
+        struct cmd_header hdr;
         __le16 action;
         __le16 nr_of_adrs;
         u8 maclist[ETH_ALEN * MRVDRV_MAX_MULTICAST_LIST_SIZE];
@@ -499,6 +500,7 @@ struct cmd_ds_802_11_data_rate {
 };
 
 struct cmd_ds_802_11_rate_adapt_rateset {
+        struct cmd_header hdr;
         __le16 action;
         __le16 enablehwauto;
         __le16 bitmap;
@@ -702,8 +704,6 @@ struct cmd_ds_command {
                 struct cmd_ds_802_11_rf_tx_power txp;
                 struct cmd_ds_802_11_rf_antenna rant;
                 struct cmd_ds_802_11_monitor_mode monitor;
-                struct cmd_ds_802_11_rate_adapt_rateset rateset;
-                struct cmd_ds_mac_multicast_adr madr;
                 struct cmd_ds_802_11_ad_hoc_join adj;
                 struct cmd_ds_802_11_rssi rssi;
                 struct cmd_ds_802_11_rssi_rsp rssirsp;
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index 54280e292ea5..873ab10a0786 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -148,76 +148,72 @@ static int if_cs_poll_while_fw_download(struct if_cs_card *card, uint addr, u8 r
 {
         int i;
 
-        for (i = 0; i < 1000; i++) {
+        for (i = 0; i < 100000; i++) {
                 u8 val = if_cs_read8(card, addr);
                 if (val == reg)
                         return i;
-                udelay(500);
+                udelay(5);
         }
         return -ETIME;
 }
 
 
 
-/* Host control registers and their bit definitions */
+/* First the bitmasks for the host/card interrupt/status registers: */
+#define IF_CS_BIT_TX                    0x0001
+#define IF_CS_BIT_RX                    0x0002
+#define IF_CS_BIT_COMMAND               0x0004
+#define IF_CS_BIT_RESP                  0x0008
+#define IF_CS_BIT_EVENT                 0x0010
+#define IF_CS_BIT_MASK                  0x001f
 
-#define IF_CS_H_STATUS                  0x00000000
-#define IF_CS_H_STATUS_TX_OVER          0x0001
-#define IF_CS_H_STATUS_RX_OVER          0x0002
-#define IF_CS_H_STATUS_DNLD_OVER        0x0004
+/* And now the individual registers and assorted masks */
+#define IF_CS_HOST_STATUS               0x00000000
 
-#define IF_CS_H_INT_CAUSE               0x00000002
-#define IF_CS_H_IC_TX_OVER              0x0001
-#define IF_CS_H_IC_RX_OVER              0x0002
-#define IF_CS_H_IC_DNLD_OVER            0x0004
-#define IF_CS_H_IC_POWER_DOWN           0x0008
-#define IF_CS_H_IC_HOST_EVENT           0x0010
-#define IF_CS_H_IC_MASK                 0x001f
+#define IF_CS_HOST_INT_CAUSE            0x00000002
 
-#define IF_CS_H_INT_MASK                0x00000004
-#define IF_CS_H_IM_MASK                 0x001f
+#define IF_CS_HOST_INT_MASK             0x00000004
 
-#define IF_CS_H_WRITE_LEN               0x00000014
+#define IF_CS_HOST_WRITE                0x00000016
+#define IF_CS_HOST_WRITE_LEN            0x00000014
 
-#define IF_CS_H_WRITE                   0x00000016
+#define IF_CS_HOST_CMD                  0x0000001A
+#define IF_CS_HOST_CMD_LEN              0x00000018
 
-#define IF_CS_H_CMD_LEN                 0x00000018
+#define IF_CS_READ                      0x00000010
+#define IF_CS_READ_LEN                  0x00000024
 
-#define IF_CS_H_CMD                     0x0000001A
+#define IF_CS_CARD_CMD                  0x00000012
+#define IF_CS_CARD_CMD_LEN              0x00000030
 
-#define IF_CS_C_READ_LEN                0x00000024
+#define IF_CS_CARD_STATUS               0x00000020
+#define IF_CS_CARD_STATUS_MASK          0x7f00
 
-#define IF_CS_H_READ                    0x00000010
+#define IF_CS_CARD_INT_CAUSE            0x00000022
 
-/* Card control registers and their bit definitions */
-
-#define IF_CS_C_STATUS                  0x00000020
-#define IF_CS_C_S_TX_DNLD_RDY           0x0001
-#define IF_CS_C_S_RX_UPLD_RDY           0x0002
-#define IF_CS_C_S_CMD_DNLD_RDY          0x0004
-#define IF_CS_C_S_CMD_UPLD_RDY          0x0008
-#define IF_CS_C_S_CARDEVENT             0x0010
-#define IF_CS_C_S_MASK                  0x001f
-#define IF_CS_C_S_STATUS_MASK           0x7f00
-
-#define IF_CS_C_INT_CAUSE               0x00000022
-#define IF_CS_C_IC_MASK                 0x001f
-
-#define IF_CS_C_SQ_READ_LOW             0x00000028
-#define IF_CS_C_SQ_HELPER_OK            0x10
-
-#define IF_CS_C_CMD_LEN                 0x00000030
-
-#define IF_CS_C_CMD                     0x00000012
+#define IF_CS_CARD_SQ_READ_LOW          0x00000028
+#define IF_CS_CARD_SQ_HELPER_OK         0x10
 
 #define IF_CS_SCRATCH                   0x0000003F
 
 
 
 /********************************************************************/
-/* I/O                                                              */
+/* I/O and interrupt handling                                       */
 /********************************************************************/
 
+static inline void if_cs_enable_ints(struct if_cs_card *card)
+{
+        lbs_deb_enter(LBS_DEB_CS);
+        if_cs_write16(card, IF_CS_HOST_INT_MASK, 0);
+}
+
+static inline void if_cs_disable_ints(struct if_cs_card *card)
+{
+        lbs_deb_enter(LBS_DEB_CS);
+        if_cs_write16(card, IF_CS_HOST_INT_MASK, IF_CS_BIT_MASK);
+}
+
 /*
  * Called from if_cs_host_to_card to send a command to the hardware
  */
@@ -228,11 +224,12 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
         int loops = 0;
 
         lbs_deb_enter(LBS_DEB_CS);
+        if_cs_disable_ints(card);
 
         /* Is hardware ready? */
         while (1) {
-                u16 val = if_cs_read16(card, IF_CS_C_STATUS);
-                if (val & IF_CS_C_S_CMD_DNLD_RDY)
+                u16 val = if_cs_read16(card, IF_CS_CARD_STATUS);
+                if (val & IF_CS_BIT_COMMAND)
                         break;
                 if (++loops > 100) {
                         lbs_pr_err("card not ready for commands\n");
@@ -241,51 +238,56 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
                 mdelay(1);
         }
 
-        if_cs_write16(card, IF_CS_H_CMD_LEN, nb);
+        if_cs_write16(card, IF_CS_HOST_CMD_LEN, nb);
 
-        if_cs_write16_rep(card, IF_CS_H_CMD, buf, nb / 2);
+        if_cs_write16_rep(card, IF_CS_HOST_CMD, buf, nb / 2);
         /* Are we supposed to transfer an odd amount of bytes? */
         if (nb & 1)
-                if_cs_write8(card, IF_CS_H_CMD, buf[nb-1]);
+                if_cs_write8(card, IF_CS_HOST_CMD, buf[nb-1]);
 
         /* "Assert the download over interrupt command in the Host
          * status register" */
-        if_cs_write16(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
+        if_cs_write16(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
 
         /* "Assert the download over interrupt command in the Card
          * interrupt case register" */
-        if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+        if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
         ret = 0;
 
 done:
+        if_cs_enable_ints(card);
         lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
         return ret;
 }
 
-
 /*
  * Called from if_cs_host_to_card to send a data to the hardware
  */
 static void if_cs_send_data(struct lbs_private *priv, u8 *buf, u16 nb)
 {
         struct if_cs_card *card = (struct if_cs_card *)priv->card;
+        u16 status;
 
         lbs_deb_enter(LBS_DEB_CS);
+        if_cs_disable_ints(card);
+
+        status = if_cs_read16(card, IF_CS_CARD_STATUS);
+        BUG_ON((status & IF_CS_BIT_TX) == 0);
 
-        if_cs_write16(card, IF_CS_H_WRITE_LEN, nb);
+        if_cs_write16(card, IF_CS_HOST_WRITE_LEN, nb);
 
         /* write even number of bytes, then odd byte if necessary */
-        if_cs_write16_rep(card, IF_CS_H_WRITE, buf, nb / 2);
+        if_cs_write16_rep(card, IF_CS_HOST_WRITE, buf, nb / 2);
         if (nb & 1)
-                if_cs_write8(card, IF_CS_H_WRITE, buf[nb-1]);
+                if_cs_write8(card, IF_CS_HOST_WRITE, buf[nb-1]);
 
-        if_cs_write16(card, IF_CS_H_STATUS, IF_CS_H_STATUS_TX_OVER);
-        if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_STATUS_TX_OVER);
+        if_cs_write16(card, IF_CS_HOST_STATUS, IF_CS_BIT_TX);
+        if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_TX);
+        if_cs_enable_ints(card);
 
         lbs_deb_leave(LBS_DEB_CS);
 }
 
-
 /*
  * Get the command result out of the card.
  */
@@ -293,27 +295,28 @@ static int if_cs_receive_cmdres(struct lbs_private *priv, u8 *data, u32 *len)
 {
         unsigned long flags;
         int ret = -1;
-        u16 val;
+        u16 status;
 
         lbs_deb_enter(LBS_DEB_CS);
 
         /* is hardware ready? */
-        val = if_cs_read16(priv->card, IF_CS_C_STATUS);
-        if ((val & IF_CS_C_S_CMD_UPLD_RDY) == 0) {
-                lbs_pr_err("card not ready for CMD\n");
+        status = if_cs_read16(priv->card, IF_CS_CARD_STATUS);
+        if ((status & IF_CS_BIT_RESP) == 0) {
+                lbs_pr_err("no cmd response in card\n");
+                *len = 0;
                 goto out;
         }
 
-        *len = if_cs_read16(priv->card, IF_CS_C_CMD_LEN);
+        *len = if_cs_read16(priv->card, IF_CS_CARD_CMD_LEN);
         if ((*len == 0) || (*len > LBS_CMD_BUFFER_SIZE)) {
                 lbs_pr_err("card cmd buffer has invalid # of bytes (%d)\n", *len);
                 goto out;
         }
 
         /* read even number of bytes, then odd byte if necessary */
-        if_cs_read16_rep(priv->card, IF_CS_C_CMD, data, *len/sizeof(u16));
+        if_cs_read16_rep(priv->card, IF_CS_CARD_CMD, data, *len/sizeof(u16));
         if (*len & 1)
-                data[*len-1] = if_cs_read8(priv->card, IF_CS_C_CMD);
+                data[*len-1] = if_cs_read8(priv->card, IF_CS_CARD_CMD);
 
         /* This is a workaround for a firmware that reports too much
          * bytes */
@@ -330,7 +333,6 @@ out:
         return ret;
 }
 
-
 static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 {
         struct sk_buff *skb = NULL;
@@ -339,7 +341,7 @@ static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 
         lbs_deb_enter(LBS_DEB_CS);
 
-        len = if_cs_read16(priv->card, IF_CS_C_READ_LEN);
+        len = if_cs_read16(priv->card, IF_CS_READ_LEN);
         if (len == 0 || len > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE) {
                 lbs_pr_err("card data buffer has invalid # of bytes (%d)\n", len);
                 priv->stats.rx_dropped++;
@@ -354,38 +356,19 @@ static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
         data = skb->data;
 
         /* read even number of bytes, then odd byte if necessary */
-        if_cs_read16_rep(priv->card, IF_CS_H_READ, data, len/sizeof(u16));
+        if_cs_read16_rep(priv->card, IF_CS_READ, data, len/sizeof(u16));
         if (len & 1)
-                data[len-1] = if_cs_read8(priv->card, IF_CS_H_READ);
+                data[len-1] = if_cs_read8(priv->card, IF_CS_READ);
 
 dat_err:
-        if_cs_write16(priv->card, IF_CS_H_STATUS, IF_CS_H_STATUS_RX_OVER);
-        if_cs_write16(priv->card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_RX_OVER);
+        if_cs_write16(priv->card, IF_CS_HOST_STATUS, IF_CS_BIT_RX);
+        if_cs_write16(priv->card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_RX);
 
 out:
         lbs_deb_leave_args(LBS_DEB_CS, "ret %p", skb);
         return skb;
 }
 
-
-
-/********************************************************************/
-/* Interrupts                                                       */
-/********************************************************************/
-
-static inline void if_cs_enable_ints(struct if_cs_card *card)
-{
-        lbs_deb_enter(LBS_DEB_CS);
-        if_cs_write16(card, IF_CS_H_INT_MASK, 0);
-}
-
-static inline void if_cs_disable_ints(struct if_cs_card *card)
-{
-        lbs_deb_enter(LBS_DEB_CS);
-        if_cs_write16(card, IF_CS_H_INT_MASK, IF_CS_H_IM_MASK);
-}
-
-
 static irqreturn_t if_cs_interrupt(int irq, void *data)
 {
         struct if_cs_card *card = data;
@@ -394,10 +377,8 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 
         lbs_deb_enter(LBS_DEB_CS);
 
-        cause = if_cs_read16(card, IF_CS_C_INT_CAUSE);
-        if_cs_write16(card, IF_CS_C_INT_CAUSE, cause & IF_CS_C_IC_MASK);
-
-        lbs_deb_cs("cause 0x%04x\n", cause);
+        /* Ask card interrupt cause register if there is something for us */
+        cause = if_cs_read16(card, IF_CS_CARD_INT_CAUSE);
         if (cause == 0) {
                 /* Not for us */
                 return IRQ_NONE;
@@ -409,11 +390,11 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
                 return IRQ_HANDLED;
         }
 
-        /* TODO: I'm not sure what the best ordering is */
-
-        cause = if_cs_read16(card, IF_CS_C_STATUS) & IF_CS_C_S_MASK;
+        /* Clear interrupt cause */
+        if_cs_write16(card, IF_CS_CARD_INT_CAUSE, cause & IF_CS_BIT_MASK);
+        lbs_deb_cs("cause 0x%04x\n", cause);
 
-        if (cause & IF_CS_C_S_RX_UPLD_RDY) {
+        if (cause & IF_CS_BIT_RX) {
                 struct sk_buff *skb;
                 lbs_deb_cs("rx packet\n");
                 skb = if_cs_receive_data(priv);
@@ -421,16 +402,16 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
                         lbs_process_rxed_packet(priv, skb);
         }
 
-        if (cause & IF_CS_H_IC_TX_OVER) {
-                lbs_deb_cs("tx over\n");
+        if (cause & IF_CS_BIT_TX) {
+                lbs_deb_cs("tx done\n");
                 lbs_host_to_card_done(priv);
         }
 
-        if (cause & IF_CS_C_S_CMD_UPLD_RDY) {
+        if (cause & IF_CS_BIT_RESP) {
                 unsigned long flags;
                 u8 i;
 
-                lbs_deb_cs("cmd upload ready\n");
+                lbs_deb_cs("cmd resp\n");
                 spin_lock_irqsave(&priv->driver_lock, flags);
                 i = (priv->resp_idx == 0) ? 1 : 0;
                 spin_unlock_irqrestore(&priv->driver_lock, flags);
@@ -444,15 +425,16 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
                 spin_unlock_irqrestore(&priv->driver_lock, flags);
         }
 
-        if (cause & IF_CS_H_IC_HOST_EVENT) {
-                u16 event = if_cs_read16(priv->card, IF_CS_C_STATUS)
-                        & IF_CS_C_S_STATUS_MASK;
-                if_cs_write16(priv->card, IF_CS_H_INT_CAUSE,
-                        IF_CS_H_IC_HOST_EVENT);
-                lbs_deb_cs("eventcause 0x%04x\n", event);
+        if (cause & IF_CS_BIT_EVENT) {
+                u16 event = if_cs_read16(priv->card, IF_CS_CARD_STATUS)
+                        & IF_CS_CARD_STATUS_MASK;
+                if_cs_write16(priv->card, IF_CS_HOST_INT_CAUSE,
+                        IF_CS_BIT_EVENT);
+                lbs_deb_cs("host event 0x%04x\n", event);
                 lbs_queue_event(priv, event >> 8 & 0xff);
         }
 
+        lbs_deb_leave(LBS_DEB_CS);
         return IRQ_HANDLED;
 }
 
@@ -514,26 +496,26 @@ static int if_cs_prog_helper(struct if_cs_card *card)
 
                 /* "write the number of bytes to be sent to the I/O Command
                  * write length register" */
-                if_cs_write16(card, IF_CS_H_CMD_LEN, count);
+                if_cs_write16(card, IF_CS_HOST_CMD_LEN, count);
 
                 /* "write this to I/O Command port register as 16 bit writes */
                 if (count)
-                        if_cs_write16_rep(card, IF_CS_H_CMD,
+                        if_cs_write16_rep(card, IF_CS_HOST_CMD,
                                 &fw->data[sent],
                                 count >> 1);
 
                 /* "Assert the download over interrupt command in the Host
                  * status register" */
-                if_cs_write8(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
+                if_cs_write8(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
 
                 /* "Assert the download over interrupt command in the Card
                  * interrupt case register" */
-                if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+                if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
 
                 /* "The host polls the Card Status register ... for 50 ms before
                    declaring a failure */
-                ret = if_cs_poll_while_fw_download(card, IF_CS_C_STATUS,
-                        IF_CS_C_S_CMD_DNLD_RDY);
+                ret = if_cs_poll_while_fw_download(card, IF_CS_CARD_STATUS,
+                        IF_CS_BIT_COMMAND);
                 if (ret < 0) {
                         lbs_pr_err("can't download helper at 0x%x, ret %d\n",
                                 sent, ret);
@@ -575,14 +557,15 @@ static int if_cs_prog_real(struct if_cs_card *card)
         }
         lbs_deb_cs("fw size %td\n", fw->size);
 
-        ret = if_cs_poll_while_fw_download(card, IF_CS_C_SQ_READ_LOW, IF_CS_C_SQ_HELPER_OK);
+        ret = if_cs_poll_while_fw_download(card, IF_CS_CARD_SQ_READ_LOW,
+                IF_CS_CARD_SQ_HELPER_OK);
         if (ret < 0) {
                 lbs_pr_err("helper firmware doesn't answer\n");
                 goto err_release;
         }
 
         for (sent = 0; sent < fw->size; sent += len) {
-                len = if_cs_read16(card, IF_CS_C_SQ_READ_LOW);
+                len = if_cs_read16(card, IF_CS_CARD_SQ_READ_LOW);
                 if (len & 1) {
                         retry++;
                         lbs_pr_info("odd, need to retry this firmware block\n");
@@ -600,16 +583,16 @@ static int if_cs_prog_real(struct if_cs_card *card)
                 }
 
 
-                if_cs_write16(card, IF_CS_H_CMD_LEN, len);
+                if_cs_write16(card, IF_CS_HOST_CMD_LEN, len);
 
-                if_cs_write16_rep(card, IF_CS_H_CMD,
+                if_cs_write16_rep(card, IF_CS_HOST_CMD,
                         &fw->data[sent],
                         (len+1) >> 1);
-                if_cs_write8(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
-                if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+                if_cs_write8(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
+                if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
 
-                ret = if_cs_poll_while_fw_download(card, IF_CS_C_STATUS,
-                        IF_CS_C_S_CMD_DNLD_RDY);
+                ret = if_cs_poll_while_fw_download(card, IF_CS_CARD_STATUS,
+                        IF_CS_BIT_COMMAND);
                 if (ret < 0) {
                         lbs_pr_err("can't download firmware at 0x%x\n", sent);
                         goto err_release;
@@ -837,7 +820,7 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 
         /* Clear any interrupt cause that happend while sending
          * firmware/initializing card */
-        if_cs_write16(card, IF_CS_C_INT_CAUSE, IF_CS_C_IC_MASK);
+        if_cs_write16(card, IF_CS_CARD_INT_CAUSE, IF_CS_BIT_MASK);
         if_cs_enable_ints(card);
 
         /* And finally bring the card up */
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 8032df72aaab..24783103a7dd 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -7,6 +7,10 @@
 #include <linux/netdevice.h>
 #include <linux/usb.h>
 
+#ifdef CONFIG_OLPC
+#include <asm/olpc.h>
+#endif
+
 #define DRV_NAME "usb8xxx"
 
 #include "host.h"
@@ -146,6 +150,14 @@ static void if_usb_fw_timeo(unsigned long priv)
         wake_up(&cardp->fw_wq);
 }
 
+#ifdef CONFIG_OLPC
+static void if_usb_reset_olpc_card(struct lbs_private *priv)
+{
+        printk(KERN_CRIT "Resetting OLPC wireless via EC...\n");
+        olpc_ec_cmd(0x25, NULL, 0, NULL, 0);
+}
+#endif
+
 /**
  *  @brief sets the configuration values
  *  @param ifnum        interface number
@@ -231,6 +243,11 @@ static int if_usb_probe(struct usb_interface *intf,
         cardp->priv->fw_ready = 1;
 
         priv->hw_host_to_card = if_usb_host_to_card;
+#ifdef CONFIG_OLPC
+        if (machine_is_olpc())
+                priv->reset_card = if_usb_reset_olpc_card;
+#endif
+
         cardp->boot2_version = udev->descriptor.bcdDevice;
 
         if_usb_submit_rx_urb(cardp);
@@ -364,6 +381,11 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
         ret = usb_reset_device(cardp->udev);
         msleep(100);
 
+#ifdef CONFIG_OLPC
+        if (ret && machine_is_olpc())
+                if_usb_reset_olpc_card(NULL);
+#endif
+
         lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
 
         return ret;
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 02e4fb639428..b7ab3590b586 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -11,6 +11,7 @@
 #include <linux/if_arp.h>
 #include <linux/kthread.h>
 #include <linux/kfifo.h>
+#include <linux/stddef.h>
 
 #include <net/iw_handler.h>
 #include <net/ieee80211.h>
@@ -343,14 +344,15 @@ static ssize_t lbs_mesh_set(struct device *dev,
 {
         struct lbs_private *priv = to_net_dev(dev)->priv;
         int enable;
-        int ret;
+        int ret, action = CMD_ACT_MESH_CONFIG_STOP;
 
         sscanf(buf, "%x", &enable);
         enable = !!enable;
         if (enable == !!priv->mesh_dev)
                 return count;
-
-        ret = lbs_mesh_config(priv, enable, priv->curbssparams.channel);
+        if (enable)
+                action = CMD_ACT_MESH_CONFIG_START;
+        ret = lbs_mesh_config(priv, action, priv->curbssparams.channel);
         if (ret)
                 return ret;
 
@@ -446,6 +448,8 @@ static int lbs_mesh_stop(struct net_device *dev)
 
         spin_unlock_irq(&priv->driver_lock);
 
+        schedule_work(&priv->mcast_work);
+
         lbs_deb_leave(LBS_DEB_MESH);
         return 0;
 }
@@ -467,6 +471,8 @@ static int lbs_eth_stop(struct net_device *dev)
         netif_stop_queue(dev);
         spin_unlock_irq(&priv->driver_lock);
 
+        schedule_work(&priv->mcast_work);
+
         lbs_deb_leave(LBS_DEB_NET);
         return 0;
 }
@@ -563,89 +569,116 @@ done:
         return ret;
 }
 
-static int lbs_copy_multicast_address(struct lbs_private *priv,
-                                     struct net_device *dev)
+
+static inline int mac_in_list(unsigned char *list, int list_len,
+                              unsigned char *mac)
 {
-        int i = 0;
-        struct dev_mc_list *mcptr = dev->mc_list;
+        while (list_len) {
+                if (!memcmp(list, mac, ETH_ALEN))
+                        return 1;
+                list += ETH_ALEN;
+                list_len--;
+        }
+        return 0;
+}
+
+
+static int lbs_add_mcast_addrs(struct cmd_ds_mac_multicast_adr *cmd,
+                               struct net_device *dev, int nr_addrs)
+{
+        int i = nr_addrs;
+        struct dev_mc_list *mc_list;
+        DECLARE_MAC_BUF(mac);
+
+        if ((dev->flags & (IFF_UP|IFF_MULTICAST)) != (IFF_UP|IFF_MULTICAST))
+                return nr_addrs;
+
+        netif_tx_lock_bh(dev);
+        for (mc_list = dev->mc_list; mc_list; mc_list = mc_list->next) {
+                if (mac_in_list(cmd->maclist, nr_addrs, mc_list->dmi_addr)) {
+                        lbs_deb_net("mcast address %s:%s skipped\n", dev->name,
+                                    print_mac(mac, mc_list->dmi_addr));
+                        continue;
+                }
 
-        for (i = 0; i < dev->mc_count; i++) {
-                memcpy(&priv->multicastlist[i], mcptr->dmi_addr, ETH_ALEN);
-                mcptr = mcptr->next;
+                if (i == MRVDRV_MAX_MULTICAST_LIST_SIZE)
+                        break;
+                memcpy(&cmd->maclist[6*i], mc_list->dmi_addr, ETH_ALEN);
+                lbs_deb_net("mcast address %s:%s added to filter\n", dev->name,
+                            print_mac(mac, mc_list->dmi_addr));
+                i++;
         }
+        netif_tx_unlock_bh(dev);
+        if (mc_list)
+                return -EOVERFLOW;
+
         return i;
 }
 
-static void lbs_set_multicast_list(struct net_device *dev)
+static void lbs_set_mcast_worker(struct work_struct *work)
 {
-        struct lbs_private *priv = dev->priv;
-        int old_mac_control;
-        DECLARE_MAC_BUF(mac);
+        struct lbs_private *priv = container_of(work, struct lbs_private, mcast_work);
+        struct cmd_ds_mac_multicast_adr mcast_cmd;
+        int dev_flags;
+        int nr_addrs;
+        int old_mac_control = priv->mac_control;
 
         lbs_deb_enter(LBS_DEB_NET);
 
-        old_mac_control = priv->mac_control;
-
-        if (dev->flags & IFF_PROMISC) {
-                lbs_deb_net("enable promiscuous mode\n");
-                priv->mac_control |=
-                    CMD_ACT_MAC_PROMISCUOUS_ENABLE;
-                priv->mac_control &=
-                    ~(CMD_ACT_MAC_ALL_MULTICAST_ENABLE |
-                      CMD_ACT_MAC_MULTICAST_ENABLE);
-        } else {
-                /* Multicast */
-                priv->mac_control &=
-                    ~CMD_ACT_MAC_PROMISCUOUS_ENABLE;
-
-                if (dev->flags & IFF_ALLMULTI || dev->mc_count >
-                    MRVDRV_MAX_MULTICAST_LIST_SIZE) {
-                        lbs_deb_net( "enabling all multicast\n");
-                        priv->mac_control |=
-                            CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
-                        priv->mac_control &=
-                            ~CMD_ACT_MAC_MULTICAST_ENABLE;
-                } else {
-                        priv->mac_control &=
-                            ~CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
-
-                        if (!dev->mc_count) {
-                                lbs_deb_net("no multicast addresses, "
-                                       "disabling multicast\n");
-                                priv->mac_control &=
-                                    ~CMD_ACT_MAC_MULTICAST_ENABLE;
-                        } else {
-                                int i;
-
-                                priv->mac_control |=
-                                    CMD_ACT_MAC_MULTICAST_ENABLE;
-
-                                priv->nr_of_multicastmacaddr =
-                                    lbs_copy_multicast_address(priv, dev);
-
-                                lbs_deb_net("multicast addresses: %d\n",
-                                       dev->mc_count);
-
-                                for (i = 0; i < dev->mc_count; i++) {
-                                        lbs_deb_net("Multicast address %d: %s\n",
-                                               i, print_mac(mac,
-                                               priv->multicastlist[i]));
-                                }
-                                /* send multicast addresses to firmware */
-                                lbs_prepare_and_send_command(priv,
-                                                      CMD_MAC_MULTICAST_ADR,
-                                                      CMD_ACT_SET, 0, 0,
-                                                      NULL);
-                        }
-                }
+        dev_flags = priv->dev->flags;
+        if (priv->mesh_dev)
+                dev_flags |= priv->mesh_dev->flags;
+
+        if (dev_flags & IFF_PROMISC) {
+                priv->mac_control |= CMD_ACT_MAC_PROMISCUOUS_ENABLE;
+                priv->mac_control &= ~(CMD_ACT_MAC_ALL_MULTICAST_ENABLE |
+                                       CMD_ACT_MAC_MULTICAST_ENABLE);
+                goto out_set_mac_control;
+        } else if (dev_flags & IFF_ALLMULTI) {
+        do_allmulti:
+                priv->mac_control |= CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
+                priv->mac_control &= ~(CMD_ACT_MAC_PROMISCUOUS_ENABLE |
+                                       CMD_ACT_MAC_MULTICAST_ENABLE);
+                goto out_set_mac_control;
         }
 
+        /* Once for priv->dev, again for priv->mesh_dev if it exists */
+        nr_addrs = lbs_add_mcast_addrs(&mcast_cmd, priv->dev, 0);
+        if (nr_addrs >= 0 && priv->mesh_dev)
+                nr_addrs = lbs_add_mcast_addrs(&mcast_cmd, priv->mesh_dev, nr_addrs);
+        if (nr_addrs < 0)
+                goto do_allmulti;
+
+        if (nr_addrs) {
+                int size = offsetof(struct cmd_ds_mac_multicast_adr,
+                                    maclist[6*nr_addrs]);
+
+                mcast_cmd.action = cpu_to_le16(CMD_ACT_SET);
+                mcast_cmd.hdr.size = cpu_to_le16(size);
+                mcast_cmd.nr_of_adrs = cpu_to_le16(nr_addrs);
+
+                lbs_cmd_async(priv, CMD_MAC_MULTICAST_ADR, &mcast_cmd.hdr, size);
+
+                priv->mac_control |= CMD_ACT_MAC_MULTICAST_ENABLE;
+        } else
+                priv->mac_control &= ~CMD_ACT_MAC_MULTICAST_ENABLE;
+
+        priv->mac_control &= ~(CMD_ACT_MAC_PROMISCUOUS_ENABLE |
+                               CMD_ACT_MAC_ALL_MULTICAST_ENABLE);
+ out_set_mac_control:
         if (priv->mac_control != old_mac_control)
                 lbs_set_mac_control(priv);
 
         lbs_deb_leave(LBS_DEB_NET);
 }
 
+static void lbs_set_multicast_list(struct net_device *dev)
+{
+        struct lbs_private *priv = dev->priv;
+
+        schedule_work(&priv->mcast_work);
+}
+
 /**
  *  @brief This function handles the major jobs in the LBS driver.
  *  It handles all events generated by firmware, RX data received
@@ -689,14 +722,14 @@ static int lbs_thread(void *data)
                         shouldsleep = 1;        /* Something is en route to the device already */
                 else if (priv->tx_pending_len > 0)
                         shouldsleep = 0;        /* We've a packet to send */
+                else if (priv->resp_len[priv->resp_idx])
+                        shouldsleep = 0;        /* We have a command response */
                 else if (priv->cur_cmd)
                         shouldsleep = 1;        /* Can't send a command; one already running */
                 else if (!list_empty(&priv->cmdpendingq))
                         shouldsleep = 0;        /* We have a command to send */
                 else if (__kfifo_len(priv->event_fifo))
                         shouldsleep = 0;        /* We have an event to process */
-                else if (priv->resp_len[priv->resp_idx])
-                        shouldsleep = 0;        /* We have a command response */
                 else
                         shouldsleep = 1;        /* No command */
 
@@ -749,16 +782,21 @@ static int lbs_thread(void *data)
                 if (priv->cmd_timed_out && priv->cur_cmd) {
                         struct cmd_ctrl_node *cmdnode = priv->cur_cmd;
 
-                        if (++priv->nr_retries > 10) {
-                                lbs_pr_info("Excessive timeouts submitting command %x\n",
-                                            le16_to_cpu(cmdnode->cmdbuf->command));
+                        if (++priv->nr_retries > 3) {
+                                lbs_pr_info("Excessive timeouts submitting "
+                                        "command 0x%04x\n",
+                                        le16_to_cpu(cmdnode->cmdbuf->command));
                                 lbs_complete_command(priv, cmdnode, -ETIMEDOUT);
                                 priv->nr_retries = 0;
+                                if (priv->reset_card)
+                                        priv->reset_card(priv);
                         } else {
                                 priv->cur_cmd = NULL;
                                 priv->dnld_sent = DNLD_RES_RECEIVED;
-                                lbs_pr_info("requeueing command %x due to timeout (#%d)\n",
-                                            le16_to_cpu(cmdnode->cmdbuf->command), priv->nr_retries);
+                                lbs_pr_info("requeueing command 0x%04x due "
+                                        "to timeout (#%d)\n",
+                                        le16_to_cpu(cmdnode->cmdbuf->command),
+                                        priv->nr_retries);
 
                                 /* Stick it back at the _top_ of the pending queue
                                    for immediate resubmission */
@@ -949,12 +987,11 @@ static void command_timer_fn(unsigned long data)
         lbs_deb_enter(LBS_DEB_CMD);
         spin_lock_irqsave(&priv->driver_lock, flags);
 
-        if (!priv->cur_cmd) {
-                lbs_pr_info("Command timer expired; no pending command\n");
+        if (!priv->cur_cmd)
                 goto out;
-        }
 
-        lbs_pr_info("Command %x timed out\n", le16_to_cpu(priv->cur_cmd->cmdbuf->command));
+        lbs_pr_info("command 0x%04x timed out\n",
+                le16_to_cpu(priv->cur_cmd->cmdbuf->command));
 
         priv->cmd_timed_out = 1;
         wake_up_interruptible(&priv->waitq);
@@ -1008,7 +1045,7 @@ static int lbs_init_adapter(struct lbs_private *priv)
         priv->curbssparams.channel = DEFAULT_AD_HOC_CHANNEL;
         priv->mac_control = CMD_ACT_MAC_RX_ON | CMD_ACT_MAC_TX_ON;
         priv->radioon = RADIO_ON;
-        priv->auto_rate = 1;
+        priv->enablehwauto = 1;
         priv->capability = WLAN_CAPABILITY_SHORT_PREAMBLE;
         priv->psmode = LBS802_11POWERMODECAM;
         priv->psstate = PS_STATE_FULL_POWER;
@@ -1123,6 +1160,7 @@ struct lbs_private *lbs_add_card(void *card, struct device *dmdev)
         priv->work_thread = create_singlethread_workqueue("lbs_worker");
         INIT_DELAYED_WORK(&priv->assoc_work, lbs_association_worker);
         INIT_DELAYED_WORK(&priv->scan_work, lbs_scan_worker);
+        INIT_WORK(&priv->mcast_work, lbs_set_mcast_worker);
         INIT_WORK(&priv->sync_channel, lbs_sync_channel_worker);
 
         sprintf(priv->mesh_ssid, "mesh");
@@ -1159,6 +1197,7 @@ void lbs_remove_card(struct lbs_private *priv)
 
         cancel_delayed_work_sync(&priv->scan_work);
         cancel_delayed_work_sync(&priv->assoc_work);
+        cancel_work_sync(&priv->mcast_work);
         destroy_workqueue(priv->work_thread);
 
         if (priv->psmode == LBS802_11POWERMODEMAX_PSP) {
@@ -1224,9 +1263,11 @@ int lbs_start_card(struct lbs_private *priv)
                    useful */
 
                 priv->mesh_tlv = 0x100 + 291;
-                if (lbs_mesh_config(priv, 1, priv->curbssparams.channel)) {
+                if (lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+                                    priv->curbssparams.channel)) {
                         priv->mesh_tlv = 0x100 + 37;
-                        if (lbs_mesh_config(priv, 1, priv->curbssparams.channel))
+                        if (lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+                                            priv->curbssparams.channel))
                                 priv->mesh_tlv = 0;
                 }
                 if (priv->mesh_tlv) {
@@ -1266,8 +1307,9 @@ void lbs_stop_card(struct lbs_private *priv)
 
         lbs_debugfs_remove_one(priv);
         device_remove_file(&dev->dev, &dev_attr_lbs_rtap);
-        if (priv->mesh_tlv)
+        if (priv->mesh_tlv) {
                 device_remove_file(&dev->dev, &dev_attr_lbs_mesh);
+        }
 
         /* Flush pending command nodes */
         del_timer_sync(&priv->command_timer);
@@ -1323,6 +1365,8 @@ static int lbs_add_mesh(struct lbs_private *priv)
 #ifdef  WIRELESS_EXT
         mesh_dev->wireless_handlers = (struct iw_handler_def *)&mesh_handler_def;
 #endif
+        mesh_dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
+        mesh_dev->set_multicast_list = lbs_set_multicast_list;
         /* Register virtual mesh interface */
         ret = register_netdev(mesh_dev);
         if (ret) {
@@ -1334,6 +1378,8 @@ static int lbs_add_mesh(struct lbs_private *priv)
         if (ret)
                 goto err_unregister;
 
+        lbs_persist_config_init(mesh_dev);
+
         /* Everything successful */
         ret = 0;
         goto done;
@@ -1360,8 +1406,9 @@ static void lbs_remove_mesh(struct lbs_private *priv)
 
         lbs_deb_enter(LBS_DEB_MESH);
         netif_stop_queue(mesh_dev);
-        netif_carrier_off(priv->mesh_dev);
+        netif_carrier_off(mesh_dev);
         sysfs_remove_group(&(mesh_dev->dev.kobj), &lbs_mesh_attr_group);
+        lbs_persist_config_remove(mesh_dev);
         unregister_netdev(mesh_dev);
         priv->mesh_dev = NULL;
         free_netdev(mesh_dev);
@@ -1555,7 +1602,6 @@ static int lbs_add_rtap(struct lbs_private *priv)
         rtap_dev->stop = lbs_rtap_stop;
         rtap_dev->get_stats = lbs_rtap_get_stats;
         rtap_dev->hard_start_xmit = lbs_rtap_hard_start_xmit;
-        rtap_dev->set_multicast_list = lbs_set_multicast_list;
         rtap_dev->priv = priv;
         SET_NETDEV_DEV(rtap_dev, priv->dev->dev.parent);
 
diff --git a/drivers/net/wireless/libertas/persistcfg.c b/drivers/net/wireless/libertas/persistcfg.c
new file mode 100644
index 000000000000..6d0ff8decaf7
--- /dev/null
+++ b/drivers/net/wireless/libertas/persistcfg.c
@@ -0,0 +1,453 @@
+#include <linux/moduleparam.h>
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/kthread.h>
+#include <linux/kfifo.h>
+
+#include "host.h"
+#include "decl.h"
+#include "dev.h"
+#include "wext.h"
+#include "debugfs.h"
+#include "scan.h"
+#include "assoc.h"
+#include "cmd.h"
+
+static int mesh_get_default_parameters(struct device *dev,
+                                       struct mrvl_mesh_defaults *defs)
+{
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        struct cmd_ds_mesh_config cmd;
+        int ret;
+
+        memset(&cmd, 0, sizeof(struct cmd_ds_mesh_config));
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_GET,
+                                   CMD_TYPE_MESH_GET_DEFAULTS);
+
+        if (ret)
+                return -EOPNOTSUPP;
+
+        memcpy(defs, &cmd.data[0], sizeof(struct mrvl_mesh_defaults));
+
+        return 0;
+}
+
+/**
+ * @brief Get function for sysfs attribute bootflag
+ */
+static ssize_t bootflag_get(struct device *dev,
+                            struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 12, "0x%x\n", le32_to_cpu(defs.bootflag));
+}
+
+/**
+ * @brief Set function for sysfs attribute bootflag
+ */
+static ssize_t bootflag_set(struct device *dev, struct device_attribute *attr,
+                            const char *buf, size_t count)
+{
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        struct cmd_ds_mesh_config cmd;
+        uint32_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%x", &datum);
+        if (ret != 1)
+                return -EINVAL;
+
+        *((__le32 *)&cmd.data[0]) = cpu_to_le32(!!datum);
+        cmd.length = cpu_to_le16(sizeof(uint32_t));
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_BOOTFLAG);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute boottime
+ */
+static ssize_t boottime_get(struct device *dev,
+                            struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 12, "0x%x\n", defs.boottime);
+}
+
+/**
+ * @brief Set function for sysfs attribute boottime
+ */
+static ssize_t boottime_set(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        struct cmd_ds_mesh_config cmd;
+        uint32_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%x", &datum);
+        if (ret != 1)
+                return -EINVAL;
+
+        /* A too small boot time will result in the device booting into
+         * standalone (no-host) mode before the host can take control of it,
+         * so the change will be hard to revert.  This may be a desired
+         * feature (e.g to configure a very fast boot time for devices that
+         * will not be attached to a host), but dangerous.  So I'm enforcing a
+         * lower limit of 20 seconds:  remove and recompile the driver if this
+         * does not work for you.
+         */
+        datum = (datum < 20) ? 20 : datum;
+        cmd.data[0] = datum;
+        cmd.length = cpu_to_le16(sizeof(uint8_t));
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_BOOTTIME);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute channel
+ */
+static ssize_t channel_get(struct device *dev,
+                           struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 12, "0x%x\n", le16_to_cpu(defs.channel));
+}
+
+/**
+ * @brief Set function for sysfs attribute channel
+ */
+static ssize_t channel_set(struct device *dev, struct device_attribute *attr,
+                           const char *buf, size_t count)
+{
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        struct cmd_ds_mesh_config cmd;
+        uint16_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%hx", &datum);
+        if (ret != 1 || datum < 1 || datum > 11)
+                return -EINVAL;
+
+        *((__le16 *)&cmd.data[0]) = cpu_to_le16(datum);
+        cmd.length = cpu_to_le16(sizeof(uint16_t));
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_DEF_CHANNEL);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute mesh_id
+ */
+static ssize_t mesh_id_get(struct device *dev, struct device_attribute *attr,
+                           char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int maxlen;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        if (defs.meshie.val.mesh_id_len > IW_ESSID_MAX_SIZE) {
+                lbs_pr_err("inconsistent mesh ID length");
+                defs.meshie.val.mesh_id_len = IW_ESSID_MAX_SIZE;
+        }
+
+        /* SSID not null terminated: reserve room for \0 + \n */
+        maxlen = defs.meshie.val.mesh_id_len + 2;
+        maxlen = (PAGE_SIZE > maxlen) ? maxlen : PAGE_SIZE;
+
+        defs.meshie.val.mesh_id[defs.meshie.val.mesh_id_len] = '\0';
+
+        return snprintf(buf, maxlen, "%s\n", defs.meshie.val.mesh_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute mesh_id
+ */
+static ssize_t mesh_id_set(struct device *dev, struct device_attribute *attr,
+                           const char *buf, size_t count)
+{
+        struct cmd_ds_mesh_config cmd;
+        struct mrvl_mesh_defaults defs;
+        struct mrvl_meshie *ie;
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        int len;
+        int ret;
+
+        if (count < 2 || count > IW_ESSID_MAX_SIZE + 1)
+                return -EINVAL;
+
+        memset(&cmd, 0, sizeof(struct cmd_ds_mesh_config));
+        ie = (struct mrvl_meshie *) &cmd.data[0];
+
+        /* fetch all other Information Element parameters */
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+        /* transfer IE elements */
+        memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+
+        len = count - 1;
+        memcpy(ie->val.mesh_id, buf, len);
+        /* SSID len */
+        ie->val.mesh_id_len = len;
+        /* IE len */
+        ie->hdr.len = sizeof(struct mrvl_meshie_val) - IW_ESSID_MAX_SIZE + len;
+
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_MESH_IE);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute protocol_id
+ */
+static ssize_t protocol_id_get(struct device *dev,
+                               struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 5, "%d\n", defs.meshie.val.active_protocol_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute protocol_id
+ */
+static ssize_t protocol_id_set(struct device *dev,
+                struct device_attribute *attr, const char *buf, size_t count)
+{
+        struct cmd_ds_mesh_config cmd;
+        struct mrvl_mesh_defaults defs;
+        struct mrvl_meshie *ie;
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        uint32_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%x", &datum);
+        if (ret != 1)
+                return -EINVAL;
+
+        /* fetch all other Information Element parameters */
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+        /* transfer IE elements */
+        ie = (struct mrvl_meshie *) &cmd.data[0];
+        memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+        /* update protocol id */
+        ie->val.active_protocol_id = datum;
+
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_MESH_IE);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute metric_id
+ */
+static ssize_t metric_id_get(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 5, "%d\n", defs.meshie.val.active_metric_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute metric_id
+ */
+static ssize_t metric_id_set(struct device *dev, struct device_attribute *attr,
+                             const char *buf, size_t count)
+{
+        struct cmd_ds_mesh_config cmd;
+        struct mrvl_mesh_defaults defs;
+        struct mrvl_meshie *ie;
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        uint32_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%x", &datum);
+        if (ret != 1)
+                return -EINVAL;
+
+        /* fetch all other Information Element parameters */
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+        /* transfer IE elements */
+        ie = (struct mrvl_meshie *) &cmd.data[0];
+        memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+        /* update metric id */
+        ie->val.active_metric_id = datum;
+
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_MESH_IE);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute capability
+ */
+static ssize_t capability_get(struct device *dev,
+                struct device_attribute *attr, char *buf)
+{
+        struct mrvl_mesh_defaults defs;
+        int ret;
+
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        if (ret)
+                return ret;
+
+        return snprintf(buf, 5, "%d\n", defs.meshie.val.mesh_capability);
+}
+
+/**
+ * @brief Set function for sysfs attribute capability
+ */
+static ssize_t capability_set(struct device *dev, struct device_attribute *attr,
+                              const char *buf, size_t count)
+{
+        struct cmd_ds_mesh_config cmd;
+        struct mrvl_mesh_defaults defs;
+        struct mrvl_meshie *ie;
+        struct lbs_private *priv = to_net_dev(dev)->priv;
+        uint32_t datum;
+        int ret;
+
+        memset(&cmd, 0, sizeof(cmd));
+        ret = sscanf(buf, "%x", &datum);
+        if (ret != 1)
+                return -EINVAL;
+
+        /* fetch all other Information Element parameters */
+        ret = mesh_get_default_parameters(dev, &defs);
+
+        cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+        /* transfer IE elements */
+        ie = (struct mrvl_meshie *) &cmd.data[0];
+        memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+        /* update value */
+        ie->val.mesh_capability = datum;
+
+        ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+                                   CMD_TYPE_MESH_SET_MESH_IE);
+        if (ret)
+                return ret;
+
+        return strlen(buf);
+}
+
+
+static DEVICE_ATTR(bootflag, 0644, bootflag_get, bootflag_set);
+static DEVICE_ATTR(boottime, 0644, boottime_get, boottime_set);
+static DEVICE_ATTR(channel, 0644, channel_get, channel_set);
+static DEVICE_ATTR(mesh_id, 0644, mesh_id_get, mesh_id_set);
+static DEVICE_ATTR(protocol_id, 0644, protocol_id_get, protocol_id_set);
+static DEVICE_ATTR(metric_id, 0644, metric_id_get, metric_id_set);
+static DEVICE_ATTR(capability, 0644, capability_get, capability_set);
+
+static struct attribute *boot_opts_attrs[] = {
+        &dev_attr_bootflag.attr,
+        &dev_attr_boottime.attr,
+        &dev_attr_channel.attr,
+        NULL
+};
+
+static struct attribute_group boot_opts_group = {
+        .name = "boot_options",
+        .attrs = boot_opts_attrs,
+};
+
+static struct attribute *mesh_ie_attrs[] = {
+        &dev_attr_mesh_id.attr,
+        &dev_attr_protocol_id.attr,
+        &dev_attr_metric_id.attr,
+        &dev_attr_capability.attr,
+        NULL
+};
+
+static struct attribute_group mesh_ie_group = {
+        .name = "mesh_ie",
+        .attrs = mesh_ie_attrs,
+};
+
+void lbs_persist_config_init(struct net_device *dev)
+{
+        int ret;
+        ret = sysfs_create_group(&(dev->dev.kobj), &boot_opts_group);
+        ret = sysfs_create_group(&(dev->dev.kobj), &mesh_ie_group);
+}
+
+void lbs_persist_config_remove(struct net_device *dev)
+{
+        sysfs_remove_group(&(dev->dev.kobj), &boot_opts_group);
+        sysfs_remove_group(&(dev->dev.kobj), &mesh_ie_group);
+}
diff --git a/drivers/net/wireless/libertas/rx.c b/drivers/net/wireless/libertas/rx.c
index 05af7316f698..5749f22b296f 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/libertas/rx.c
@@ -237,7 +237,7 @@ int lbs_process_rxed_packet(struct lbs_private *priv, struct sk_buff *skb)
         /* Take the data rate from the rxpd structure
          * only if the rate is auto
          */
-        if (priv->auto_rate)
+        if (priv->enablehwauto)
                 priv->cur_rate = lbs_fw_index_to_data_rate(p_rx_pd->rx_rate);
 
         lbs_compute_rssi(priv, p_rx_pd);
@@ -383,7 +383,7 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
         /* Take the data rate from the rxpd structure
          * only if the rate is auto
          */
-        if (priv->auto_rate)
+        if (priv->enablehwauto)
                 priv->cur_rate = lbs_fw_index_to_data_rate(prxpd->rx_rate);
 
         lbs_compute_rssi(priv, prxpd);
diff --git a/drivers/net/wireless/libertas/types.h b/drivers/net/wireless/libertas/types.h
index 4031be420862..e0c2599da92f 100644
--- a/drivers/net/wireless/libertas/types.h
+++ b/drivers/net/wireless/libertas/types.h
@@ -6,6 +6,8 @@
 
 #include <linux/if_ether.h>
 #include <asm/byteorder.h>
+#include <linux/wireless.h>
+#include <net/ieee80211.h>
 
 struct ieeetypes_cfparamset {
         u8 elementid;
@@ -252,4 +254,32 @@ struct mrvlietypes_ledbhv {
         struct led_bhv ledbhv[1];
 } __attribute__ ((packed));
 
+/* Meant to be packed as the value member of a struct ieee80211_info_element.
+ * Note that the len member of the ieee80211_info_element varies depending on
+ * the mesh_id_len */
+struct mrvl_meshie_val {
+        uint8_t oui[P80211_OUI_LEN];
+        uint8_t type;
+        uint8_t subtype;
+        uint8_t version;
+        uint8_t active_protocol_id;
+        uint8_t active_metric_id;
+        uint8_t mesh_capability;
+        uint8_t mesh_id_len;
+        uint8_t mesh_id[IW_ESSID_MAX_SIZE];
+} __attribute__ ((packed));
+
+struct mrvl_meshie {
+        struct ieee80211_info_element hdr;
+        struct mrvl_meshie_val val;
+} __attribute__ ((packed));
+
+struct mrvl_mesh_defaults {
+        __le32 bootflag;
+        uint8_t boottime;
+        uint8_t reserved;
+        __le16 channel;
+        struct mrvl_meshie meshie;
+} __attribute__ ((packed));
+
 #endif
diff --git a/drivers/net/wireless/libertas/wext.c b/drivers/net/wireless/libertas/wext.c
index 0973d015a520..8b3ed77860b3 100644
--- a/drivers/net/wireless/libertas/wext.c
+++ b/drivers/net/wireless/libertas/wext.c
@@ -1002,7 +1002,7 @@ static int lbs_mesh_set_freq(struct net_device *dev,
                 else if (priv->mode == IW_MODE_ADHOC)
                         lbs_stop_adhoc_network(priv);
         }
-        lbs_mesh_config(priv, 1, fwrq->m);
+        lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START, fwrq->m);
         lbs_update_channel(priv);
         ret = 0;
 
@@ -1021,29 +1021,38 @@ static int lbs_set_rate(struct net_device *dev, struct iw_request_info *info,
 
         lbs_deb_enter(LBS_DEB_WEXT);
         lbs_deb_wext("vwrq->value %d\n", vwrq->value);
+        lbs_deb_wext("vwrq->fixed %d\n", vwrq->fixed);
+
+        if (vwrq->fixed && vwrq->value == -1)
+                goto out;
 
         /* Auto rate? */
-        if (vwrq->value == -1) {
-                priv->auto_rate = 1;
+        priv->enablehwauto = !vwrq->fixed;
+
+        if (vwrq->value == -1)
                 priv->cur_rate = 0;
-        } else {
+        else {
                 if (vwrq->value % 100000)
                         goto out;
 
+                new_rate = vwrq->value / 500000;
+                priv->cur_rate = new_rate;
+                /* the rest is only needed for lbs_set_data_rate() */
                 memset(rates, 0, sizeof(rates));
                 copy_active_data_rates(priv, rates);
-                new_rate = vwrq->value / 500000;
                 if (!memchr(rates, new_rate, sizeof(rates))) {
                         lbs_pr_alert("fixed data rate 0x%X out of range\n",
                                 new_rate);
                         goto out;
                 }
-
-                priv->cur_rate = new_rate;
-                priv->auto_rate = 0;
         }
 
-        ret = lbs_set_data_rate(priv, new_rate);
+        /* Try the newer command first (Firmware Spec 5.1 and above) */
+        ret = lbs_cmd_802_11_rate_adapt_rateset(priv, CMD_ACT_SET);
+
+        /* Fallback to older version */
+        if (ret)
+                ret = lbs_set_data_rate(priv, new_rate);
 
 out:
         lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -1060,7 +1069,7 @@ static int lbs_get_rate(struct net_device *dev, struct iw_request_info *info,
         if (priv->connect_status == LBS_CONNECTED) {
                 vwrq->value = priv->cur_rate * 500000;
 
-                if (priv->auto_rate)
+                if (priv->enablehwauto)
                         vwrq->fixed = 0;
                 else
                         vwrq->fixed = 1;
@@ -2011,7 +2020,8 @@ static int lbs_mesh_set_essid(struct net_device *dev,
                 priv->mesh_ssid_len = dwrq->length;
         }
 
-        lbs_mesh_config(priv, 1, priv->curbssparams.channel);
+        lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+                        priv->curbssparams.channel);
  out:
         lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
         return ret;
diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 9cbef5bce0f6..9f7224de6fd1 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -375,9 +375,6 @@ static void inline p54_wake_free_queues(struct ieee80211_hw *dev)
         struct p54_common *priv = dev->priv;
         int i;
 
-        /* ieee80211_start_queues is great if all queues are really empty.
-         * But, what if some are full? */
-
         for (i = 0; i < dev->queues; i++)
                 if (priv->tx_stats[i].len < priv->tx_stats[i].limit)
                         ieee80211_wake_queue(dev, i);
@@ -395,44 +392,42 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
         u32 last_addr = priv->rx_start;
 
         while (entry != (struct sk_buff *)&priv->tx_queue) {
-                range = (struct memrecord *)&entry->cb;
+                struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
+                range = (void *)info->driver_data;
                 if (range->start_addr == addr) {
-                        struct ieee80211_tx_status status;
                         struct p54_control_hdr *entry_hdr;
                         struct p54_tx_control_allocdata *entry_data;
                         int pad = 0;
 
-                        if (entry->next != (struct sk_buff *)&priv->tx_queue)
-                                freed = ((struct memrecord *)&entry->next->cb)->start_addr - last_addr;
-                        else
+                        if (entry->next != (struct sk_buff *)&priv->tx_queue) {
+                                struct ieee80211_tx_info *ni;
+                                struct memrecord *mr;
+
+                                ni = IEEE80211_SKB_CB(entry->next);
+                                mr = (struct memrecord *)ni->driver_data;
+                                freed = mr->start_addr - last_addr;
+                        } else
                                 freed = priv->rx_end - last_addr;
 
                         last_addr = range->end_addr;
                         __skb_unlink(entry, &priv->tx_queue);
-                        if (!range->control) {
-                                kfree_skb(entry);
-                                break;
-                        }
-                        memset(&status, 0, sizeof(status));
-                        memcpy(&status.control, range->control,
-                               sizeof(status.control));
-                        kfree(range->control);
-                        priv->tx_stats[status.control.queue].len--;
+                        memset(&info->status, 0, sizeof(info->status));
+                        priv->tx_stats[skb_get_queue_mapping(skb)].len--;
                         entry_hdr = (struct p54_control_hdr *) entry->data;
                         entry_data = (struct p54_tx_control_allocdata *) entry_hdr->data;
                         if ((entry_hdr->magic1 & cpu_to_le16(0x4000)) != 0)
                                 pad = entry_data->align[0];
 
-                        if (!(status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
+                        if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                                 if (!(payload->status & 0x01))
-                                        status.flags |= IEEE80211_TX_STATUS_ACK;
+                                        info->flags |= IEEE80211_TX_STAT_ACK;
                                 else
-                                        status.excessive_retries = 1;
+                                        info->status.excessive_retries = 1;
                         }
-                        status.retry_count = payload->retries - 1;
-                        status.ack_signal = le16_to_cpu(payload->ack_rssi);
+                        info->status.retry_count = payload->retries - 1;
+                        info->status.ack_signal = le16_to_cpu(payload->ack_rssi);
                         skb_pull(entry, sizeof(*hdr) + pad + sizeof(*entry_data));
-                        ieee80211_tx_status_irqsafe(dev, entry, &status);
+                        ieee80211_tx_status_irqsafe(dev, entry);
                         break;
                 } else
                         last_addr = range->end_addr;
@@ -497,13 +492,11 @@ EXPORT_SYMBOL_GPL(p54_rx);
  * allocated areas.
  */
 static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
-                               struct p54_control_hdr *data, u32 len,
-                               struct ieee80211_tx_control *control)
+                               struct p54_control_hdr *data, u32 len)
 {
         struct p54_common *priv = dev->priv;
         struct sk_buff *entry = priv->tx_queue.next;
         struct sk_buff *target_skb = NULL;
-        struct memrecord *range;
         u32 last_addr = priv->rx_start;
         u32 largest_hole = 0;
         u32 target_addr = priv->rx_start;
@@ -515,7 +508,8 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
         left = skb_queue_len(&priv->tx_queue);
         while (left--) {
                 u32 hole_size;
-                range = (struct memrecord *)&entry->cb;
+                struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
+                struct memrecord *range = (void *)info->driver_data;
                 hole_size = range->start_addr - last_addr;
                 if (!target_skb && hole_size >= len) {
                         target_skb = entry->prev;
@@ -530,17 +524,18 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
                 target_skb = priv->tx_queue.prev;
                 largest_hole = max(largest_hole, priv->rx_end - last_addr - len);
                 if (!skb_queue_empty(&priv->tx_queue)) {
-                        range = (struct memrecord *)&target_skb->cb;
+                        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(target_skb);
+                        struct memrecord *range = (void *)info->driver_data;
                         target_addr = range->end_addr;
                 }
         } else
                 largest_hole = max(largest_hole, priv->rx_end - last_addr);
 
         if (skb) {
-                range = (struct memrecord *)&skb->cb;
+                struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+                struct memrecord *range = (void *)info->driver_data;
                 range->start_addr = target_addr;
                 range->end_addr = target_addr + len;
-                range->control = control;
                 __skb_queue_after(&priv->tx_queue, target_skb, skb);
                 if (largest_hole < IEEE80211_MAX_RTS_THRESHOLD + 0x170 +
                                    sizeof(struct p54_control_hdr))
@@ -551,32 +546,27 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
         data->req_id = cpu_to_le32(target_addr + 0x70);
 }
 
-static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-                  struct ieee80211_tx_control *control)
+static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct ieee80211_tx_queue_stats *current_queue;
         struct p54_common *priv = dev->priv;
         struct p54_control_hdr *hdr;
         struct p54_tx_control_allocdata *txhdr;
-        struct ieee80211_tx_control *control_copy;
         size_t padding, len;
         u8 rate;
 
-        current_queue = &priv->tx_stats[control->queue];
+        current_queue = &priv->tx_stats[skb_get_queue_mapping(skb)];
         if (unlikely(current_queue->len > current_queue->limit))
                 return NETDEV_TX_BUSY;
         current_queue->len++;
         current_queue->count++;
         if (current_queue->len == current_queue->limit)
-                ieee80211_stop_queue(dev, control->queue);
+                ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
 
         padding = (unsigned long)(skb->data - (sizeof(*hdr) + sizeof(*txhdr))) & 3;
         len = skb->len;
 
-        control_copy = kmalloc(sizeof(*control), GFP_ATOMIC);
-        if (control_copy)
-                memcpy(control_copy, control, sizeof(*control));
-
         txhdr = (struct p54_tx_control_allocdata *)
                         skb_push(skb, sizeof(*txhdr) + padding);
         hdr = (struct p54_control_hdr *) skb_push(skb, sizeof(*hdr));
@@ -586,35 +576,37 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
         else
                 hdr->magic1 = cpu_to_le16(0x0010);
         hdr->len = cpu_to_le16(len);
-        hdr->type = (control->flags & IEEE80211_TXCTL_NO_ACK) ? 0 : cpu_to_le16(1);
-        hdr->retry1 = hdr->retry2 = control->retry_limit;
-        p54_assign_address(dev, skb, hdr, skb->len, control_copy);
+        hdr->type = (info->flags & IEEE80211_TX_CTL_NO_ACK) ? 0 : cpu_to_le16(1);
+        hdr->retry1 = hdr->retry2 = info->control.retry_limit;
 
         memset(txhdr->wep_key, 0x0, 16);
         txhdr->padding = 0;
         txhdr->padding2 = 0;
 
         /* TODO: add support for alternate retry TX rates */
-        rate = control->tx_rate->hw_value;
-        if (control->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
+        rate = ieee80211_get_tx_rate(dev, info)->hw_value;
+        if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
                 rate |= 0x10;
-        if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
                 rate |= 0x40;
-        else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+        else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
                 rate |= 0x20;
         memset(txhdr->rateset, rate, 8);
         txhdr->wep_key_present = 0;
         txhdr->wep_key_len = 0;
-        txhdr->frame_type = cpu_to_le32(control->queue + 4);
+        txhdr->frame_type = cpu_to_le32(skb_get_queue_mapping(skb) + 4);
         txhdr->magic4 = 0;
-        txhdr->antenna = (control->antenna_sel_tx == 0) ?
-                2 : control->antenna_sel_tx - 1;
+        txhdr->antenna = (info->antenna_sel_tx == 0) ?
+                2 : info->antenna_sel_tx - 1;
         txhdr->output_power = 0x7f; // HW Maximum
-        txhdr->magic5 = (control->flags & IEEE80211_TXCTL_NO_ACK) ?
+        txhdr->magic5 = (info->flags & IEEE80211_TX_CTL_NO_ACK) ?
                 0 : ((rate > 0x3) ? cpu_to_le32(0x33) : cpu_to_le32(0x23));
         if (padding)
                 txhdr->align[0] = padding;
 
+        /* modifies skb->cb and with it info, so must be last! */
+        p54_assign_address(dev, skb, hdr, skb->len);
+
         priv->tx(dev, hdr, skb->len, 0);
         return 0;
 }
@@ -637,7 +629,7 @@ static int p54_set_filter(struct ieee80211_hw *dev, u16 filter_type,
         filter = (struct p54_tx_control_filter *) hdr->data;
         hdr->magic1 = cpu_to_le16(0x8001);
         hdr->len = cpu_to_le16(sizeof(*filter));
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*filter), NULL);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*filter));
         hdr->type = cpu_to_le16(P54_CONTROL_TYPE_FILTER_SET);
 
         filter->filter_type = cpu_to_le16(filter_type);
@@ -681,7 +673,7 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
         hdr->magic1 = cpu_to_le16(0x8001);
         hdr->len = cpu_to_le16(sizeof(*chan));
         hdr->type = cpu_to_le16(P54_CONTROL_TYPE_CHANNEL_CHANGE);
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len, NULL);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len);
 
         chan->magic1 = cpu_to_le16(0x1);
         chan->magic2 = cpu_to_le16(0x0);
@@ -754,7 +746,7 @@ static int p54_set_leds(struct ieee80211_hw *dev, int mode, int link, int act)
         hdr->magic1 = cpu_to_le16(0x8001);
         hdr->len = cpu_to_le16(sizeof(*led));
         hdr->type = cpu_to_le16(P54_CONTROL_TYPE_LED);
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*led), NULL);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*led));
 
         led = (struct p54_tx_control_led *) hdr->data;
         led->mode = cpu_to_le16(mode);
@@ -804,7 +796,7 @@ static void p54_set_vdcf(struct ieee80211_hw *dev)
 
         hdr = (void *)priv->cached_vdcf + priv->tx_hdr_len;
 
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*vdcf), NULL);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*vdcf));
 
         vdcf = (struct p54_tx_control_vdcf *) hdr->data;
 
@@ -840,12 +832,8 @@ static void p54_stop(struct ieee80211_hw *dev)
 {
         struct p54_common *priv = dev->priv;
         struct sk_buff *skb;
-        while ((skb = skb_dequeue(&priv->tx_queue))) {
-                struct memrecord *range = (struct memrecord *)&skb->cb;
-                if (range->control)
-                        kfree(range->control);
+        while ((skb = skb_dequeue(&priv->tx_queue)))
                 kfree_skb(skb);
-        }
         priv->stop(dev);
         priv->mode = IEEE80211_IF_TYPE_INVALID;
 }
diff --git a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
index c15b56e1d75e..2245fcce92dc 100644
--- a/drivers/net/wireless/p54/p54common.h
+++ b/drivers/net/wireless/p54/p54common.h
@@ -152,7 +152,6 @@ struct pda_pa_curve_data {
 struct memrecord {
         u32 start_addr;
         u32 end_addr;
-        struct ieee80211_tx_control *control;
 };
 
 struct p54_eeprom_lm86 {
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index fa527723fbe0..7dd4add4bf4e 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -665,7 +665,7 @@ static int p54p_resume(struct pci_dev *pdev)
 
         if (priv->common.mode != IEEE80211_IF_TYPE_INVALID) {
                 p54p_open(dev);
-                ieee80211_start_queues(dev);
+                ieee80211_wake_queues(dev);
         }
 
         return 0;
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index d0b1fb15c709..ed310f84f28b 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -1108,7 +1108,7 @@ static int rndis_iw_get_range(struct net_device *dev,
         /* fill in 802.11g rates */
         if (has_80211g_rates) {
                 num = range->num_bitrates;
-                for (i = 0; i < sizeof(rates_80211g); i++) {
+                for (i = 0; i < ARRAY_SIZE(rates_80211g); i++) {
                         for (j = 0; j < num; j++) {
                                 if (range->bitrate[j] ==
                                         rates_80211g[i] * 1000000)
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index afa565c63621..900140d3b304 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -620,48 +620,38 @@ static void rt2400pci_link_tuner(struct rt2x00_dev *rt2x00dev)
 static void rt2400pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
                                    struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_rx->desc, 2, &word);
+        rt2x00_desc_read(entry_priv->desc, 2, &word);
         rt2x00_set_field32(&word, RXD_W2_BUFFER_LENGTH,
                            entry->queue->data_size);
-        rt2x00_desc_write(priv_rx->desc, 2, word);
+        rt2x00_desc_write(entry_priv->desc, 2, word);
 
-        rt2x00_desc_read(priv_rx->desc, 1, &word);
-        rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, priv_rx->data_dma);
-        rt2x00_desc_write(priv_rx->desc, 1, word);
+        rt2x00_desc_read(entry_priv->desc, 1, &word);
+        rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, entry_priv->data_dma);
+        rt2x00_desc_write(entry_priv->desc, 1, word);
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-        rt2x00_desc_write(priv_rx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt2400pci_init_txentry(struct rt2x00_dev *rt2x00dev,
                                    struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_tx->desc, 1, &word);
-        rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, priv_tx->data_dma);
-        rt2x00_desc_write(priv_tx->desc, 1, word);
-
-        rt2x00_desc_read(priv_tx->desc, 2, &word);
-        rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH,
-                           entry->queue->data_size);
-        rt2x00_desc_write(priv_tx->desc, 2, word);
-
-        rt2x00_desc_read(priv_tx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, TXD_W0_VALID, 0);
         rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-        rt2x00_desc_write(priv_tx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-        struct queue_entry_priv_pci_rx *priv_rx;
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         u32 reg;
 
         /*
@@ -674,28 +664,28 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
         rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
-        priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
         rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
 
-        priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
         rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-        priv_tx = rt2x00dev->bcn[1].entries[0].priv_data;
+        entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
         rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-        priv_tx = rt2x00dev->bcn[0].entries[0].priv_data;
+        entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
         rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
 
         rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
@@ -703,9 +693,10 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
         rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
 
-        priv_rx = rt2x00dev->rx->entries[0].priv_data;
+        entry_priv = rt2x00dev->rx->entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
-        rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER, priv_rx->desc_dma);
+        rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
 
         return 0;
@@ -1001,17 +992,22 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                     struct sk_buff *skb,
-                                    struct txentry_desc *txdesc,
-                                    struct ieee80211_tx_control *control)
+                                    struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
         __le32 *txd = skbdesc->desc;
         u32 word;
 
         /*
          * Start writing the descriptor words.
          */
+        rt2x00_desc_read(entry_priv->desc, 1, &word);
+        rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, entry_priv->data_dma);
+        rt2x00_desc_write(entry_priv->desc, 1, word);
+
         rt2x00_desc_read(txd, 2, &word);
+        rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH, skbdesc->data_len);
         rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, skbdesc->data_len);
         rt2x00_desc_write(txd, 2, word);
 
@@ -1046,8 +1042,7 @@ static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                            test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
         rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-                           !!(control->flags &
-                              IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
         rt2x00_desc_write(txd, 0, word);
 }
 
@@ -1083,16 +1078,15 @@ static void rt2400pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2400pci_fill_rxdone(struct queue_entry *entry,
                                   struct rxdone_entry_desc *rxdesc)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word0;
         u32 word2;
         u32 word3;
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word0);
-        rt2x00_desc_read(priv_rx->desc, 2, &word2);
-        rt2x00_desc_read(priv_rx->desc, 3, &word3);
+        rt2x00_desc_read(entry_priv->desc, 0, &word0);
+        rt2x00_desc_read(entry_priv->desc, 2, &word2);
+        rt2x00_desc_read(entry_priv->desc, 3, &word3);
 
-        rxdesc->flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
         if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
@@ -1108,7 +1102,7 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
             entry->queue->rt2x00dev->rssi_offset;
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-        rxdesc->dev_flags = RXDONE_SIGNAL_PLCP;
+        rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 }
@@ -1120,15 +1114,15 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
                              const enum data_queue_qid queue_idx)
 {
         struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         struct queue_entry *entry;
         struct txdone_entry_desc txdesc;
         u32 word;
 
         while (!rt2x00queue_empty(queue)) {
                 entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
-                priv_tx = entry->priv_data;
-                rt2x00_desc_read(priv_tx->desc, 0, &word);
+                entry_priv = entry->priv_data;
+                rt2x00_desc_read(entry_priv->desc, 0, &word);
 
                 if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
                     !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1137,7 +1131,18 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
                 /*
                  * Obtain the status about this packet.
                  */
-                txdesc.status = rt2x00_get_field32(word, TXD_W0_RESULT);
+                txdesc.flags = 0;
+                switch (rt2x00_get_field32(word, TXD_W0_RESULT)) {
+                case 0: /* Success */
+                case 1: /* Success with retry */
+                        __set_bit(TXDONE_SUCCESS, &txdesc.flags);
+                        break;
+                case 2: /* Failure, excessive retries */
+                        __set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+                        /* Don't break, this is a failed frame! */
+                default: /* Failure */
+                        __set_bit(TXDONE_FAILURE, &txdesc.flags);
+                }
                 txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
                 rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
@@ -1364,7 +1369,6 @@ static void rt2400pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
         rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
                                IEEE80211_HW_SIGNAL_DBM;
         rt2x00dev->hw->extra_tx_headroom = 0;
-        rt2x00dev->hw->queues = 2;
 
         SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
         SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -1480,18 +1484,27 @@ static u64 rt2400pci_get_tsf(struct ieee80211_hw *hw)
         return tsf;
 }
 
-static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                                   struct ieee80211_tx_control *control)
+static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
-        struct rt2x00_intf *intf = vif_to_intf(control->vif);
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+        struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
+        struct queue_entry_priv_pci *entry_priv;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         u32 reg;
 
         if (unlikely(!intf->beacon))
                 return -ENOBUFS;
-        priv_tx = intf->beacon->priv_data;
+        entry_priv = intf->beacon->priv_data;
+
+        /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        intf->beacon->skb = skb;
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
 
         /*
          * Fill in skb descriptor
@@ -1501,7 +1514,7 @@ static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
         skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
         skbdesc->data = skb->data;
         skbdesc->data_len = skb->len;
-        skbdesc->desc = priv_tx->desc;
+        skbdesc->desc = entry_priv->desc;
         skbdesc->desc_len = intf->beacon->queue->desc_size;
         skbdesc->entry = intf->beacon;
 
@@ -1520,8 +1533,8 @@ static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
          * Write entire beacon with descriptor to register,
          * and kick the beacon generator.
          */
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-        memcpy(priv_tx->data, skb->data, skb->len);
+        memcpy(entry_priv->data, skb->data, skb->len);
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
         rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, QID_BEACON);
 
         return 0;
@@ -1581,28 +1594,28 @@ static const struct data_queue_desc rt2400pci_queue_rx = {
         .entry_num              = RX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = RXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_rx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_tx = {
         .entry_num              = TX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_bcn = {
         .entry_num              = BEACON_ENTRIES,
         .data_size              = MGMT_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_atim = {
         .entry_num              = ATIM_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt2400pci_ops = {
@@ -1611,6 +1624,7 @@ static const struct rt2x00_ops rt2400pci_ops = {
         .max_ap_intf    = 1,
         .eeprom_size    = EEPROM_SIZE,
         .rf_size        = RF_SIZE,
+        .tx_queues      = NUM_TX_QUEUES,
         .rx             = &rt2400pci_queue_rx,
         .tx             = &rt2400pci_queue_tx,
         .bcn            = &rt2400pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.h b/drivers/net/wireless/rt2x00/rt2400pci.h
index a5210f9a3360..e9aa326be9f6 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.h
+++ b/drivers/net/wireless/rt2x00/rt2400pci.h
@@ -52,6 +52,11 @@
 #define RF_SIZE                         0x0010
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES                   2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index c06f1b5e5887..673350953b89 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -715,38 +715,33 @@ dynamic_cca_tune:
 static void rt2500pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
                                    struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_rx->desc, 1, &word);
-        rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, priv_rx->data_dma);
-        rt2x00_desc_write(priv_rx->desc, 1, word);
+        rt2x00_desc_read(entry_priv->desc, 1, &word);
+        rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, entry_priv->data_dma);
+        rt2x00_desc_write(entry_priv->desc, 1, word);
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-        rt2x00_desc_write(priv_rx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt2500pci_init_txentry(struct rt2x00_dev *rt2x00dev,
                                    struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_tx->desc, 1, &word);
-        rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, priv_tx->data_dma);
-        rt2x00_desc_write(priv_tx->desc, 1, word);
-
-        rt2x00_desc_read(priv_tx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, TXD_W0_VALID, 0);
         rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-        rt2x00_desc_write(priv_tx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-        struct queue_entry_priv_pci_rx *priv_rx;
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         u32 reg;
 
         /*
@@ -759,28 +754,28 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
         rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
-        priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
         rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
 
-        priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
         rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-        priv_tx = rt2x00dev->bcn[1].entries[0].priv_data;
+        entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
         rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-        priv_tx = rt2x00dev->bcn[0].entries[0].priv_data;
+        entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
         rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
 
         rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
@@ -788,9 +783,10 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
         rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
 
-        priv_rx = rt2x00dev->rx->entries[0].priv_data;
+        entry_priv = rt2x00dev->rx->entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
-        rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER, priv_rx->desc_dma);
+        rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
 
         return 0;
@@ -1155,16 +1151,20 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                     struct sk_buff *skb,
-                                    struct txentry_desc *txdesc,
-                                    struct ieee80211_tx_control *control)
+                                    struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
         __le32 *txd = skbdesc->desc;
         u32 word;
 
         /*
          * Start writing the descriptor words.
          */
+        rt2x00_desc_read(entry_priv->desc, 1, &word);
+        rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, entry_priv->data_dma);
+        rt2x00_desc_write(entry_priv->desc, 1, word);
+
         rt2x00_desc_read(txd, 2, &word);
         rt2x00_set_field32(&word, TXD_W2_IV_OFFSET, IEEE80211_HEADER);
         rt2x00_set_field32(&word, TXD_W2_AIFS, txdesc->aifs);
@@ -1198,9 +1198,7 @@ static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W0_CIPHER_OWNER, 1);
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
         rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-                           !!(control->flags &
-                              IEEE80211_TXCTL_LONG_RETRY_LIMIT));
-        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
         rt2x00_desc_write(txd, 0, word);
 }
@@ -1237,14 +1235,13 @@ static void rt2500pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2500pci_fill_rxdone(struct queue_entry *entry,
                                   struct rxdone_entry_desc *rxdesc)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word0;
         u32 word2;
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word0);
-        rt2x00_desc_read(priv_rx->desc, 2, &word2);
+        rt2x00_desc_read(entry_priv->desc, 0, &word0);
+        rt2x00_desc_read(entry_priv->desc, 2, &word2);
 
-        rxdesc->flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
         if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
@@ -1261,7 +1258,6 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
             entry->queue->rt2x00dev->rssi_offset;
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-        rxdesc->dev_flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1275,15 +1271,15 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
                              const enum data_queue_qid queue_idx)
 {
         struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         struct queue_entry *entry;
         struct txdone_entry_desc txdesc;
         u32 word;
 
         while (!rt2x00queue_empty(queue)) {
                 entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
-                priv_tx = entry->priv_data;
-                rt2x00_desc_read(priv_tx->desc, 0, &word);
+                entry_priv = entry->priv_data;
+                rt2x00_desc_read(entry_priv->desc, 0, &word);
 
                 if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
                     !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1292,7 +1288,18 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
                 /*
                  * Obtain the status about this packet.
                  */
-                txdesc.status = rt2x00_get_field32(word, TXD_W0_RESULT);
+                txdesc.flags = 0;
+                switch (rt2x00_get_field32(word, TXD_W0_RESULT)) {
+                case 0: /* Success */
+                case 1: /* Success with retry */
+                        __set_bit(TXDONE_SUCCESS, &txdesc.flags);
+                        break;
+                case 2: /* Failure, excessive retries */
+                        __set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+                        /* Don't break, this is a failed frame! */
+                default: /* Failure */
+                        __set_bit(TXDONE_FAILURE, &txdesc.flags);
+                }
                 txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
                 rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
@@ -1684,7 +1691,6 @@ static void rt2500pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
                                IEEE80211_HW_SIGNAL_DBM;
 
         rt2x00dev->hw->extra_tx_headroom = 0;
-        rt2x00dev->hw->queues = 2;
 
         SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
         SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -1793,19 +1799,28 @@ static u64 rt2500pci_get_tsf(struct ieee80211_hw *hw)
         return tsf;
 }
 
-static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                                   struct ieee80211_tx_control *control)
+static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
-        struct rt2x00_intf *intf = vif_to_intf(control->vif);
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+        struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
+        struct queue_entry_priv_pci *entry_priv;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         u32 reg;
 
         if (unlikely(!intf->beacon))
                 return -ENOBUFS;
 
-        priv_tx = intf->beacon->priv_data;
+        entry_priv = intf->beacon->priv_data;
+
+        /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        intf->beacon->skb = skb;
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
 
         /*
          * Fill in skb descriptor
@@ -1815,7 +1830,7 @@ static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
         skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
         skbdesc->data = skb->data;
         skbdesc->data_len = skb->len;
-        skbdesc->desc = priv_tx->desc;
+        skbdesc->desc = entry_priv->desc;
         skbdesc->desc_len = intf->beacon->queue->desc_size;
         skbdesc->entry = intf->beacon;
 
@@ -1834,8 +1849,8 @@ static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
          * Write entire beacon with descriptor to register,
          * and kick the beacon generator.
          */
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-        memcpy(priv_tx->data, skb->data, skb->len);
+        memcpy(entry_priv->data, skb->data, skb->len);
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
         rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, QID_BEACON);
 
         return 0;
@@ -1895,28 +1910,28 @@ static const struct data_queue_desc rt2500pci_queue_rx = {
         .entry_num              = RX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = RXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_rx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_tx = {
         .entry_num              = TX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_bcn = {
         .entry_num              = BEACON_ENTRIES,
         .data_size              = MGMT_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_atim = {
         .entry_num              = ATIM_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt2500pci_ops = {
@@ -1925,6 +1940,7 @@ static const struct rt2x00_ops rt2500pci_ops = {
         .max_ap_intf    = 1,
         .eeprom_size    = EEPROM_SIZE,
         .rf_size        = RF_SIZE,
+        .tx_queues      = NUM_TX_QUEUES,
         .rx             = &rt2500pci_queue_rx,
         .tx             = &rt2500pci_queue_tx,
         .bcn            = &rt2500pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.h b/drivers/net/wireless/rt2x00/rt2500pci.h
index 13899550465a..ea93b8f423a9 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.h
+++ b/drivers/net/wireless/rt2x00/rt2500pci.h
@@ -63,6 +63,11 @@
 #define RF_SIZE                         0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES                   2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 88bafdf8f0fa..cca1504550dc 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1033,8 +1033,7 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                     struct sk_buff *skb,
-                                    struct txentry_desc *txdesc,
-                                    struct ieee80211_tx_control *control)
+                                    struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
         __le32 *txd = skbdesc->desc;
@@ -1058,7 +1057,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_desc_write(txd, 2, word);
 
         rt2x00_desc_read(txd, 0, &word);
-        rt2x00_set_field32(&word, TXD_W0_RETRY_LIMIT, control->retry_limit);
+        rt2x00_set_field32(&word, TXD_W0_RETRY_LIMIT, txdesc->retry_limit);
         rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
                            test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_ACK,
@@ -1068,7 +1067,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W0_OFDM,
                            test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
-                           !!(control->flags & IEEE80211_TXCTL_FIRST_FRAGMENT));
+                           test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
         rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
         rt2x00_set_field32(&word, TXD_W0_CIPHER, CIPHER_NONE);
@@ -1125,30 +1124,32 @@ static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2500usb_fill_rxdone(struct queue_entry *entry,
                                   struct rxdone_entry_desc *rxdesc)
 {
-        struct queue_entry_priv_usb_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
         __le32 *rxd =
             (__le32 *)(entry->skb->data +
-                       (priv_rx->urb->actual_length - entry->queue->desc_size));
-        unsigned int offset = entry->queue->desc_size + 2;
+                       (entry_priv->urb->actual_length -
+                        entry->queue->desc_size));
         u32 word0;
         u32 word1;
 
         /*
-         * Copy descriptor to the available headroom inside the skbuffer.
+         * Copy descriptor to the skb->cb array, this has 2 benefits:
+         * 1) Each descriptor word is 4 byte aligned.
+         * 2) Descriptor is safe  from moving of frame data in rt2x00usb.
          */
-        skb_push(entry->skb, offset);
-        memcpy(entry->skb->data, rxd, entry->queue->desc_size);
-        rxd = (__le32 *)entry->skb->data;
+        skbdesc->desc_len =
+            min_t(u16, entry->queue->desc_size, sizeof(entry->skb->cb));
+        memcpy(entry->skb->cb, rxd, skbdesc->desc_len);
+        skbdesc->desc = entry->skb->cb;
+        rxd = (__le32 *)skbdesc->desc;
 
         /*
-         * The descriptor is now aligned to 4 bytes and thus it is
-         * now safe to read it on all architectures.
+         * It is now safe to read the descriptor on all architectures.
          */
         rt2x00_desc_read(rxd, 0, &word0);
         rt2x00_desc_read(rxd, 1, &word1);
 
-        rxdesc->flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
         if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
@@ -1165,7 +1166,6 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
             entry->queue->rt2x00dev->rssi_offset;
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-        rxdesc->dev_flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1174,16 +1174,9 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
         /*
          * Adjust the skb memory window to the frame boundaries.
          */
-        skb_pull(entry->skb, offset);
         skb_trim(entry->skb, rxdesc->size);
-
-        /*
-         * Set descriptor and data pointer.
-         */
         skbdesc->data = entry->skb->data;
         skbdesc->data_len = rxdesc->size;
-        skbdesc->desc = rxd;
-        skbdesc->desc_len = entry->queue->desc_size;
 }
 
 /*
@@ -1192,7 +1185,7 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 static void rt2500usb_beacondone(struct urb *urb)
 {
         struct queue_entry *entry = (struct queue_entry *)urb->context;
-        struct queue_entry_priv_usb_bcn *priv_bcn = entry->priv_data;
+        struct queue_entry_priv_usb_bcn *bcn_priv = entry->priv_data;
 
         if (!test_bit(DEVICE_ENABLED_RADIO, &entry->queue->rt2x00dev->flags))
                 return;
@@ -1203,9 +1196,9 @@ static void rt2500usb_beacondone(struct urb *urb)
          * Otherwise we should free the sk_buffer, the device
          * should be doing the rest of the work now.
          */
-        if (priv_bcn->guardian_urb == urb) {
-                usb_submit_urb(priv_bcn->urb, GFP_ATOMIC);
-        } else if (priv_bcn->urb == urb) {
+        if (bcn_priv->guardian_urb == urb) {
+                usb_submit_urb(bcn_priv->urb, GFP_ATOMIC);
+        } else if (bcn_priv->urb == urb) {
                 dev_kfree_skb(entry->skb);
                 entry->skb = NULL;
         }
@@ -1591,7 +1584,6 @@ static void rt2500usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
             IEEE80211_HW_SIGNAL_DBM;
 
         rt2x00dev->hw->extra_tx_headroom = TXD_DESC_SIZE;
-        rt2x00dev->hw->queues = 2;
 
         SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_usb(rt2x00dev)->dev);
         SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -1674,15 +1666,15 @@ static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 /*
  * IEEE80211 stack callback functions.
  */
-static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
-                                   struct sk_buff *skb,
-                                   struct ieee80211_tx_control *control)
+static int rt2500usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
         struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
-        struct rt2x00_intf *intf = vif_to_intf(control->vif);
-        struct queue_entry_priv_usb_bcn *priv_bcn;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+        struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
+        struct queue_entry_priv_usb_bcn *bcn_priv;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         int pipe = usb_sndbulkpipe(usb_dev, 1);
         int length;
         u16 reg;
@@ -1690,7 +1682,15 @@ static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
         if (unlikely(!intf->beacon))
                 return -ENOBUFS;
 
-        priv_bcn = intf->beacon->priv_data;
+        bcn_priv = intf->beacon->priv_data;
+
+        /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        intf->beacon->skb = skb;
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
 
         /*
          * Add the descriptor in front of the skb.
@@ -1720,7 +1720,7 @@ static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
         rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
         rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
 
         /*
          * USB devices cannot blindly pass the skb->len as the
@@ -1729,7 +1729,7 @@ static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
          */
         length = rt2500usb_get_tx_data_len(rt2x00dev, skb);
 
-        usb_fill_bulk_urb(priv_bcn->urb, usb_dev, pipe,
+        usb_fill_bulk_urb(bcn_priv->urb, usb_dev, pipe,
                           skb->data, length, rt2500usb_beacondone,
                           intf->beacon);
 
@@ -1738,15 +1738,15 @@ static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
          * We only need a single byte, so lets recycle
          * the 'flags' field we are not using for beacons.
          */
-        priv_bcn->guardian_data = 0;
-        usb_fill_bulk_urb(priv_bcn->guardian_urb, usb_dev, pipe,
-                          &priv_bcn->guardian_data, 1, rt2500usb_beacondone,
+        bcn_priv->guardian_data = 0;
+        usb_fill_bulk_urb(bcn_priv->guardian_urb, usb_dev, pipe,
+                          &bcn_priv->guardian_data, 1, rt2500usb_beacondone,
                           intf->beacon);
 
         /*
          * Send out the guardian byte.
          */
-        usb_submit_urb(priv_bcn->guardian_urb, GFP_ATOMIC);
+        usb_submit_urb(bcn_priv->guardian_urb, GFP_ATOMIC);
 
         /*
          * Enable beacon generation.
@@ -1797,14 +1797,14 @@ static const struct data_queue_desc rt2500usb_queue_rx = {
         .entry_num              = RX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = RXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_rx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2500usb_queue_tx = {
         .entry_num              = TX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2500usb_queue_bcn = {
@@ -1818,7 +1818,7 @@ static const struct data_queue_desc rt2500usb_queue_atim = {
         .entry_num              = ATIM_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct rt2x00_ops rt2500usb_ops = {
@@ -1827,6 +1827,7 @@ static const struct rt2x00_ops rt2500usb_ops = {
         .max_ap_intf    = 1,
         .eeprom_size    = EEPROM_SIZE,
         .rf_size        = RF_SIZE,
+        .tx_queues      = NUM_TX_QUEUES,
         .rx             = &rt2500usb_queue_rx,
         .tx             = &rt2500usb_queue_tx,
         .bcn            = &rt2500usb_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.h b/drivers/net/wireless/rt2x00/rt2500usb.h
index a37a068d0c71..7d50098f0cc5 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.h
+++ b/drivers/net/wireless/rt2x00/rt2500usb.h
@@ -63,6 +63,11 @@
 #define RF_SIZE                         0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES                   2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 79bd9c9f8963..5c7220ea46e6 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -44,7 +44,7 @@
 /*
  * Module information.
  */
-#define DRV_VERSION     "2.1.5"
+#define DRV_VERSION     "2.1.6"
 #define DRV_PROJECT     "http://rt2x00.serialmonkey.com"
 
 /*
@@ -540,11 +540,9 @@ struct rt2x00lib_ops {
          */
         void (*write_tx_desc) (struct rt2x00_dev *rt2x00dev,
                                struct sk_buff *skb,
-                               struct txentry_desc *txdesc,
-                               struct ieee80211_tx_control *control);
+                               struct txentry_desc *txdesc);
         int (*write_tx_data) (struct rt2x00_dev *rt2x00dev,
-                              struct data_queue *queue, struct sk_buff *skb,
-                              struct ieee80211_tx_control *control);
+                              struct data_queue *queue, struct sk_buff *skb);
         int (*get_tx_data_len) (struct rt2x00_dev *rt2x00dev,
                                 struct sk_buff *skb);
         void (*kick_tx_queue) (struct rt2x00_dev *rt2x00dev,
@@ -592,6 +590,7 @@ struct rt2x00_ops {
         const unsigned int max_ap_intf;
         const unsigned int eeprom_size;
         const unsigned int rf_size;
+        const unsigned int tx_queues;
         const struct data_queue_desc *rx;
         const struct data_queue_desc *tx;
         const struct data_queue_desc *bcn;
@@ -927,6 +926,39 @@ static inline u16 get_duration_res(const unsigned int size, const u8 rate)
 }
 
 /**
+ * rt2x00queue_create_tx_descriptor - Create TX descriptor from mac80211 input
+ * @entry: The entry which will be used to transfer the TX frame.
+ * @txdesc: rt2x00 TX descriptor which will be initialized by this function.
+ *
+ * This function will initialize the &struct txentry_desc based on information
+ * from mac80211. This descriptor can then be used by rt2x00lib and the drivers
+ * to correctly initialize the hardware descriptor.
+ * Note that before calling this function the skb->cb array must be untouched
+ * by rt2x00lib. Only after this function completes will it be save to
+ * overwrite the skb->cb information.
+ * The reason for this is that mac80211 writes its own tx information into
+ * the skb->cb array, and this function will use that information to initialize
+ * the &struct txentry_desc structure.
+ */
+void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
+                                      struct txentry_desc *txdesc);
+
+/**
+ * rt2x00queue_write_tx_descriptor - Write TX descriptor to hardware
+ * @entry: The entry which will be used to transfer the TX frame.
+ * @txdesc: TX descriptor which will be used to write hardware descriptor
+ *
+ * This function will write a TX descriptor initialized by
+ * &rt2x00queue_create_tx_descriptor to the hardware. After this call
+ * has completed the frame is now owned by the hardware, the hardware
+ * queue will have automatically be kicked unless this frame was generated
+ * by rt2x00lib, in which case the frame is "special" and must be kicked
+ * by the caller.
+ */
+void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
+                                     struct txentry_desc *txdesc);
+
+/**
  * rt2x00queue_get_queue - Convert queue index to queue pointer
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
  * @queue: rt2x00 queue index (see &enum data_queue_qid).
@@ -964,17 +996,9 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
                       struct rxdone_entry_desc *rxdesc);
 
 /*
- * TX descriptor initializer
- */
-void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
-                             struct sk_buff *skb,
-                             struct ieee80211_tx_control *control);
-
-/*
  * mac80211 handlers.
  */
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                 struct ieee80211_tx_control *control);
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 int rt2x00mac_start(struct ieee80211_hw *hw);
 void rt2x00mac_stop(struct ieee80211_hw *hw);
 int rt2x00mac_add_interface(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index bfab3b8780d6..bd92cb8e68e0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -115,7 +115,7 @@ struct rt2x00debug_intf {
 };
 
 void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
-                            struct sk_buff *skb)
+                            enum rt2x00_dump_type type, struct sk_buff *skb)
 {
         struct rt2x00debug_intf *intf = rt2x00dev->debugfs_intf;
         struct skb_frame_desc *desc = get_skb_frame_desc(skb);
@@ -148,7 +148,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
         dump_hdr->chip_rt = cpu_to_le16(rt2x00dev->chip.rt);
         dump_hdr->chip_rf = cpu_to_le16(rt2x00dev->chip.rf);
         dump_hdr->chip_rev = cpu_to_le32(rt2x00dev->chip.rev);
-        dump_hdr->type = cpu_to_le16(desc->frame_type);
+        dump_hdr->type = cpu_to_le16(type);
         dump_hdr->queue_index = desc->entry->queue->qid;
         dump_hdr->entry_index = desc->entry->entry_idx;
         dump_hdr->timestamp_sec = cpu_to_le32(timestamp.tv_sec);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 3a49c256789f..69e233610c94 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -28,7 +28,6 @@
 
 #include "rt2x00.h"
 #include "rt2x00lib.h"
-#include "rt2x00dump.h"
 
 /*
  * Link tuning handlers
@@ -126,7 +125,7 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
         /*
          * Start the TX queues.
          */
-        ieee80211_start_queues(rt2x00dev->hw);
+        ieee80211_wake_queues(rt2x00dev->hw);
 
         return 0;
 }
@@ -416,7 +415,6 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
         struct rt2x00_dev *rt2x00dev = data;
         struct rt2x00_intf *intf = vif_to_intf(vif);
         struct sk_buff *skb;
-        struct ieee80211_tx_control control;
         struct ieee80211_bss_conf conf;
         int delayed_flags;
 
@@ -434,9 +432,9 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
         spin_unlock(&intf->lock);
 
         if (delayed_flags & DELAYED_UPDATE_BEACON) {
-                skb = ieee80211_beacon_get(rt2x00dev->hw, vif, &control);
-                if (skb && rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw,
-                                                             skb, &control))
+                skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
+                if (skb &&
+                    rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw, skb))
                         dev_kfree_skb(skb);
         }
 
@@ -495,61 +493,55 @@ void rt2x00lib_txdone(struct queue_entry *entry,
                       struct txdone_entry_desc *txdesc)
 {
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-        struct skb_frame_desc *skbdesc;
-        struct ieee80211_tx_status tx_status;
-        int success = !!(txdesc->status == TX_SUCCESS ||
-                         txdesc->status == TX_SUCCESS_RETRY);
-        int fail = !!(txdesc->status == TX_FAIL_RETRY ||
-                      txdesc->status == TX_FAIL_INVALID ||
-                      txdesc->status == TX_FAIL_OTHER);
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
+
+        /*
+         * Send frame to debugfs immediately, after this call is completed
+         * we are going to overwrite the skb->cb array.
+         */
+        rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TXDONE, entry->skb);
 
         /*
          * Update TX statistics.
          */
-        rt2x00dev->link.qual.tx_success += success;
-        rt2x00dev->link.qual.tx_failed += txdesc->retry + fail;
+        rt2x00dev->link.qual.tx_success +=
+            test_bit(TXDONE_SUCCESS, &txdesc->flags);
+        rt2x00dev->link.qual.tx_failed +=
+            txdesc->retry + !!test_bit(TXDONE_FAILURE, &txdesc->flags);
 
         /*
          * Initialize TX status
          */
-        tx_status.flags = 0;
-        tx_status.ack_signal = 0;
-        tx_status.excessive_retries = (txdesc->status == TX_FAIL_RETRY);
-        tx_status.retry_count = txdesc->retry;
-        memcpy(&tx_status.control, txdesc->control, sizeof(*txdesc->control));
+        memset(&tx_info->status, 0, sizeof(tx_info->status));
+        tx_info->status.ack_signal = 0;
+        tx_info->status.excessive_retries =
+            test_bit(TXDONE_EXCESSIVE_RETRY, &txdesc->flags);
+        tx_info->status.retry_count = txdesc->retry;
 
-        if (!(tx_status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
-                if (success)
-                        tx_status.flags |= IEEE80211_TX_STATUS_ACK;
-                else
+        if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+                if (test_bit(TXDONE_SUCCESS, &txdesc->flags))
+                        tx_info->flags |= IEEE80211_TX_STAT_ACK;
+                else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
                         rt2x00dev->low_level_stats.dot11ACKFailureCount++;
         }
 
-        if (tx_status.control.flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-                if (success)
+        if (tx_info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
+                if (test_bit(TXDONE_SUCCESS, &txdesc->flags))
                         rt2x00dev->low_level_stats.dot11RTSSuccessCount++;
-                else
+                else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
                         rt2x00dev->low_level_stats.dot11RTSFailureCount++;
         }
 
         /*
-         * Send the tx_status to debugfs. Only send the status report
-         * to mac80211 when the frame originated from there. If this was
-         * a extra frame coming through a mac80211 library call (RTS/CTS)
-         * then we should not send the status report back.
-         * If send to mac80211, mac80211 will clean up the skb structure,
-         * otherwise we have to do it ourself.
+         * Only send the status report to mac80211 when TX status was
+         * requested by it. If this was a extra frame coming through
+         * a mac80211 library call (RTS/CTS) then we should not send the
+         * status report back.
          */
-        skbdesc = get_skb_frame_desc(entry->skb);
-        skbdesc->frame_type = DUMP_FRAME_TXDONE;
-
-        rt2x00debug_dump_frame(rt2x00dev, entry->skb);
-
-        if (!(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
-                ieee80211_tx_status_irqsafe(rt2x00dev->hw,
-                                            entry->skb, &tx_status);
+        if (tx_info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
+                ieee80211_tx_status_irqsafe(rt2x00dev->hw, entry->skb);
         else
-                dev_kfree_skb(entry->skb);
+                dev_kfree_skb_irq(entry->skb);
         entry->skb = NULL;
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_txdone);
@@ -610,154 +602,13 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
          * Send frame to mac80211 & debugfs.
          * mac80211 will clean up the skb structure.
          */
-        get_skb_frame_desc(entry->skb)->frame_type = DUMP_FRAME_RXDONE;
-        rt2x00debug_dump_frame(rt2x00dev, entry->skb);
+        rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_RXDONE, entry->skb);
         ieee80211_rx_irqsafe(rt2x00dev->hw, entry->skb, rx_status);
         entry->skb = NULL;
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_rxdone);
 
 /*
- * TX descriptor initializer
- */
-void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
-                             struct sk_buff *skb,
-                             struct ieee80211_tx_control *control)
-{
-        struct txentry_desc txdesc;
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skbdesc->data;
-        const struct rt2x00_rate *rate;
-        int tx_rate;
-        int length;
-        int duration;
-        int residual;
-        u16 frame_control;
-        u16 seq_ctrl;
-
-        memset(&txdesc, 0, sizeof(txdesc));
-
-        txdesc.queue = skbdesc->entry->queue->qid;
-        txdesc.cw_min = skbdesc->entry->queue->cw_min;
-        txdesc.cw_max = skbdesc->entry->queue->cw_max;
-        txdesc.aifs = skbdesc->entry->queue->aifs;
-
-        /*
-         * Read required fields from ieee80211 header.
-         */
-        frame_control = le16_to_cpu(hdr->frame_control);
-        seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
-
-        tx_rate = control->tx_rate->hw_value;
-
-        /*
-         * Check whether this frame is to be acked
-         */
-        if (!(control->flags & IEEE80211_TXCTL_NO_ACK))
-                __set_bit(ENTRY_TXD_ACK, &txdesc.flags);
-
-        /*
-         * Check if this is a RTS/CTS frame
-         */
-        if (is_rts_frame(frame_control) || is_cts_frame(frame_control)) {
-                __set_bit(ENTRY_TXD_BURST, &txdesc.flags);
-                if (is_rts_frame(frame_control)) {
-                        __set_bit(ENTRY_TXD_RTS_FRAME, &txdesc.flags);
-                        __set_bit(ENTRY_TXD_ACK, &txdesc.flags);
-                } else
-                        __clear_bit(ENTRY_TXD_ACK, &txdesc.flags);
-                if (control->rts_cts_rate)
-                        tx_rate = control->rts_cts_rate->hw_value;
-        }
-
-        rate = rt2x00_get_rate(tx_rate);
-
-        /*
-         * Check if more fragments are pending
-         */
-        if (ieee80211_get_morefrag(hdr)) {
-                __set_bit(ENTRY_TXD_BURST, &txdesc.flags);
-                __set_bit(ENTRY_TXD_MORE_FRAG, &txdesc.flags);
-        }
-
-        /*
-         * Beacons and probe responses require the tsf timestamp
-         * to be inserted into the frame.
-         */
-        if (txdesc.queue == QID_BEACON || is_probe_resp(frame_control))
-                __set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc.flags);
-
-        /*
-         * Determine with what IFS priority this frame should be send.
-         * Set ifs to IFS_SIFS when the this is not the first fragment,
-         * or this fragment came after RTS/CTS.
-         */
-        if ((seq_ctrl & IEEE80211_SCTL_FRAG) > 0 ||
-            test_bit(ENTRY_TXD_RTS_FRAME, &txdesc.flags))
-                txdesc.ifs = IFS_SIFS;
-        else
-                txdesc.ifs = IFS_BACKOFF;
-
-        /*
-         * PLCP setup
-         * Length calculation depends on OFDM/CCK rate.
-         */
-        txdesc.signal = rate->plcp;
-        txdesc.service = 0x04;
-
-        length = skbdesc->data_len + FCS_LEN;
-        if (rate->flags & DEV_RATE_OFDM) {
-                __set_bit(ENTRY_TXD_OFDM_RATE, &txdesc.flags);
-
-                txdesc.length_high = (length >> 6) & 0x3f;
-                txdesc.length_low = length & 0x3f;
-        } else {
-                /*
-                 * Convert length to microseconds.
-                 */
-                residual = get_duration_res(length, rate->bitrate);
-                duration = get_duration(length, rate->bitrate);
-
-                if (residual != 0) {
-                        duration++;
-
-                        /*
-                         * Check if we need to set the Length Extension
-                         */
-                        if (rate->bitrate == 110 && residual <= 30)
-                                txdesc.service |= 0x80;
-                }
-
-                txdesc.length_high = (duration >> 8) & 0xff;
-                txdesc.length_low = duration & 0xff;
-
-                /*
-                 * When preamble is enabled we should set the
-                 * preamble bit for the signal.
-                 */
-                if (rt2x00_get_rate_preamble(tx_rate))
-                        txdesc.signal |= 0x08;
-        }
-
-        rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, skb, &txdesc, control);
-
-        /*
-         * Update queue entry.
-         */
-        skbdesc->entry->skb = skb;
-
-        /*
-         * The frame has been completely initialized and ready
-         * for sending to the device. The caller will push the
-         * frame to the device, but we are going to push the
-         * frame to debugfs here.
-         */
-        skbdesc->frame_type = DUMP_FRAME_TX;
-        rt2x00debug_dump_frame(rt2x00dev, skb);
-}
-EXPORT_SYMBOL_GPL(rt2x00lib_write_tx_desc);
-
-/*
  * Driver initialization handlers.
  */
 const struct rt2x00_rate rt2x00_supported_rates[12] = {
@@ -973,6 +824,11 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
                 return status;
 
         /*
+         * Initialize HW fields.
+         */
+        rt2x00dev->hw->queues = rt2x00dev->ops->tx_queues;
+
+        /*
          * Register HW.
          */
         status = ieee80211_register_hw(rt2x00dev->hw);
@@ -1327,7 +1183,7 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
          * In that case we have disabled the TX queue and should
          * now enable it again
          */
-        ieee80211_start_queues(rt2x00dev->hw);
+        ieee80211_wake_queues(rt2x00dev->hw);
 
         /*
          * During interface iteration we might have changed the
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 41ee02cd2825..c4ce534e3cdb 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -26,6 +26,8 @@
 #ifndef RT2X00LIB_H
 #define RT2X00LIB_H
 
+#include "rt2x00dump.h"
+
 /*
  * Interval defines
  * Both the link tuner as the rfkill will be called once per second.
@@ -128,7 +130,8 @@ static inline void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 void rt2x00debug_register(struct rt2x00_dev *rt2x00dev);
 void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev);
-void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
+                            enum rt2x00_dump_type type, struct sk_buff *skb);
 #else
 static inline void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 {
@@ -139,6 +142,7 @@ static inline void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
 }
 
 static inline void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
+                                          enum rt2x00_dump_type type,
                                           struct sk_buff *skb)
 {
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 767e0ffce04e..c05e05b58887 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -31,14 +31,15 @@
 
 static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
                                 struct data_queue *queue,
-                                struct sk_buff *frag_skb,
-                                struct ieee80211_tx_control *control)
+                                struct sk_buff *frag_skb)
 {
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(frag_skb);
         struct skb_frame_desc *skbdesc;
+        struct ieee80211_tx_info *rts_info;
         struct sk_buff *skb;
         int size;
 
-        if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+        if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
                 size = sizeof(struct ieee80211_cts);
         else
                 size = sizeof(struct ieee80211_rts);
@@ -52,13 +53,33 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
         skb_reserve(skb, rt2x00dev->hw->extra_tx_headroom);
         skb_put(skb, size);
 
-        if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
-                ieee80211_ctstoself_get(rt2x00dev->hw, control->vif,
-                                        frag_skb->data, frag_skb->len, control,
+        /*
+         * Copy TX information over from original frame to
+         * RTS/CTS frame. Note that we set the no encryption flag
+         * since we don't want this frame to be encrypted.
+         * RTS frames should be acked, while CTS-to-self frames
+         * should not. The ready for TX flag is cleared to prevent
+         * it being automatically send when the descriptor is
+         * written to the hardware.
+         */
+        memcpy(skb->cb, frag_skb->cb, sizeof(skb->cb));
+        rts_info = IEEE80211_SKB_CB(skb);
+        rts_info->flags |= IEEE80211_TX_CTL_DO_NOT_ENCRYPT;
+        rts_info->flags &= ~IEEE80211_TX_CTL_USE_CTS_PROTECT;
+        rts_info->flags &= ~IEEE80211_TX_CTL_REQ_TX_STATUS;
+
+        if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
+                rts_info->flags |= IEEE80211_TX_CTL_NO_ACK;
+        else
+                rts_info->flags &= ~IEEE80211_TX_CTL_NO_ACK;
+
+        if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
+                ieee80211_ctstoself_get(rt2x00dev->hw, tx_info->control.vif,
+                                        frag_skb->data, size, tx_info,
                                         (struct ieee80211_cts *)(skb->data));
         else
-                ieee80211_rts_get(rt2x00dev->hw, control->vif,
-                                  frag_skb->data, frag_skb->len, control,
+                ieee80211_rts_get(rt2x00dev->hw, tx_info->control.vif,
+                                  frag_skb->data, size, tx_info,
                                   (struct ieee80211_rts *)(skb->data));
 
         /*
@@ -68,7 +89,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
         memset(skbdesc, 0, sizeof(*skbdesc));
         skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
 
-        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb, control)) {
+        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb)) {
                 WARNING(rt2x00dev, "Failed to send RTS/CTS frame.\n");
                 return NETDEV_TX_BUSY;
         }
@@ -76,14 +97,13 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
         return NETDEV_TX_OK;
 }
 
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                 struct ieee80211_tx_control *control)
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
         struct ieee80211_hdr *ieee80211hdr = (struct ieee80211_hdr *)skb->data;
-        enum data_queue_qid qid = mac80211_queue_to_qid(control->queue);
+        enum data_queue_qid qid = skb_get_queue_mapping(skb);
         struct data_queue *queue;
-        struct skb_frame_desc *skbdesc;
         u16 frame_control;
 
         /*
@@ -100,7 +120,7 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
         /*
          * Determine which queue to put packet on.
          */
-        if (control->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM &&
+        if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM &&
             test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
                 queue = rt2x00queue_get_queue(rt2x00dev, QID_ATIM);
         else
@@ -125,33 +145,27 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
          */
         frame_control = le16_to_cpu(ieee80211hdr->frame_control);
         if (!is_rts_frame(frame_control) && !is_cts_frame(frame_control) &&
-            (control->flags & (IEEE80211_TXCTL_USE_RTS_CTS |
-                               IEEE80211_TXCTL_USE_CTS_PROTECT)) &&
+            (tx_info->flags & (IEEE80211_TX_CTL_USE_RTS_CTS |
+                               IEEE80211_TX_CTL_USE_CTS_PROTECT)) &&
             !rt2x00dev->ops->hw->set_rts_threshold) {
                 if (rt2x00queue_available(queue) <= 1) {
-                        ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+                        ieee80211_stop_queue(rt2x00dev->hw, qid);
                         return NETDEV_TX_BUSY;
                 }
 
-                if (rt2x00mac_tx_rts_cts(rt2x00dev, queue, skb, control)) {
-                        ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+                if (rt2x00mac_tx_rts_cts(rt2x00dev, queue, skb)) {
+                        ieee80211_stop_queue(rt2x00dev->hw, qid);
                         return NETDEV_TX_BUSY;
                 }
         }
 
-        /*
-         * Initialize skb descriptor
-         */
-        skbdesc = get_skb_frame_desc(skb);
-        memset(skbdesc, 0, sizeof(*skbdesc));
-
-        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb, control)) {
-                ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb)) {
+                ieee80211_stop_queue(rt2x00dev->hw, qid);
                 return NETDEV_TX_BUSY;
         }
 
         if (rt2x00queue_full(queue))
-                ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+                ieee80211_stop_queue(rt2x00dev->hw, qid);
 
         if (rt2x00dev->ops->lib->kick_tx_queue)
                 rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, qid);
@@ -380,9 +394,7 @@ int rt2x00mac_config_interface(struct ieee80211_hw *hw,
         if (conf->type != IEEE80211_IF_TYPE_AP || !conf->beacon)
                 return 0;
 
-        status = rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw,
-                                                   conf->beacon,
-                                                   conf->beacon_control);
+        status = rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw, conf->beacon);
         if (status)
                 dev_kfree_skb(conf->beacon);
 
@@ -456,7 +468,7 @@ int rt2x00mac_get_tx_stats(struct ieee80211_hw *hw,
         struct rt2x00_dev *rt2x00dev = hw->priv;
         unsigned int i;
 
-        for (i = 0; i < hw->queues; i++) {
+        for (i = 0; i < rt2x00dev->ops->tx_queues; i++) {
                 stats[i].len = rt2x00dev->tx[i].length;
                 stats[i].limit = rt2x00dev->tx[i].limit;
                 stats[i].count = rt2x00dev->tx[i].count;
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index c17078eac197..70a3d135f64e 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -35,18 +35,18 @@
  * TX data handlers.
  */
 int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
-                            struct data_queue *queue, struct sk_buff *skb,
-                            struct ieee80211_tx_control *control)
+                            struct data_queue *queue, struct sk_buff *skb)
 {
         struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
-        struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         u32 word;
 
         if (rt2x00queue_full(queue))
                 return -EINVAL;
 
-        rt2x00_desc_read(priv_tx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
 
         if (rt2x00_get_field32(word, TXD_ENTRY_OWNER_NIC) ||
             rt2x00_get_field32(word, TXD_ENTRY_VALID)) {
@@ -58,19 +58,27 @@ int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
         }
 
         /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        entry->skb = skb;
+        rt2x00queue_create_tx_descriptor(entry, &txdesc);
+
+        /*
          * Fill in skb descriptor
          */
         skbdesc = get_skb_frame_desc(skb);
+        memset(skbdesc, 0, sizeof(*skbdesc));
         skbdesc->data = skb->data;
         skbdesc->data_len = skb->len;
-        skbdesc->desc = priv_tx->desc;
+        skbdesc->desc = entry_priv->desc;
         skbdesc->desc_len = queue->desc_size;
         skbdesc->entry = entry;
 
-        memcpy(&priv_tx->control, control, sizeof(priv_tx->control));
-        memcpy(priv_tx->data, skb->data, skb->len);
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+        memcpy(entry_priv->data, skb->data, skb->len);
 
+        rt2x00queue_write_tx_descriptor(entry, &txdesc);
         rt2x00queue_index_inc(queue, Q_INDEX);
 
         return 0;
@@ -84,7 +92,7 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 {
         struct data_queue *queue = rt2x00dev->rx;
         struct queue_entry *entry;
-        struct queue_entry_priv_pci_rx *priv_rx;
+        struct queue_entry_priv_pci *entry_priv;
         struct ieee80211_hdr *hdr;
         struct skb_frame_desc *skbdesc;
         struct rxdone_entry_desc rxdesc;
@@ -94,8 +102,8 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 
         while (1) {
                 entry = rt2x00queue_get_entry(queue, Q_INDEX);
-                priv_rx = entry->priv_data;
-                rt2x00_desc_read(priv_rx->desc, 0, &word);
+                entry_priv = entry->priv_data;
+                rt2x00_desc_read(entry_priv->desc, 0, &word);
 
                 if (rt2x00_get_field32(word, RXD_ENTRY_OWNER_NIC))
                         break;
@@ -103,7 +111,7 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
                 memset(&rxdesc, 0, sizeof(rxdesc));
                 rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
 
-                hdr = (struct ieee80211_hdr *)priv_rx->data;
+                hdr = (struct ieee80211_hdr *)entry_priv->data;
                 header_size =
                     ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
 
@@ -123,7 +131,7 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 
                 skb_reserve(entry->skb, align);
                 memcpy(skb_put(entry->skb, rxdesc.size),
-                       priv_rx->data, rxdesc.size);
+                       entry_priv->data, rxdesc.size);
 
                 /*
                  * Fill in skb descriptor
@@ -132,7 +140,7 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
                 memset(skbdesc, 0, sizeof(*skbdesc));
                 skbdesc->data = entry->skb->data;
                 skbdesc->data_len = entry->skb->len;
-                skbdesc->desc = priv_rx->desc;
+                skbdesc->desc = entry_priv->desc;
                 skbdesc->desc_len = queue->desc_size;
                 skbdesc->entry = entry;
 
@@ -143,7 +151,7 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 
                 if (test_bit(DEVICE_ENABLED_RADIO, &queue->rt2x00dev->flags)) {
                         rt2x00_set_field32(&word, RXD_ENTRY_OWNER_NIC, 1);
-                        rt2x00_desc_write(priv_rx->desc, 0, word);
+                        rt2x00_desc_write(entry_priv->desc, 0, word);
                 }
 
                 rt2x00queue_index_inc(queue, Q_INDEX);
@@ -154,10 +162,10 @@ EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
 void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
                       struct txdone_entry_desc *txdesc)
 {
-        struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+        enum data_queue_qid qid = skb_get_queue_mapping(entry->skb);
         u32 word;
 
-        txdesc->control = &priv_tx->control;
         rt2x00lib_txdone(entry, txdesc);
 
         /*
@@ -165,10 +173,10 @@ void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
          */
         entry->flags = 0;
 
-        rt2x00_desc_read(priv_tx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, TXD_ENTRY_OWNER_NIC, 0);
         rt2x00_set_field32(&word, TXD_ENTRY_VALID, 0);
-        rt2x00_desc_write(priv_tx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 
         rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
 
@@ -178,7 +186,7 @@ void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
          * is reenabled when the txdone handler has finished.
          */
         if (!rt2x00queue_full(entry->queue))
-                ieee80211_wake_queue(rt2x00dev->hw, priv_tx->control.queue);
+                ieee80211_wake_queue(rt2x00dev->hw, qid);
 
 }
 EXPORT_SYMBOL_GPL(rt2x00pci_txdone);
@@ -217,14 +225,9 @@ static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
                                      struct data_queue *queue)
 {
         struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
-        struct queue_entry_priv_pci_rx *priv_rx;
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         void *addr;
         dma_addr_t dma;
-        void *desc_addr;
-        dma_addr_t desc_dma;
-        void *data_addr;
-        dma_addr_t data_dma;
         unsigned int i;
 
         /*
@@ -240,24 +243,11 @@ static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
          * Initialize all queue entries to contain valid addresses.
          */
         for (i = 0; i < queue->limit; i++) {
-                desc_addr = desc_offset(queue, addr, i);
-                desc_dma = desc_offset(queue, dma, i);
-                data_addr = data_offset(queue, addr, i);
-                data_dma = data_offset(queue, dma, i);
-
-                if (queue->qid == QID_RX) {
-                        priv_rx = queue->entries[i].priv_data;
-                        priv_rx->desc = desc_addr;
-                        priv_rx->desc_dma = desc_dma;
-                        priv_rx->data = data_addr;
-                        priv_rx->data_dma = data_dma;
-                } else {
-                        priv_tx = queue->entries[i].priv_data;
-                        priv_tx->desc = desc_addr;
-                        priv_tx->desc_dma = desc_dma;
-                        priv_tx->data = data_addr;
-                        priv_tx->data_dma = data_dma;
-                }
+                entry_priv = queue->entries[i].priv_data;
+                entry_priv->desc = desc_offset(queue, addr, i);
+                entry_priv->desc_dma = desc_offset(queue, dma, i);
+                entry_priv->data = data_offset(queue, addr, i);
+                entry_priv->data_dma = data_offset(queue, dma, i);
         }
 
         return 0;
@@ -267,28 +257,13 @@ static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
                                      struct data_queue *queue)
 {
         struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
-        struct queue_entry_priv_pci_rx *priv_rx;
-        struct queue_entry_priv_pci_tx *priv_tx;
-        void *data_addr;
-        dma_addr_t data_dma;
-
-        if (queue->qid == QID_RX) {
-                priv_rx = queue->entries[0].priv_data;
-                data_addr = priv_rx->data;
-                data_dma = priv_rx->data_dma;
-
-                priv_rx->data = NULL;
-        } else {
-                priv_tx = queue->entries[0].priv_data;
-                data_addr = priv_tx->data;
-                data_dma = priv_tx->data_dma;
-
-                priv_tx->data = NULL;
-        }
+        struct queue_entry_priv_pci *entry_priv =
+            queue->entries[0].priv_data;
 
-        if (data_addr)
+        if (entry_priv->data)
                 pci_free_consistent(pci_dev, dma_size(queue),
-                                    data_addr, data_dma);
+                                    entry_priv->data, entry_priv->data_dma);
+        entry_priv->data = NULL;
 }
 
 int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
index 2b0ef17bba6e..37c851e442c1 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -91,42 +91,22 @@ rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
  * TX data handlers.
  */
 int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
-                            struct data_queue *queue, struct sk_buff *skb,
-                            struct ieee80211_tx_control *control);
+                            struct data_queue *queue, struct sk_buff *skb);
 
 /**
- * struct queue_entry_priv_pci_rx: Per RX entry PCI specific information
- *
- * @desc: Pointer to device descriptor.
- * @desc_dma: DMA pointer to @desc.
- * @data: Pointer to device's entry memory.
- * @data_dma: DMA pointer to &data.
- */
-struct queue_entry_priv_pci_rx {
-        __le32 *desc;
-        dma_addr_t desc_dma;
-
-        void *data;
-        dma_addr_t data_dma;
-};
-
-/**
- * struct queue_entry_priv_pci_tx: Per TX entry PCI specific information
+ * struct queue_entry_priv_pci: Per entry PCI specific information
  *
  * @desc: Pointer to device descriptor
- * @desc_dma: DMA pointer to @desc.
+ * @desc_dma: DMA pointer to &desc.
  * @data: Pointer to device's entry memory.
  * @data_dma: DMA pointer to &data.
- * @control: mac80211 control structure used to transmit data.
  */
-struct queue_entry_priv_pci_tx {
+struct queue_entry_priv_pci {
         __le32 *desc;
         dma_addr_t desc_dma;
 
         void *data;
         dma_addr_t data_dma;
-
-        struct ieee80211_tx_control control;
 };
 
 /**
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index e5b861f8416d..e69ef4b19239 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -29,12 +29,171 @@
 #include "rt2x00.h"
 #include "rt2x00lib.h"
 
+void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
+                                      struct txentry_desc *txdesc)
+{
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
+        struct ieee80211_rate *rate =
+            ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
+        const struct rt2x00_rate *hwrate;
+        unsigned int data_length;
+        unsigned int duration;
+        unsigned int residual;
+        u16 frame_control;
+
+        memset(txdesc, 0, sizeof(*txdesc));
+
+        /*
+         * Initialize information from queue
+         */
+        txdesc->queue = entry->queue->qid;
+        txdesc->cw_min = entry->queue->cw_min;
+        txdesc->cw_max = entry->queue->cw_max;
+        txdesc->aifs = entry->queue->aifs;
+
+        /* Data length should be extended with 4 bytes for CRC */
+        data_length = entry->skb->len + 4;
+
+        /*
+         * Read required fields from ieee80211 header.
+         */
+        frame_control = le16_to_cpu(hdr->frame_control);
+
+        /*
+         * Check whether this frame is to be acked.
+         */
+        if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
+                __set_bit(ENTRY_TXD_ACK, &txdesc->flags);
+
+        /*
+         * Check if this is a RTS/CTS frame
+         */
+        if (is_rts_frame(frame_control) || is_cts_frame(frame_control)) {
+                __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
+                if (is_rts_frame(frame_control))
+                        __set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
+                else
+                        __set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
+                if (tx_info->control.rts_cts_rate_idx >= 0)
+                        rate =
+                            ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
+        }
+
+        /*
+         * Determine retry information.
+         */
+        txdesc->retry_limit = tx_info->control.retry_limit;
+        if (tx_info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
+                __set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
+
+        /*
+         * Check if more fragments are pending
+         */
+        if (ieee80211_get_morefrag(hdr)) {
+                __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
+                __set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
+        }
+
+        /*
+         * Beacons and probe responses require the tsf timestamp
+         * to be inserted into the frame.
+         */
+        if (txdesc->queue == QID_BEACON || is_probe_resp(frame_control))
+                __set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->flags);
+
+        /*
+         * Determine with what IFS priority this frame should be send.
+         * Set ifs to IFS_SIFS when the this is not the first fragment,
+         * or this fragment came after RTS/CTS.
+         */
+        if (test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
+                txdesc->ifs = IFS_SIFS;
+        } else if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) {
+                __set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
+                txdesc->ifs = IFS_BACKOFF;
+        } else {
+                txdesc->ifs = IFS_SIFS;
+        }
+
+        /*
+         * PLCP setup
+         * Length calculation depends on OFDM/CCK rate.
+         */
+        hwrate = rt2x00_get_rate(rate->hw_value);
+        txdesc->signal = hwrate->plcp;
+        txdesc->service = 0x04;
+
+        if (hwrate->flags & DEV_RATE_OFDM) {
+                __set_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags);
+
+                txdesc->length_high = (data_length >> 6) & 0x3f;
+                txdesc->length_low = data_length & 0x3f;
+        } else {
+                /*
+                 * Convert length to microseconds.
+                 */
+                residual = get_duration_res(data_length, hwrate->bitrate);
+                duration = get_duration(data_length, hwrate->bitrate);
+
+                if (residual != 0) {
+                        duration++;
+
+                        /*
+                         * Check if we need to set the Length Extension
+                         */
+                        if (hwrate->bitrate == 110 && residual <= 30)
+                                txdesc->service |= 0x80;
+                }
+
+                txdesc->length_high = (duration >> 8) & 0xff;
+                txdesc->length_low = duration & 0xff;
+
+                /*
+                 * When preamble is enabled we should set the
+                 * preamble bit for the signal.
+                 */
+                if (rt2x00_get_rate_preamble(rate->hw_value))
+                        txdesc->signal |= 0x08;
+        }
+}
+EXPORT_SYMBOL_GPL(rt2x00queue_create_tx_descriptor);
+
+void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
+                                     struct txentry_desc *txdesc)
+{
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+
+        rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, entry->skb, txdesc);
+
+        /*
+         * All processing on the frame has been completed, this means
+         * it is now ready to be dumped to userspace through debugfs.
+         */
+        rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TX, entry->skb);
+
+        /*
+         * We are done writing the frame to the queue entry,
+         * also kick the queue in case the correct flags are set,
+         * note that this will automatically filter beacons and
+         * RTS/CTS frames since those frames don't have this flag
+         * set.
+         */
+        if (rt2x00dev->ops->lib->kick_tx_queue &&
+            !(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
+                rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev,
+                                                   entry->queue->qid);
+}
+EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);
+
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
                                          const enum data_queue_qid queue)
 {
         int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
 
-        if (queue < rt2x00dev->hw->queues && rt2x00dev->tx)
+        if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
                 return &rt2x00dev->tx[queue];
 
         if (!rt2x00dev->bcn)
@@ -255,11 +414,11 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
         /*
          * We need the following queues:
          * RX: 1
-         * TX: hw->queues
+         * TX: ops->tx_queues
          * Beacon: 1
          * Atim: 1 (if required)
          */
-        rt2x00dev->data_queues = 2 + rt2x00dev->hw->queues + req_atim;
+        rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
 
         queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
         if (!queue) {
@@ -272,7 +431,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
          */
         rt2x00dev->rx = queue;
         rt2x00dev->tx = &queue[1];
-        rt2x00dev->bcn = &queue[1 + rt2x00dev->hw->queues];
+        rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
 
         /*
          * Initialize queue parameters.
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index d1707a7ca41f..4d00ced14cc7 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -80,19 +80,6 @@ enum data_queue_qid {
 };
 
 /**
- * mac80211_queue_to_qid - Convert mac80211 queue to rt2x00 qid
- * @queue: mac80211 queue.
- */
-static inline enum data_queue_qid mac80211_queue_to_qid(unsigned int queue)
-{
-        /* Regular TX queues are mapped directly */
-        if (queue < 4)
-                return queue;
-        WARN_ON(1);
-        return QID_OTHER;
-}
-
-/**
  * enum skb_frame_desc_flags: Flags for &struct skb_frame_desc
  *
  * @FRAME_DESC_DRIVER_GENERATED: Frame was generated inside driver
@@ -105,11 +92,10 @@ enum skb_frame_desc_flags {
 /**
  * struct skb_frame_desc: Descriptor information for the skb buffer
  *
- * This structure is placed over the skb->cb array, this means that
- * this structure should not exceed the size of that array (48 bytes).
+ * This structure is placed over the driver_data array, this means that
+ * this structure should not exceed the size of that array (40 bytes).
  *
  * @flags: Frame flags, see &enum skb_frame_desc_flags.
- * @frame_type: Frame type, see &enum rt2x00_dump_type.
  * @data: Pointer to data part of frame (Start of ieee80211 header).
  * @desc: Pointer to descriptor part of the frame.
  *      Note that this pointer could point to something outside
@@ -121,21 +107,24 @@ enum skb_frame_desc_flags {
 struct skb_frame_desc {
         unsigned int flags;
 
-        unsigned int frame_type;
+        unsigned short data_len;
+        unsigned short desc_len;
 
         void *data;
         void *desc;
 
-        unsigned int data_len;
-        unsigned int desc_len;
-
         struct queue_entry *entry;
 };
 
+/**
+ * get_skb_frame_desc - Obtain the rt2x00 frame descriptor from a sk_buff.
+ * @skb: &struct sk_buff from where we obtain the &struct skb_frame_desc
+ */
 static inline struct skb_frame_desc* get_skb_frame_desc(struct sk_buff *skb)
 {
-        BUILD_BUG_ON(sizeof(struct skb_frame_desc) > sizeof(skb->cb));
-        return (struct skb_frame_desc *)&skb->cb[0];
+        BUILD_BUG_ON(sizeof(struct skb_frame_desc) >
+                     IEEE80211_TX_INFO_DRIVER_DATA_SIZE);
+        return (struct skb_frame_desc *)&IEEE80211_SKB_CB(skb)->driver_data;
 }
 
 /**
@@ -171,18 +160,32 @@ struct rxdone_entry_desc {
 };
 
 /**
+ * enum txdone_entry_desc_flags: Flags for &struct txdone_entry_desc
+ *
+ * @TXDONE_UNKNOWN: Hardware could not determine success of transmission.
+ * @TXDONE_SUCCESS: Frame was successfully send
+ * @TXDONE_FAILURE: Frame was not successfully send
+ * @TXDONE_EXCESSIVE_RETRY: In addition to &TXDONE_FAILURE, the
+ *      frame transmission failed due to excessive retries.
+ */
+enum txdone_entry_desc_flags {
+        TXDONE_UNKNOWN = 1 << 0,
+        TXDONE_SUCCESS = 1 << 1,
+        TXDONE_FAILURE = 1 << 2,
+        TXDONE_EXCESSIVE_RETRY = 1 << 3,
+};
+
+/**
  * struct txdone_entry_desc: TX done entry descriptor
  *
  * Summary of information that has been read from the TX frame descriptor
  * after the device is done with transmission.
  *
- * @control: Control structure which was used to transmit the frame.
- * @status: TX status (See &enum tx_status).
+ * @flags: TX done flags (See &enum txdone_entry_desc_flags).
  * @retry: Retry count.
  */
 struct txdone_entry_desc {
-        struct ieee80211_tx_control *control;
-        int status;
+        unsigned long flags;
         int retry;
 };
 
@@ -190,19 +193,25 @@ struct txdone_entry_desc {
  * enum txentry_desc_flags: Status flags for TX entry descriptor
  *
  * @ENTRY_TXD_RTS_FRAME: This frame is a RTS frame.
+ * @ENTRY_TXD_CTS_FRAME: This frame is a CTS-to-self frame.
  * @ENTRY_TXD_OFDM_RATE: This frame is send out with an OFDM rate.
+ * @ENTRY_TXD_FIRST_FRAGMENT: This is the first frame.
  * @ENTRY_TXD_MORE_FRAG: This frame is followed by another fragment.
  * @ENTRY_TXD_REQ_TIMESTAMP: Require timestamp to be inserted.
  * @ENTRY_TXD_BURST: This frame belongs to the same burst event.
  * @ENTRY_TXD_ACK: An ACK is required for this frame.
+ * @ENTRY_TXD_RETRY_MODE: When set, the long retry count is used.
  */
 enum txentry_desc_flags {
         ENTRY_TXD_RTS_FRAME,
+        ENTRY_TXD_CTS_FRAME,
         ENTRY_TXD_OFDM_RATE,
+        ENTRY_TXD_FIRST_FRAGMENT,
         ENTRY_TXD_MORE_FRAG,
         ENTRY_TXD_REQ_TIMESTAMP,
         ENTRY_TXD_BURST,
         ENTRY_TXD_ACK,
+        ENTRY_TXD_RETRY_MODE,
 };
 
 /**
@@ -216,6 +225,7 @@ enum txentry_desc_flags {
  * @length_low: PLCP length low word.
  * @signal: PLCP signal.
  * @service: PLCP service.
+ * @retry_limit: Max number of retries.
  * @aifs: AIFS value.
  * @ifs: IFS value.
  * @cw_min: cwmin value.
@@ -231,10 +241,11 @@ struct txentry_desc {
         u16 signal;
         u16 service;
 
-        int aifs;
-        int ifs;
-        int cw_min;
-        int cw_max;
+        short retry_limit;
+        short aifs;
+        short ifs;
+        short cw_min;
+        short cw_max;
 };
 
 /**
@@ -378,7 +389,7 @@ struct data_queue_desc {
  * the end of the TX queue array.
  */
 #define tx_queue_end(__dev) \
-        &(__dev)->tx[(__dev)->hw->queues]
+        &(__dev)->tx[(__dev)->ops->tx_queues]
 
 /**
  * queue_loop - Loop through the queues within a specific range (HELPER MACRO).
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
index 0325bed2fbf5..3f255df58b78 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -27,17 +27,6 @@
 #define RT2X00REG_H
 
 /*
- * TX result flags.
- */
-enum tx_status {
-        TX_SUCCESS = 0,
-        TX_SUCCESS_RETRY = 1,
-        TX_FAIL_RETRY = 2,
-        TX_FAIL_INVALID = 3,
-        TX_FAIL_OTHER = 4,
-};
-
-/*
  * Antenna values
  */
 enum antenna {
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 98aafc2d584a..52d12fdc0ccf 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -129,9 +129,9 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 {
         struct queue_entry *entry = (struct queue_entry *)urb->context;
         struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-        struct queue_entry_priv_usb_tx *priv_tx = entry->priv_data;
         struct txdone_entry_desc txdesc;
         __le32 *txd = (__le32 *)entry->skb->data;
+        enum data_queue_qid qid = skb_get_queue_mapping(entry->skb);
         u32 word;
 
         if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
@@ -147,10 +147,18 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 
         /*
          * Obtain the status about this packet.
+         * Note that when the status is 0 it does not mean the
+         * frame was send out correctly. It only means the frame
+         * was succesfully pushed to the hardware, we have no
+         * way to determine the transmission status right now.
+         * (Only indirectly by looking at the failed TX counters
+         * in the register).
          */
-        txdesc.status = !urb->status ? TX_SUCCESS : TX_FAIL_RETRY;
+        if (!urb->status)
+                __set_bit(TXDONE_UNKNOWN, &txdesc.flags);
+        else
+                __set_bit(TXDONE_FAILURE, &txdesc.flags);
         txdesc.retry = 0;
-        txdesc.control = &priv_tx->control;
 
         rt2x00lib_txdone(entry, &txdesc);
 
@@ -166,17 +174,17 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
          * is reenabled when the txdone handler has finished.
          */
         if (!rt2x00queue_full(entry->queue))
-                ieee80211_wake_queue(rt2x00dev->hw, priv_tx->control.queue);
+                ieee80211_wake_queue(rt2x00dev->hw, qid);
 }
 
 int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
-                            struct data_queue *queue, struct sk_buff *skb,
-                            struct ieee80211_tx_control *control)
+                            struct data_queue *queue, struct sk_buff *skb)
 {
         struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
         struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
-        struct queue_entry_priv_usb_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         u32 length;
 
         if (rt2x00queue_full(queue))
@@ -191,6 +199,14 @@ int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
         }
 
         /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        entry->skb = skb;
+        rt2x00queue_create_tx_descriptor(entry, &txdesc);
+
+        /*
          * Add the descriptor in front of the skb.
          */
         skb_push(skb, queue->desc_size);
@@ -200,14 +216,14 @@ int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
          * Fill in skb descriptor
          */
         skbdesc = get_skb_frame_desc(skb);
+        memset(skbdesc, 0, sizeof(*skbdesc));
         skbdesc->data = skb->data + queue->desc_size;
         skbdesc->data_len = skb->len - queue->desc_size;
         skbdesc->desc = skb->data;
         skbdesc->desc_len = queue->desc_size;
         skbdesc->entry = entry;
 
-        memcpy(&priv_tx->control, control, sizeof(priv_tx->control));
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+        rt2x00queue_write_tx_descriptor(entry, &txdesc);
 
         /*
          * USB devices cannot blindly pass the skb->len as the
@@ -220,9 +236,9 @@ int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
          * Initialize URB and send the frame to the device.
          */
         __set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-        usb_fill_bulk_urb(priv_tx->urb, usb_dev, usb_sndbulkpipe(usb_dev, 1),
+        usb_fill_bulk_urb(entry_priv->urb, usb_dev, usb_sndbulkpipe(usb_dev, 1),
                           skb->data, length, rt2x00usb_interrupt_txdone, entry);
-        usb_submit_urb(priv_tx->urb, GFP_ATOMIC);
+        usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
 
         rt2x00queue_index_inc(queue, Q_INDEX);
 
@@ -237,22 +253,35 @@ static struct sk_buff* rt2x00usb_alloc_rxskb(struct data_queue *queue)
 {
         struct sk_buff *skb;
         unsigned int frame_size;
+        unsigned int reserved_size;
 
         /*
-         * As alignment we use 2 and not NET_IP_ALIGN because we need
-         * to be sure we have 2 bytes room in the head. (NET_IP_ALIGN
-         * can be 0 on some hardware). We use these 2 bytes for frame
-         * alignment later, we assume that the chance that
-         * header_size % 4 == 2 is bigger then header_size % 2 == 0
-         * and thus optimize alignment by reserving the 2 bytes in
-         * advance.
+         * The frame size includes descriptor size, because the
+         * hardware directly receive the frame into the skbuffer.
          */
         frame_size = queue->data_size + queue->desc_size;
-        skb = dev_alloc_skb(queue->desc_size + frame_size + 2);
+
+        /*
+         * For the allocation we should keep a few things in mind:
+         * 1) 4byte alignment of 802.11 payload
+         *
+         * For (1) we need at most 4 bytes to guarentee the correct
+         * alignment. We are going to optimize the fact that the chance
+         * that the 802.11 header_size % 4 == 2 is much bigger then
+         * anything else. However since we need to move the frame up
+         * to 3 bytes to the front, which means we need to preallocate
+         * 6 bytes.
+         */
+        reserved_size = 6;
+
+        /*
+         * Allocate skbuffer.
+         */
+        skb = dev_alloc_skb(frame_size + reserved_size);
         if (!skb)
                 return NULL;
 
-        skb_reserve(skb, queue->desc_size + 2);
+        skb_reserve(skb, reserved_size);
         skb_put(skb, frame_size);
 
         return skb;
@@ -265,7 +294,8 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
         struct sk_buff *skb;
         struct skb_frame_desc *skbdesc;
         struct rxdone_entry_desc rxdesc;
-        int header_size;
+        unsigned int header_size;
+        unsigned int align;
 
         if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
             !test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
@@ -289,19 +319,29 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
         memset(&rxdesc, 0, sizeof(rxdesc));
         rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
 
+        header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
+
         /*
          * The data behind the ieee80211 header must be
-         * aligned on a 4 byte boundary.
+         * aligned on a 4 byte boundary. We already reserved
+         * 2 bytes for header_size % 4 == 2 optimization.
+         * To determine the number of bytes which the data
+         * should be moved to the left, we must add these
+         * 2 bytes to the header_size.
          */
-        header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
-        if (header_size % 4 == 0) {
-                skb_push(entry->skb, 2);
-                memmove(entry->skb->data, entry->skb->data + 2,
-                        entry->skb->len - 2);
-                skbdesc->data = entry->skb->data;
-                skb_trim(entry->skb,entry->skb->len - 2);
+        align = (header_size + 2) % 4;
+
+        if (align) {
+                skb_push(entry->skb, align);
+                /* Move entire frame in 1 command */
+                memmove(entry->skb->data, entry->skb->data + align,
+                        rxdesc.size);
         }
 
+        /* Update data pointers, trim buffer to correct size */
+        skbdesc->data = entry->skb->data;
+        skb_trim(entry->skb, rxdesc.size);
+
         /*
          * Allocate a new sk buffer to replace the current one.
          * If allocation fails, we should drop the current frame
@@ -338,10 +378,8 @@ skip_entry:
  */
 void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-        struct queue_entry_priv_usb_rx *priv_rx;
-        struct queue_entry_priv_usb_tx *priv_tx;
-        struct queue_entry_priv_usb_bcn *priv_bcn;
-        struct data_queue *queue;
+        struct queue_entry_priv_usb *entry_priv;
+        struct queue_entry_priv_usb_bcn *bcn_priv;
         unsigned int i;
 
         rt2x00usb_vendor_request_sw(rt2x00dev, USB_RX_CONTROL, 0, 0,
@@ -351,31 +389,17 @@ void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
          * Cancel all queues.
          */
         for (i = 0; i < rt2x00dev->rx->limit; i++) {
-                priv_rx = rt2x00dev->rx->entries[i].priv_data;
-                usb_kill_urb(priv_rx->urb);
-        }
-
-        tx_queue_for_each(rt2x00dev, queue) {
-                for (i = 0; i < queue->limit; i++) {
-                        priv_tx = queue->entries[i].priv_data;
-                        usb_kill_urb(priv_tx->urb);
-                }
+                entry_priv = rt2x00dev->rx->entries[i].priv_data;
+                usb_kill_urb(entry_priv->urb);
         }
 
+        /*
+         * Kill guardian urb.
+         */
         for (i = 0; i < rt2x00dev->bcn->limit; i++) {
-                priv_bcn = rt2x00dev->bcn->entries[i].priv_data;
-                usb_kill_urb(priv_bcn->urb);
-
-                if (priv_bcn->guardian_urb)
-                        usb_kill_urb(priv_bcn->guardian_urb);
-        }
-
-        if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-                return;
-
-        for (i = 0; i < rt2x00dev->bcn[1].limit; i++) {
-                priv_tx = rt2x00dev->bcn[1].entries[i].priv_data;
-                usb_kill_urb(priv_tx->urb);
+                bcn_priv = rt2x00dev->bcn->entries[i].priv_data;
+                if (bcn_priv->guardian_urb)
+                        usb_kill_urb(bcn_priv->guardian_urb);
         }
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
@@ -387,15 +411,15 @@ void rt2x00usb_init_rxentry(struct rt2x00_dev *rt2x00dev,
                             struct queue_entry *entry)
 {
         struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
-        struct queue_entry_priv_usb_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
 
-        usb_fill_bulk_urb(priv_rx->urb, usb_dev,
+        usb_fill_bulk_urb(entry_priv->urb, usb_dev,
                           usb_rcvbulkpipe(usb_dev, 1),
                           entry->skb->data, entry->skb->len,
                           rt2x00usb_interrupt_rxdone, entry);
 
         __set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-        usb_submit_urb(priv_rx->urb, GFP_ATOMIC);
+        usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_init_rxentry);
 
@@ -409,38 +433,31 @@ EXPORT_SYMBOL_GPL(rt2x00usb_init_txentry);
 static int rt2x00usb_alloc_urb(struct rt2x00_dev *rt2x00dev,
                                struct data_queue *queue)
 {
-        struct queue_entry_priv_usb_rx *priv_rx;
-        struct queue_entry_priv_usb_tx *priv_tx;
-        struct queue_entry_priv_usb_bcn *priv_bcn;
-        struct urb *urb;
-        unsigned int guardian =
-            test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags);
+        struct queue_entry_priv_usb *entry_priv;
+        struct queue_entry_priv_usb_bcn *bcn_priv;
         unsigned int i;
 
+        for (i = 0; i < queue->limit; i++) {
+                entry_priv = queue->entries[i].priv_data;
+                entry_priv->urb = usb_alloc_urb(0, GFP_KERNEL);
+                if (!entry_priv->urb)
+                        return -ENOMEM;
+        }
+
         /*
-         * Allocate the URB's
+         * If this is not the beacon queue or
+         * no guardian byte was required for the beacon,
+         * then we are done.
          */
+        if (rt2x00dev->bcn != queue ||
+            !test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags))
+                return 0;
+
         for (i = 0; i < queue->limit; i++) {
-                urb = usb_alloc_urb(0, GFP_KERNEL);
-                if (!urb)
+                bcn_priv = queue->entries[i].priv_data;
+                bcn_priv->guardian_urb = usb_alloc_urb(0, GFP_KERNEL);
+                if (!bcn_priv->guardian_urb)
                         return -ENOMEM;
-
-                if (queue->qid == QID_RX) {
-                        priv_rx = queue->entries[i].priv_data;
-                        priv_rx->urb = urb;
-                } else if (queue->qid == QID_MGMT && guardian) {
-                        priv_bcn = queue->entries[i].priv_data;
-                        priv_bcn->urb = urb;
-
-                        urb = usb_alloc_urb(0, GFP_KERNEL);
-                        if (!urb)
-                                return -ENOMEM;
-
-                        priv_bcn->guardian_urb = urb;
-                } else {
-                        priv_tx = queue->entries[i].priv_data;
-                        priv_tx->urb = urb;
-                }
         }
 
         return 0;
@@ -449,38 +466,35 @@ static int rt2x00usb_alloc_urb(struct rt2x00_dev *rt2x00dev,
 static void rt2x00usb_free_urb(struct rt2x00_dev *rt2x00dev,
                                struct data_queue *queue)
 {
-        struct queue_entry_priv_usb_rx *priv_rx;
-        struct queue_entry_priv_usb_tx *priv_tx;
-        struct queue_entry_priv_usb_bcn *priv_bcn;
-        struct urb *urb;
-        unsigned int guardian =
-            test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags);
+        struct queue_entry_priv_usb *entry_priv;
+        struct queue_entry_priv_usb_bcn *bcn_priv;
         unsigned int i;
 
         if (!queue->entries)
                 return;
 
         for (i = 0; i < queue->limit; i++) {
-                if (queue->qid == QID_RX) {
-                        priv_rx = queue->entries[i].priv_data;
-                        urb = priv_rx->urb;
-                } else if (queue->qid == QID_MGMT && guardian) {
-                        priv_bcn = queue->entries[i].priv_data;
-
-                        usb_kill_urb(priv_bcn->guardian_urb);
-                        usb_free_urb(priv_bcn->guardian_urb);
-
-                        urb = priv_bcn->urb;
-                } else {
-                        priv_tx = queue->entries[i].priv_data;
-                        urb = priv_tx->urb;
-                }
-
-                usb_kill_urb(urb);
-                usb_free_urb(urb);
+                entry_priv = queue->entries[i].priv_data;
+                usb_kill_urb(entry_priv->urb);
+                usb_free_urb(entry_priv->urb);
                 if (queue->entries[i].skb)
                         kfree_skb(queue->entries[i].skb);
         }
+
+        /*
+         * If this is not the beacon queue or
+         * no guardian byte was required for the beacon,
+         * then we are done.
+         */
+        if (rt2x00dev->bcn != queue ||
+            !test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags))
+                return;
+
+        for (i = 0; i < queue->limit; i++) {
+                bcn_priv = queue->entries[i].priv_data;
+                usb_kill_urb(bcn_priv->guardian_urb);
+                usb_free_urb(bcn_priv->guardian_urb);
+        }
 }
 
 int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev)
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
index 4da9eb376ebd..26f53f868af6 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -216,47 +216,31 @@ void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev);
  * TX data handlers.
  */
 int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
-                            struct data_queue *queue, struct sk_buff *skb,
-                            struct ieee80211_tx_control *control);
+                            struct data_queue *queue, struct sk_buff *skb);
 
 /**
- * struct queue_entry_priv_usb_rx: Per RX entry USB specific information
+ * struct queue_entry_priv_usb: Per entry USB specific information
  *
  * @urb: Urb structure used for device communication.
  */
-struct queue_entry_priv_usb_rx {
+struct queue_entry_priv_usb {
         struct urb *urb;
 };
 
 /**
- * struct queue_entry_priv_usb_tx: Per TX entry USB specific information
+ * struct queue_entry_priv_usb_bcn: Per TX entry USB specific information
  *
- * @urb: Urb structure used for device communication.
- * @control: mac80211 control structure used to transmit data.
- */
-struct queue_entry_priv_usb_tx {
-        struct urb *urb;
-
-        struct ieee80211_tx_control control;
-};
-
-/**
- * struct queue_entry_priv_usb_tx: Per TX entry USB specific information
- *
- * The first section should match &struct queue_entry_priv_usb_tx exactly.
+ * The first section should match &struct queue_entry_priv_usb exactly.
  * rt2500usb can use this structure to send a guardian byte when working
  * with beacons.
  *
  * @urb: Urb structure used for device communication.
- * @control: mac80211 control structure used to transmit data.
  * @guardian_data: Set to 0, used for sending the guardian data.
  * @guardian_urb: Urb structure used to send the guardian data.
  */
 struct queue_entry_priv_usb_bcn {
         struct urb *urb;
 
-        struct ieee80211_tx_control control;
-
         unsigned int guardian_data;
         struct urb *guardian_urb;
 };
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index edddbf35bbab..e13ed5ced26e 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -1018,49 +1018,34 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
 static void rt61pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
                                  struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_rx->desc, 5, &word);
+        rt2x00_desc_read(entry_priv->desc, 5, &word);
         rt2x00_set_field32(&word, RXD_W5_BUFFER_PHYSICAL_ADDRESS,
-                           priv_rx->data_dma);
-        rt2x00_desc_write(priv_rx->desc, 5, word);
+                           entry_priv->data_dma);
+        rt2x00_desc_write(entry_priv->desc, 5, word);
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-        rt2x00_desc_write(priv_rx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt61pci_init_txentry(struct rt2x00_dev *rt2x00dev,
                                  struct queue_entry *entry)
 {
-        struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word;
 
-        rt2x00_desc_read(priv_tx->desc, 1, &word);
-        rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
-        rt2x00_desc_write(priv_tx->desc, 1, word);
-
-        rt2x00_desc_read(priv_tx->desc, 5, &word);
-        rt2x00_set_field32(&word, TXD_W5_PID_TYPE, entry->queue->qid);
-        rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE, entry->entry_idx);
-        rt2x00_desc_write(priv_tx->desc, 5, word);
-
-        rt2x00_desc_read(priv_tx->desc, 6, &word);
-        rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
-                           priv_tx->data_dma);
-        rt2x00_desc_write(priv_tx->desc, 6, word);
-
-        rt2x00_desc_read(priv_tx->desc, 0, &word);
+        rt2x00_desc_read(entry_priv->desc, 0, &word);
         rt2x00_set_field32(&word, TXD_W0_VALID, 0);
         rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-        rt2x00_desc_write(priv_tx->desc, 0, word);
+        rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-        struct queue_entry_priv_pci_rx *priv_rx;
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         u32 reg;
 
         /*
@@ -1082,28 +1067,28 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
                            rt2x00dev->tx[0].desc_size / 4);
         rt2x00pci_register_write(rt2x00dev, TX_RING_CSR1, reg);
 
-        priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
         rt2x00_set_field32(&reg, AC0_BASE_CSR_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, AC0_BASE_CSR, reg);
 
-        priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
         rt2x00_set_field32(&reg, AC1_BASE_CSR_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, AC1_BASE_CSR, reg);
 
-        priv_tx = rt2x00dev->tx[2].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
         rt2x00_set_field32(&reg, AC2_BASE_CSR_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, AC2_BASE_CSR, reg);
 
-        priv_tx = rt2x00dev->tx[3].entries[0].priv_data;
+        entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
         rt2x00_set_field32(&reg, AC3_BASE_CSR_RING_REGISTER,
-                           priv_tx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, AC3_BASE_CSR, reg);
 
         rt2x00pci_register_read(rt2x00dev, RX_RING_CSR, &reg);
@@ -1113,10 +1098,10 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
         rt2x00_set_field32(&reg, RX_RING_CSR_RXD_WRITEBACK_SIZE, 4);
         rt2x00pci_register_write(rt2x00dev, RX_RING_CSR, reg);
 
-        priv_rx = rt2x00dev->rx->entries[0].priv_data;
+        entry_priv = rt2x00dev->rx->entries[0].priv_data;
         rt2x00pci_register_read(rt2x00dev, RX_BASE_CSR, &reg);
         rt2x00_set_field32(&reg, RX_BASE_CSR_RING_REGISTER,
-                           priv_rx->desc_dma);
+                           entry_priv->desc_dma);
         rt2x00pci_register_write(rt2x00dev, RX_BASE_CSR, reg);
 
         rt2x00pci_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
@@ -1526,10 +1511,10 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                     struct sk_buff *skb,
-                                    struct txentry_desc *txdesc,
-                                    struct ieee80211_tx_control *control)
+                                    struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+        struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
         __le32 *txd = skbdesc->desc;
         u32 word;
 
@@ -1543,6 +1528,7 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_set_field32(&word, TXD_W1_CWMAX, txdesc->cw_max);
         rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, IEEE80211_HEADER);
         rt2x00_set_field32(&word, TXD_W1_HW_SEQUENCE, 1);
+        rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
         rt2x00_desc_write(txd, 1, word);
 
         rt2x00_desc_read(txd, 2, &word);
@@ -1553,11 +1539,19 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
         rt2x00_desc_write(txd, 2, word);
 
         rt2x00_desc_read(txd, 5, &word);
+        rt2x00_set_field32(&word, TXD_W5_PID_TYPE, skbdesc->entry->queue->qid);
+        rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE,
+                           skbdesc->entry->entry_idx);
         rt2x00_set_field32(&word, TXD_W5_TX_POWER,
                            TXPOWER_TO_DEV(rt2x00dev->tx_power));
         rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
         rt2x00_desc_write(txd, 5, word);
 
+        rt2x00_desc_read(txd, 6, &word);
+        rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
+                           entry_priv->data_dma);
+        rt2x00_desc_write(txd, 6, word);
+
         if (skbdesc->desc_len > TXINFO_SIZE) {
                 rt2x00_desc_read(txd, 11, &word);
                 rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skbdesc->data_len);
@@ -1577,8 +1571,7 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                            test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
         rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-                           !!(control->flags &
-                              IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
         rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
         rt2x00_set_field32(&word, TXD_W0_BURST,
@@ -1667,14 +1660,13 @@ static int rt61pci_agc_to_rssi(struct rt2x00_dev *rt2x00dev, int rxd_w1)
 static void rt61pci_fill_rxdone(struct queue_entry *entry,
                                 struct rxdone_entry_desc *rxdesc)
 {
-        struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+        struct queue_entry_priv_pci *entry_priv = entry->priv_data;
         u32 word0;
         u32 word1;
 
-        rt2x00_desc_read(priv_rx->desc, 0, &word0);
-        rt2x00_desc_read(priv_rx->desc, 1, &word1);
+        rt2x00_desc_read(entry_priv->desc, 0, &word0);
+        rt2x00_desc_read(entry_priv->desc, 1, &word1);
 
-        rxdesc->flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -1688,7 +1680,6 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
         rxdesc->rssi = rt61pci_agc_to_rssi(entry->queue->rt2x00dev, word1);
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-        rxdesc->dev_flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1703,7 +1694,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
         struct data_queue *queue;
         struct queue_entry *entry;
         struct queue_entry *entry_done;
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct queue_entry_priv_pci *entry_priv;
         struct txdone_entry_desc txdesc;
         u32 word;
         u32 reg;
@@ -1748,8 +1739,8 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
                         continue;
 
                 entry = &queue->entries[index];
-                priv_tx = entry->priv_data;
-                rt2x00_desc_read(priv_tx->desc, 0, &word);
+                entry_priv = entry->priv_data;
+                rt2x00_desc_read(entry_priv->desc, 0, &word);
 
                 if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
                     !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1764,7 +1755,8 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
                                 "TX status report missed for entry %d\n",
                                 entry_done->entry_idx);
 
-                        txdesc.status = TX_FAIL_OTHER;
+                        txdesc.flags = 0;
+                        __set_bit(TXDONE_UNKNOWN, &txdesc.flags);
                         txdesc.retry = 0;
 
                         rt2x00pci_txdone(rt2x00dev, entry_done, &txdesc);
@@ -1774,7 +1766,17 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
                 /*
                  * Obtain the status about this packet.
                  */
-                txdesc.status = rt2x00_get_field32(reg, STA_CSR4_TX_RESULT);
+                txdesc.flags = 0;
+                switch (rt2x00_get_field32(reg, STA_CSR4_TX_RESULT)) {
+                case 0: /* Success, maybe with retry */
+                        __set_bit(TXDONE_SUCCESS, &txdesc.flags);
+                        break;
+                case 6: /* Failure, excessive retries */
+                        __set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+                        /* Don't break, this is a failed frame! */
+                default: /* Failure */
+                        __set_bit(TXDONE_FAILURE, &txdesc.flags);
+                }
                 txdesc.retry = rt2x00_get_field32(reg, STA_CSR4_RETRY_COUNT);
 
                 rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
@@ -2248,7 +2250,6 @@ static void rt61pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
             IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
             IEEE80211_HW_SIGNAL_DBM;
         rt2x00dev->hw->extra_tx_headroom = 0;
-        rt2x00dev->hw->queues = 4;
 
         SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
         SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -2356,21 +2357,30 @@ static u64 rt61pci_get_tsf(struct ieee80211_hw *hw)
         return tsf;
 }
 
-static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                          struct ieee80211_tx_control *control)
+static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
-        struct rt2x00_intf *intf = vif_to_intf(control->vif);
-        struct queue_entry_priv_pci_tx *priv_tx;
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+        struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
+        struct queue_entry_priv_pci *entry_priv;
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         unsigned int beacon_base;
         u32 reg;
 
         if (unlikely(!intf->beacon))
                 return -ENOBUFS;
 
-        priv_tx = intf->beacon->priv_data;
-        memset(priv_tx->desc, 0, intf->beacon->queue->desc_size);
+        /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        intf->beacon->skb = skb;
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
+
+        entry_priv = intf->beacon->priv_data;
+        memset(entry_priv->desc, 0, intf->beacon->queue->desc_size);
 
         /*
          * Fill in skb descriptor
@@ -2380,7 +2390,7 @@ static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
         skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
         skbdesc->data = skb->data;
         skbdesc->data_len = skb->len;
-        skbdesc->desc = priv_tx->desc;
+        skbdesc->desc = entry_priv->desc;
         skbdesc->desc_len = intf->beacon->queue->desc_size;
         skbdesc->entry = intf->beacon;
 
@@ -2398,7 +2408,7 @@ static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
          * Write entire beacon with descriptor to register,
          * and kick the beacon generator.
          */
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
         beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
         rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
                                       skbdesc->desc, skbdesc->desc_len);
@@ -2457,21 +2467,21 @@ static const struct data_queue_desc rt61pci_queue_rx = {
         .entry_num              = RX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = RXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_rx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt61pci_queue_tx = {
         .entry_num              = TX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt61pci_queue_bcn = {
         .entry_num              = 4 * BEACON_ENTRIES,
         .data_size              = 0, /* No DMA required for beacons */
         .desc_size              = TXINFO_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_pci_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt61pci_ops = {
@@ -2480,6 +2490,7 @@ static const struct rt2x00_ops rt61pci_ops = {
         .max_ap_intf    = 4,
         .eeprom_size    = EEPROM_SIZE,
         .rf_size        = RF_SIZE,
+        .tx_queues      = NUM_TX_QUEUES,
         .rx             = &rt61pci_queue_rx,
         .tx             = &rt61pci_queue_tx,
         .bcn            = &rt61pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt61pci.h b/drivers/net/wireless/rt2x00/rt61pci.h
index 3511bba7ff65..c5a04b9329d2 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.h
+++ b/drivers/net/wireless/rt2x00/rt61pci.h
@@ -54,6 +54,11 @@
 #define RF_SIZE                         0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES                   4
+
+/*
  * PCI registers.
  */
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 51c5575ed02f..26c2e0a1a308 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -1255,8 +1255,7 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                                     struct sk_buff *skb,
-                                    struct txentry_desc *txdesc,
-                                    struct ieee80211_tx_control *control)
+                                    struct txentry_desc *txdesc)
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
         __le32 *txd = skbdesc->desc;
@@ -1301,8 +1300,7 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
                            test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
         rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-                           !!(control->flags &
-                              IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+                           test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
         rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
         rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
         rt2x00_set_field32(&word, TXD_W0_BURST2,
@@ -1405,25 +1403,26 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 {
         struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
         __le32 *rxd = (__le32 *)entry->skb->data;
-        unsigned int offset = entry->queue->desc_size + 2;
         u32 word0;
         u32 word1;
 
         /*
-         * Copy descriptor to the available headroom inside the skbuffer.
+         * Copy descriptor to the skb->cb array, this has 2 benefits:
+         * 1) Each descriptor word is 4 byte aligned.
+         * 2) Descriptor is safe  from moving of frame data in rt2x00usb.
          */
-        skb_push(entry->skb, offset);
-        memcpy(entry->skb->data, rxd, entry->queue->desc_size);
-        rxd = (__le32 *)entry->skb->data;
+        skbdesc->desc_len =
+            min_t(u16, entry->queue->desc_size, sizeof(entry->skb->cb));
+        memcpy(entry->skb->cb, rxd, skbdesc->desc_len);
+        skbdesc->desc = entry->skb->cb;
+        rxd = (__le32 *)skbdesc->desc;
 
         /*
-         * The descriptor is now aligned to 4 bytes and thus it is
-         * now safe to read it on all architectures.
+         * It is now safe to read the descriptor on all architectures.
          */
         rt2x00_desc_read(rxd, 0, &word0);
         rt2x00_desc_read(rxd, 1, &word1);
 
-        rxdesc->flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
                 rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -1437,25 +1436,18 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
         rxdesc->rssi = rt73usb_agc_to_rssi(entry->queue->rt2x00dev, word1);
         rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-        rxdesc->dev_flags = 0;
         if (rt2x00_get_field32(word0, RXD_W0_OFDM))
                 rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
         if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
                 rxdesc->dev_flags |= RXDONE_MY_BSS;
 
         /*
-         * Adjust the skb memory window to the frame boundaries.
+         * Set skb pointers, and update frame information.
          */
-        skb_pull(entry->skb, offset + entry->queue->desc_size);
+        skb_pull(entry->skb, entry->queue->desc_size);
         skb_trim(entry->skb, rxdesc->size);
-
-        /*
-         * Set descriptor and data pointer.
-         */
         skbdesc->data = entry->skb->data;
         skbdesc->data_len = rxdesc->size;
-        skbdesc->desc = rxd;
-        skbdesc->desc_len = entry->queue->desc_size;
 }
 
 /*
@@ -1833,7 +1825,6 @@ static void rt73usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
             IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
             IEEE80211_HW_SIGNAL_DBM;
         rt2x00dev->hw->extra_tx_headroom = TXD_DESC_SIZE;
-        rt2x00dev->hw->queues = 4;
 
         SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_usb(rt2x00dev)->dev);
         SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
@@ -1957,12 +1948,13 @@ static u64 rt73usb_get_tsf(struct ieee80211_hw *hw)
 #define rt73usb_get_tsf NULL
 #endif
 
-static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-                                 struct ieee80211_tx_control *control)
+static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct rt2x00_dev *rt2x00dev = hw->priv;
-        struct rt2x00_intf *intf = vif_to_intf(control->vif);
+        struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
+        struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
         struct skb_frame_desc *skbdesc;
+        struct txentry_desc txdesc;
         unsigned int beacon_base;
         u32 reg;
 
@@ -1970,6 +1962,14 @@ static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
                 return -ENOBUFS;
 
         /*
+         * Copy all TX descriptor information into txdesc,
+         * after that we are free to use the skb->cb array
+         * for our information.
+         */
+        intf->beacon->skb = skb;
+        rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
+
+        /*
          * Add the descriptor in front of the skb.
          */
         skb_push(skb, intf->beacon->queue->desc_size);
@@ -2001,7 +2001,7 @@ static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
          * Write entire beacon with descriptor to register,
          * and kick the beacon generator.
          */
-        rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+        rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
         beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
         rt2x00usb_vendor_request(rt2x00dev, USB_MULTI_WRITE,
                                  USB_VENDOR_REQUEST_OUT, beacon_base, 0,
@@ -2058,21 +2058,21 @@ static const struct data_queue_desc rt73usb_queue_rx = {
         .entry_num              = RX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = RXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_rx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt73usb_queue_tx = {
         .entry_num              = TX_ENTRIES,
         .data_size              = DATA_FRAME_SIZE,
         .desc_size              = TXD_DESC_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt73usb_queue_bcn = {
         .entry_num              = 4 * BEACON_ENTRIES,
         .data_size              = MGMT_FRAME_SIZE,
         .desc_size              = TXINFO_SIZE,
-        .priv_size              = sizeof(struct queue_entry_priv_usb_tx),
+        .priv_size              = sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct rt2x00_ops rt73usb_ops = {
@@ -2081,6 +2081,7 @@ static const struct rt2x00_ops rt73usb_ops = {
         .max_ap_intf    = 4,
         .eeprom_size    = EEPROM_SIZE,
         .rf_size        = RF_SIZE,
+        .tx_queues      = NUM_TX_QUEUES,
         .rx             = &rt73usb_queue_rx,
         .tx             = &rt73usb_queue_tx,
         .bcn            = &rt73usb_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.h b/drivers/net/wireless/rt2x00/rt73usb.h
index 06d687425fef..25cdcc9bf7c4 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.h
+++ b/drivers/net/wireless/rt2x00/rt73usb.h
@@ -54,6 +54,11 @@
 #define RF_SIZE                         0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES                   4
+
+/*
  * USB registers.
  */
 
diff --git a/drivers/net/wireless/rtl8180_dev.c b/drivers/net/wireless/rtl8180_dev.c
index c220998cee65..b7172a12c057 100644
--- a/drivers/net/wireless/rtl8180_dev.c
+++ b/drivers/net/wireless/rtl8180_dev.c
@@ -170,34 +170,29 @@ static void rtl8180_handle_tx(struct ieee80211_hw *dev, unsigned int prio)
         while (skb_queue_len(&ring->queue)) {
                 struct rtl8180_tx_desc *entry = &ring->desc[ring->idx];
                 struct sk_buff *skb;
-                struct ieee80211_tx_status status;
-                struct ieee80211_tx_control *control;
+                struct ieee80211_tx_info *info;
                 u32 flags = le32_to_cpu(entry->flags);
 
                 if (flags & RTL8180_TX_DESC_FLAG_OWN)
                         return;
 
-                memset(&status, 0, sizeof(status));
-
                 ring->idx = (ring->idx + 1) % ring->entries;
                 skb = __skb_dequeue(&ring->queue);
                 pci_unmap_single(priv->pdev, le32_to_cpu(entry->tx_buf),
                                  skb->len, PCI_DMA_TODEVICE);
 
-                control = *((struct ieee80211_tx_control **)skb->cb);
-                if (control)
-                        memcpy(&status.control, control, sizeof(*control));
-                kfree(control);
+                info = IEEE80211_SKB_CB(skb);
+                memset(&info->status, 0, sizeof(info->status));
 
-                if (!(status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
+                if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                         if (flags & RTL8180_TX_DESC_FLAG_TX_OK)
-                                status.flags = IEEE80211_TX_STATUS_ACK;
+                                info->flags |= IEEE80211_TX_STAT_ACK;
                         else
-                                status.excessive_retries = 1;
+                                info->status.excessive_retries = 1;
                 }
-                status.retry_count = flags & 0xFF;
+                info->status.retry_count = flags & 0xFF;
 
-                ieee80211_tx_status_irqsafe(dev, skb, &status);
+                ieee80211_tx_status_irqsafe(dev, skb);
                 if (ring->entries - skb_queue_len(&ring->queue) == 2)
                         ieee80211_wake_queue(dev, prio);
         }
@@ -238,9 +233,9 @@ static irqreturn_t rtl8180_interrupt(int irq, void *dev_id)
         return IRQ_HANDLED;
 }
 
-static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-                      struct ieee80211_tx_control *control)
+static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct rtl8180_priv *priv = dev->priv;
         struct rtl8180_tx_ring *ring;
         struct rtl8180_tx_desc *entry;
@@ -251,46 +246,40 @@ static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
         u16 plcp_len = 0;
         __le16 rts_duration = 0;
 
-        prio = control->queue;
+        prio = skb_get_queue_mapping(skb);
         ring = &priv->tx_ring[prio];
 
         mapping = pci_map_single(priv->pdev, skb->data,
                                  skb->len, PCI_DMA_TODEVICE);
 
-        BUG_ON(!control->tx_rate);
-
         tx_flags = RTL8180_TX_DESC_FLAG_OWN | RTL8180_TX_DESC_FLAG_FS |
                    RTL8180_TX_DESC_FLAG_LS |
-                   (control->tx_rate->hw_value << 24) | skb->len;
+                   (ieee80211_get_tx_rate(dev, info)->hw_value << 24) |
+                   skb->len;
 
         if (priv->r8185)
                 tx_flags |= RTL8180_TX_DESC_FLAG_DMA |
                             RTL8180_TX_DESC_FLAG_NO_ENC;
 
-        if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-                BUG_ON(!control->rts_cts_rate);
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
                 tx_flags |= RTL8180_TX_DESC_FLAG_RTS;
-                tx_flags |= control->rts_cts_rate->hw_value << 19;
-        } else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
-                BUG_ON(!control->rts_cts_rate);
+                tx_flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
+        } else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                 tx_flags |= RTL8180_TX_DESC_FLAG_CTS;
-                tx_flags |= control->rts_cts_rate->hw_value << 19;
+                tx_flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
         }
 
-        *((struct ieee80211_tx_control **) skb->cb) =
-                kmemdup(control, sizeof(*control), GFP_ATOMIC);
-
-        if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
                 rts_duration = ieee80211_rts_duration(dev, priv->vif, skb->len,
-                                                      control);
+                                                      info);
 
         if (!priv->r8185) {
                 unsigned int remainder;
 
                 plcp_len = DIV_ROUND_UP(16 * (skb->len + 4),
-                                        (control->tx_rate->bitrate * 2) / 10);
+                                (ieee80211_get_tx_rate(dev, info)->bitrate * 2) / 10);
                 remainder = (16 * (skb->len + 4)) %
-                            ((control->tx_rate->bitrate * 2) / 10);
+                            ((ieee80211_get_tx_rate(dev, info)->bitrate * 2) / 10);
                 if (remainder > 0 && remainder <= 6)
                         plcp_len |= 1 << 15;
         }
@@ -303,13 +292,13 @@ static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
         entry->plcp_len = cpu_to_le16(plcp_len);
         entry->tx_buf = cpu_to_le32(mapping);
         entry->frame_len = cpu_to_le32(skb->len);
-        entry->flags2 = control->alt_retry_rate != NULL ?
-                        control->alt_retry_rate->bitrate << 4 : 0;
-        entry->retry_limit = control->retry_limit;
+        entry->flags2 = info->control.alt_retry_rate_idx >= 0 ?
+                ieee80211_get_alt_retry_rate(dev, info)->bitrate << 4 : 0;
+        entry->retry_limit = info->control.retry_limit;
         entry->flags = cpu_to_le32(tx_flags);
         __skb_queue_tail(&ring->queue, skb);
         if (ring->entries - skb_queue_len(&ring->queue) < 2)
-                ieee80211_stop_queue(dev, control->queue);
+                ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
         spin_unlock_irqrestore(&priv->lock, flags);
 
         rtl818x_iowrite8(priv, &priv->map->TX_DMA_POLLING, (1 << (prio + 4)));
@@ -525,7 +514,6 @@ static void rtl8180_free_tx_ring(struct ieee80211_hw *dev, unsigned int prio)
 
                 pci_unmap_single(priv->pdev, le32_to_cpu(entry->tx_buf),
                                  skb->len, PCI_DMA_TODEVICE);
-                kfree(*((struct ieee80211_tx_control **) skb->cb));
                 kfree_skb(skb);
                 ring->idx = (ring->idx + 1) % ring->entries;
         }
diff --git a/drivers/net/wireless/rtl8187.h b/drivers/net/wireless/rtl8187.h
index 076d88b6db0e..a0cfb666de0e 100644
--- a/drivers/net/wireless/rtl8187.h
+++ b/drivers/net/wireless/rtl8187.h
@@ -44,12 +44,6 @@ struct rtl8187_rx_hdr {
         __le64 mac_time;
 } __attribute__((packed));
 
-struct rtl8187_tx_info {
-        struct ieee80211_tx_control *control;
-        struct urb *urb;
-        struct ieee80211_hw *dev;
-};
-
 struct rtl8187_tx_hdr {
         __le32 flags;
 #define RTL8187_TX_FLAG_NO_ENCRYPT      (1 << 15)
diff --git a/drivers/net/wireless/rtl8187_dev.c b/drivers/net/wireless/rtl8187_dev.c
index 23514456d373..0078c7e9918c 100644
--- a/drivers/net/wireless/rtl8187_dev.c
+++ b/drivers/net/wireless/rtl8187_dev.c
@@ -150,27 +150,22 @@ void rtl8187_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data)
 
 static void rtl8187_tx_cb(struct urb *urb)
 {
-        struct ieee80211_tx_status status;
         struct sk_buff *skb = (struct sk_buff *)urb->context;
-        struct rtl8187_tx_info *info = (struct rtl8187_tx_info *)skb->cb;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ieee80211_hw *hw = info->driver_data[0];
 
-        memset(&status, 0, sizeof(status));
-
-        usb_free_urb(info->urb);
-        if (info->control)
-                memcpy(&status.control, info->control, sizeof(status.control));
-        kfree(info->control);
+        usb_free_urb(info->driver_data[1]);
         skb_pull(skb, sizeof(struct rtl8187_tx_hdr));
-        status.flags |= IEEE80211_TX_STATUS_ACK;
-        ieee80211_tx_status_irqsafe(info->dev, skb, &status);
+        memset(&info->status, 0, sizeof(info->status));
+        info->flags |= IEEE80211_TX_STAT_ACK;
+        ieee80211_tx_status_irqsafe(hw, skb);
 }
 
-static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-                      struct ieee80211_tx_control *control)
+static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
         struct rtl8187_priv *priv = dev->priv;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         struct rtl8187_tx_hdr *hdr;
-        struct rtl8187_tx_info *info;
         struct urb *urb;
         __le16 rts_dur = 0;
         u32 flags;
@@ -185,33 +180,27 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
         flags = skb->len;
         flags |= RTL8187_TX_FLAG_NO_ENCRYPT;
 
-        BUG_ON(!control->tx_rate);
-
-        flags |= control->tx_rate->hw_value << 24;
+        flags |= ieee80211_get_tx_rate(dev, info)->hw_value << 24;
         if (ieee80211_get_morefrag((struct ieee80211_hdr *)skb->data))
                 flags |= RTL8187_TX_FLAG_MORE_FRAG;
-        if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-                BUG_ON(!control->rts_cts_rate);
+        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
                 flags |= RTL8187_TX_FLAG_RTS;
-                flags |= control->rts_cts_rate->hw_value << 19;
+                flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
                 rts_dur = ieee80211_rts_duration(dev, priv->vif,
-                                                 skb->len, control);
-        } else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
-                BUG_ON(!control->rts_cts_rate);
+                                                 skb->len, info);
+        } else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
                 flags |= RTL8187_TX_FLAG_CTS;
-                flags |= control->rts_cts_rate->hw_value << 19;
+                flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
         }
 
         hdr = (struct rtl8187_tx_hdr *)skb_push(skb, sizeof(*hdr));
         hdr->flags = cpu_to_le32(flags);
         hdr->len = 0;
         hdr->rts_duration = rts_dur;
-        hdr->retry = cpu_to_le32(control->retry_limit << 8);
+        hdr->retry = cpu_to_le32(info->control.retry_limit << 8);
 
-        info = (struct rtl8187_tx_info *)skb->cb;
-        info->control = kmemdup(control, sizeof(*control), GFP_ATOMIC);
-        info->urb = urb;
-        info->dev = dev;
+        info->driver_data[0] = dev;
+        info->driver_data[1] = urb;
         usb_fill_bulk_urb(urb, priv->udev, usb_sndbulkpipe(priv->udev, 2),
                           hdr, skb->len, rtl8187_tx_cb, skb);
         rc = usb_submit_urb(urb, GFP_ATOMIC);
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index c99d4a4fde05..d2378d083a35 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -224,36 +224,6 @@ out:
         return r;
 }
 
-/**
- * clear_tx_skb_control_block - clears the control block of tx skbuffs
- * @skb: a &struct sk_buff pointer
- *
- * This clears the control block of skbuff buffers, which were transmitted to
- * the device. Notify that the function is not thread-safe, so prevent
- * multiple calls.
- */
-static void clear_tx_skb_control_block(struct sk_buff *skb)
-{
-        struct zd_tx_skb_control_block *cb =
-                (struct zd_tx_skb_control_block *)skb->cb;
-
-        kfree(cb->control);
-        cb->control = NULL;
-}
-
-/**
- * kfree_tx_skb - frees a tx skbuff
- * @skb: a &struct sk_buff pointer
- *
- * Frees the tx skbuff. Frees also the allocated control structure in the
- * control block if necessary.
- */
-static void kfree_tx_skb(struct sk_buff *skb)
-{
-        clear_tx_skb_control_block(skb);
-        dev_kfree_skb_any(skb);
-}
-
 static void zd_op_stop(struct ieee80211_hw *hw)
 {
         struct zd_mac *mac = zd_hw_mac(hw);
@@ -276,40 +246,15 @@ static void zd_op_stop(struct ieee80211_hw *hw)
 
 
         while ((skb = skb_dequeue(ack_wait_queue)))
-                kfree_tx_skb(skb);
-}
-
-/**
- * init_tx_skb_control_block - initializes skb control block
- * @skb: a &sk_buff pointer
- * @dev: pointer to the mac80221 device
- * @control: mac80211 tx control applying for the frame in @skb
- *
- * Initializes the control block of the skbuff to be transmitted.
- */
-static int init_tx_skb_control_block(struct sk_buff *skb,
-                                     struct ieee80211_hw *hw,
-                                     struct ieee80211_tx_control *control)
-{
-        struct zd_tx_skb_control_block *cb =
-                (struct zd_tx_skb_control_block *)skb->cb;
-
-        ZD_ASSERT(sizeof(*cb) <= sizeof(skb->cb));
-        memset(cb, 0, sizeof(*cb));
-        cb->hw= hw;
-        cb->control = kmalloc(sizeof(*control), GFP_ATOMIC);
-        if (cb->control == NULL)
-                return -ENOMEM;
-        memcpy(cb->control, control, sizeof(*control));
-
-        return 0;
+                dev_kfree_skb_any(skb);
 }
 
 /**
  * tx_status - reports tx status of a packet if required
  * @hw - a &struct ieee80211_hw pointer
  * @skb - a sk-buffer
- * @status - the tx status of the packet without control information
+ * @flags: extra flags to set in the TX status info
+ * @ackssi: ACK signal strength
  * @success - True for successfull transmission of the frame
  *
  * This information calls ieee80211_tx_status_irqsafe() if required by the
@@ -319,18 +264,17 @@ static int init_tx_skb_control_block(struct sk_buff *skb,
  * If no status information has been requested, the skb is freed.
  */
 static void tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
-                      struct ieee80211_tx_status *status,
-                      bool success)
+                      u32 flags, int ackssi, bool success)
 {
-        struct zd_tx_skb_control_block *cb = (struct zd_tx_skb_control_block *)
-                skb->cb;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+        memset(&info->status, 0, sizeof(info->status));
 
-        ZD_ASSERT(cb->control != NULL);
-        memcpy(&status->control, cb->control, sizeof(status->control));
         if (!success)
-                status->excessive_retries = 1;
-        clear_tx_skb_control_block(skb);
-        ieee80211_tx_status_irqsafe(hw, skb, status);
+                info->status.excessive_retries = 1;
+        info->flags |= flags;
+        info->status.ack_signal = ackssi;
+        ieee80211_tx_status_irqsafe(hw, skb);
 }
 
 /**
@@ -345,15 +289,12 @@ void zd_mac_tx_failed(struct ieee80211_hw *hw)
 {
         struct sk_buff_head *q = &zd_hw_mac(hw)->ack_wait_queue;
         struct sk_buff *skb;
-        struct ieee80211_tx_status status;
 
         skb = skb_dequeue(q);
         if (skb == NULL)
                 return;
 
-        memset(&status, 0, sizeof(status));
-
-        tx_status(hw, skb, &status, 0);
+        tx_status(hw, skb, 0, 0, 0);
 }
 
 /**
@@ -368,28 +309,20 @@ void zd_mac_tx_failed(struct ieee80211_hw *hw)
  */
 void zd_mac_tx_to_dev(struct sk_buff *skb, int error)
 {
-        struct zd_tx_skb_control_block *cb =
-                (struct zd_tx_skb_control_block *)skb->cb;
-        struct ieee80211_hw *hw = cb->hw;
-
-        if (likely(cb->control)) {
-                skb_pull(skb, sizeof(struct zd_ctrlset));
-                if (unlikely(error ||
-                    (cb->control->flags & IEEE80211_TXCTL_NO_ACK)))
-                {
-                        struct ieee80211_tx_status status;
-                        memset(&status, 0, sizeof(status));
-                        tx_status(hw, skb, &status, !error);
-                } else {
-                        struct sk_buff_head *q =
-                                &zd_hw_mac(hw)->ack_wait_queue;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ieee80211_hw *hw = info->driver_data[0];
 
-                        skb_queue_tail(q, skb);
-                        while (skb_queue_len(q) > ZD_MAC_MAX_ACK_WAITERS)
-                                zd_mac_tx_failed(hw);
-                }
+        skb_pull(skb, sizeof(struct zd_ctrlset));
+        if (unlikely(error ||
+            (info->flags & IEEE80211_TX_CTL_NO_ACK))) {
+                tx_status(hw, skb, 0, 0, !error);
         } else {
-                kfree_tx_skb(skb);
+                struct sk_buff_head *q =
+                        &zd_hw_mac(hw)->ack_wait_queue;
+
+                skb_queue_tail(q, skb);
+                while (skb_queue_len(q) > ZD_MAC_MAX_ACK_WAITERS)
+                        zd_mac_tx_failed(hw);
         }
 }
 
@@ -454,7 +387,7 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
         cs->control = 0;
 
         /* First fragment */
-        if (flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+        if (flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
                 cs->control |= ZD_CS_NEED_RANDOM_BACKOFF;
 
         /* Multicast */
@@ -466,10 +399,10 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
             (IEEE80211_FTYPE_CTL|IEEE80211_STYPE_PSPOLL))
                 cs->control |= ZD_CS_PS_POLL_FRAME;
 
-        if (flags & IEEE80211_TXCTL_USE_RTS_CTS)
+        if (flags & IEEE80211_TX_CTL_USE_RTS_CTS)
                 cs->control |= ZD_CS_RTS;
 
-        if (flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+        if (flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
                 cs->control |= ZD_CS_SELF_CTS;
 
         /* FIXME: Management frame? */
@@ -516,25 +449,28 @@ void zd_mac_config_beacon(struct ieee80211_hw *hw, struct sk_buff *beacon)
 }
 
 static int fill_ctrlset(struct zd_mac *mac,
-                        struct sk_buff *skb,
-                        struct ieee80211_tx_control *control)
+                        struct sk_buff *skb)
 {
         int r;
         struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
         unsigned int frag_len = skb->len + FCS_LEN;
         unsigned int packet_length;
+        struct ieee80211_rate *txrate;
         struct zd_ctrlset *cs = (struct zd_ctrlset *)
                 skb_push(skb, sizeof(struct zd_ctrlset));
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
         ZD_ASSERT(frag_len <= 0xffff);
 
-        cs->modulation = control->tx_rate->hw_value;
-        if (control->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
-                cs->modulation = control->tx_rate->hw_value_short;
+        txrate = ieee80211_get_tx_rate(mac->hw, info);
+
+        cs->modulation = txrate->hw_value;
+        if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
+                cs->modulation = txrate->hw_value_short;
 
         cs->tx_length = cpu_to_le16(frag_len);
 
-        cs_set_control(mac, cs, hdr, control->flags);
+        cs_set_control(mac, cs, hdr, info->flags);
 
         packet_length = frag_len + sizeof(struct zd_ctrlset) + 10;
         ZD_ASSERT(packet_length <= 0xffff);
@@ -579,24 +515,21 @@ static int fill_ctrlset(struct zd_mac *mac,
  * control block of the skbuff will be initialized. If necessary the incoming
  * mac80211 queues will be stopped.
  */
-static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-                     struct ieee80211_tx_control *control)
+static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
         struct zd_mac *mac = zd_hw_mac(hw);
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
         int r;
 
-        r = fill_ctrlset(mac, skb, control);
+        r = fill_ctrlset(mac, skb);
         if (r)
                 return r;
 
-        r = init_tx_skb_control_block(skb, hw, control);
-        if (r)
-                return r;
+        info->driver_data[0] = hw;
+
         r = zd_usb_tx(&mac->chip.usb, skb);
-        if (r) {
-                clear_tx_skb_control_block(skb);
+        if (r)
                 return r;
-        }
         return 0;
 }
 
@@ -634,13 +567,8 @@ static int filter_ack(struct ieee80211_hw *hw, struct ieee80211_hdr *rx_hdr,
                 tx_hdr = (struct ieee80211_hdr *)skb->data;
                 if (likely(!compare_ether_addr(tx_hdr->addr2, rx_hdr->addr1)))
                 {
-                        struct ieee80211_tx_status status;
-
-                        memset(&status, 0, sizeof(status));
-                        status.flags = IEEE80211_TX_STATUS_ACK;
-                        status.ack_signal = stats->signal;
                         __skb_unlink(skb, q);
-                        tx_status(hw, skb, &status, 1);
+                        tx_status(hw, skb, IEEE80211_TX_STAT_ACK, stats->signal, 1);
                         goto out;
                 }
         }
@@ -944,8 +872,7 @@ static void zd_op_bss_info_changed(struct ieee80211_hw *hw,
 }
 
 static int zd_op_beacon_update(struct ieee80211_hw *hw,
-                               struct sk_buff *skb,
-                               struct ieee80211_tx_control *ctl)
+                               struct sk_buff *skb)
 {
         struct zd_mac *mac = zd_hw_mac(hw);
         zd_mac_config_beacon(hw, skb);
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index 71170244d2c9..18c1d56d3dd7 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -149,22 +149,6 @@ struct housekeeping {
         struct delayed_work link_led_work;
 };
 
-/**
- * struct zd_tx_skb_control_block - control block for tx skbuffs
- * @control: &struct ieee80211_tx_control pointer
- * @context: context pointer
- *
- * This structure is used to fill the cb field in an &sk_buff to transmit.
- * The control field is NULL, if there is no requirement from the mac80211
- * stack to report about the packet ACK. This is the case if the flag
- * IEEE80211_TXCTL_NO_ACK is not set in &struct ieee80211_tx_control.
- */
-struct zd_tx_skb_control_block {
-        struct ieee80211_tx_control *control;
-        struct ieee80211_hw *hw;
-        void *context;
-};
-
 #define ZD_MAC_STATS_BUFFER_SIZE 16
 
 #define ZD_MAC_MAX_ACK_WAITERS 10
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index 8941f5eb96c2..1ccff240bf97 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -169,10 +169,11 @@ static int upload_code(struct usb_device *udev,
         if (flags & REBOOT) {
                 u8 ret;
 
+                /* Use "DMA-aware" buffer. */
                 r = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
                         USB_REQ_FIRMWARE_CONFIRM,
                         USB_DIR_IN | USB_TYPE_VENDOR,
-                        0, 0, &ret, sizeof(ret), 5000 /* ms */);
+                        0, 0, p, sizeof(ret), 5000 /* ms */);
                 if (r != sizeof(ret)) {
                         dev_err(&udev->dev,
                                 "control request firmeware confirmation failed."
@@ -181,6 +182,7 @@ static int upload_code(struct usb_device *udev,
                                 r = -ENODEV;
                         goto error;
                 }
+                ret = p[0];
                 if (ret & 0x80) {
                         dev_err(&udev->dev,
                                 "Internal error while downloading."
@@ -312,22 +314,31 @@ int zd_usb_read_fw(struct zd_usb *usb, zd_addr_t addr, u8 *data, u16 len)
 {
         int r;
         struct usb_device *udev = zd_usb_to_usbdev(usb);
+        u8 *buf;
 
+        /* Use "DMA-aware" buffer. */
+        buf = kmalloc(len, GFP_KERNEL);
+        if (!buf)
+                return -ENOMEM;
         r = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
                 USB_REQ_FIRMWARE_READ_DATA, USB_DIR_IN | 0x40, addr, 0,
-                data, len, 5000);
+                buf, len, 5000);
         if (r < 0) {
                 dev_err(&udev->dev,
                         "read over firmware interface failed: %d\n", r);
-                return r;
+                goto exit;
         } else if (r != len) {
                 dev_err(&udev->dev,
                         "incomplete read over firmware interface: %d/%d\n",
                         r, len);
-                return -EIO;
+                r = -EIO;
+                goto exit;
         }
-
-        return 0;
+        r = 0;
+        memcpy(data, buf, len);
+exit:
+        kfree(buf);
+        return r;
 }
 
 #define urb_dev(urb) (&(urb)->dev->dev)
@@ -869,7 +880,7 @@ static void tx_urb_complete(struct urb *urb)
 {
         int r;
         struct sk_buff *skb;
-        struct zd_tx_skb_control_block *cb;
+        struct ieee80211_tx_info *info;
         struct zd_usb *usb;
 
         switch (urb->status) {
@@ -893,8 +904,8 @@ free_urb:
          * grab 'usb' pointer before handing off the skb (since
          * it might be freed by zd_mac_tx_to_dev or mac80211)
          */
-        cb = (struct zd_tx_skb_control_block *)skb->cb;
-        usb = &zd_hw_mac(cb->hw)->chip.usb;
+        info = IEEE80211_SKB_CB(skb);
+        usb = &zd_hw_mac(info->driver_data[0])->chip.usb;
         zd_mac_tx_to_dev(skb, urb->status);
         free_tx_urb(usb, urb);
         tx_dec_submitted_urbs(usb);