74 files changed, 3854 insertions, 2974 deletions
diff --git a/Documentation/networking/mac80211_hwsim/README b/Documentation/networking/mac80211_hwsim/README
new file mode 100644
index 000000000000..2ff8ccb8dc37
--- /dev/null
+++ b/Documentation/networking/mac80211_hwsim/README
@@ -0,0 +1,67 @@
+mac80211_hwsim - software simulator of 802.11 radio(s) for mac80211
+Copyright (c) 2008, Jouni Malinen <j@w1.fi>
+This program is free software; you can redistribute it and/or modify
+it under the terms of the GNU General Public License version 2 as
+published by the Free Software Foundation.
+Introduction
+mac80211_hwsim is a Linux kernel module that can be used to simulate
+arbitrary number of IEEE 802.11 radios for mac80211. It can be used to
+test most of the mac80211 functionality and user space tools (e.g.,
+hostapd and wpa_supplicant) in a way that matches very closely with
+the normal case of using real WLAN hardware. From the mac80211 view
+point, mac80211_hwsim is yet another hardware driver, i.e., no changes
+to mac80211 are needed to use this testing tool.
+The main goal for mac80211_hwsim is to make it easier for developers
+to test their code and work with new features to mac80211, hostapd,
+and wpa_supplicant. The simulated radios do not have the limitations
+of real hardware, so it is easy to generate an arbitrary test setup
+and always reproduce the same setup for future tests. In addition,
+since all radio operation is simulated, any channel can be used in
+tests regardless of regulatory rules.
+mac80211_hwsim kernel module has a parameter 'radios' that can be used
+to select how many radios are simulated (default 2). This allows
+configuration of both very simply setups (e.g., just a single access
+point and a station) or large scale tests (multiple access points with
+hundreds of stations).
+mac80211_hwsim works by tracking the current channel of each virtual
+radio and copying all transmitted frames to all other radios that are
+currently enabled and on the same channel as the transmitting
+radio. Software encryption in mac80211 is used so that the frames are
+actually encrypted over the virtual air interface to allow more
+complete testing of encryption.
+A global monitoring netdev, hwsim#, is created independent of
+mac80211. This interface can be used to monitor all transmitted frames
+regardless of channel.
+Simple example
+This example shows how to use mac80211_hwsim to simulate two radios:
+one to act as an access point and the other as a station that
+associates with the AP. hostapd and wpa_supplicant are used to take
+care of WPA2-PSK authentication. In addition, hostapd is also
+processing access point side of association.
+Please note that the current Linux kernel does not enable AP mode, so a
+simple patch is needed to enable AP mode selection:
+http://johannes.sipsolutions.net/patches/kernel/all/LATEST/006-allow-ap-vlan-modes.patch
+# Build mac80211_hwsim as part of kernel configuration
+# Load the module
+modprobe mac80211_hwsim
+# Run hostapd (AP) for wlan0
+hostapd hostapd.conf
+# Run wpa_supplicant (station) for wlan1
+wpa_supplicant -Dwext -iwlan1 -c wpa_supplicant.conf
diff --git a/Documentation/networking/mac80211_hwsim/hostapd.conf b/Documentation/networking/mac80211_hwsim/hostapd.conf
new file mode 100644
index 000000000000..08cde7e35f2e
--- /dev/null
+++ b/Documentation/networking/mac80211_hwsim/hostapd.conf
@@ -0,0 +1,11 @@
+interface=wlan0
+driver=nl80211
+hw_mode=g
+channel=1
+ssid=mac80211 test
+wpa=2
+wpa_key_mgmt=WPA-PSK
+wpa_pairwise=CCMP
+wpa_passphrase=12345678
diff --git a/Documentation/networking/mac80211_hwsim/wpa_supplicant.conf b/Documentation/networking/mac80211_hwsim/wpa_supplicant.conf
new file mode 100644
index 000000000000..299128cff035
--- /dev/null
+++ b/Documentation/networking/mac80211_hwsim/wpa_supplicant.conf
@@ -0,0 +1,10 @@
+ctrl_interface=/var/run/wpa_supplicant
+network={
+        ssid="mac80211 test"
+        psk="12345678"
+        key_mgmt=WPA-PSK
+        proto=WPA2
+        pairwise=CCMP
+        group=CCMP
+}
diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index fdf5aa8b8429..22e1e9a1fb73 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -673,6 +673,19 @@ config ADM8211
          Thanks to Infineon-ADMtek for their support of this driver.
+config MAC80211_HWSIM
+        tristate "Simulated radio testing tool for mac80211"
+        depends on MAC80211 && WLAN_80211
+        ---help---
+          This driver is a developer testing tool that can be used to test
+          IEEE 802.11 networking stack (mac80211) functionality. This is not
+          needed for normal wireless LAN usage and is only for testing. See
+          Documentation/networking/mac80211_hwsim for more information on how
+          to use this tool.
+          To compile this driver as a module, choose M here: the module will be
+          called mac80211_hwsim.  If unsure, say N.
 source "drivers/net/wireless/p54/Kconfig"
 source "drivers/net/wireless/ath5k/Kconfig"
 source "drivers/net/wireless/iwlwifi/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 2c343aae38d4..54a4f6f1db67 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -62,3 +62,5 @@ obj-$(CONFIG_RT2X00)	+= rt2x00/
 obj-$(CONFIG_P54_COMMON)        += p54/
 obj-$(CONFIG_ATH5K)     += ath5k/
+obj-$(CONFIG_MAC80211_HWSIM)    += mac80211_hwsim.o
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index 1e1446bf4b48..e30f8b79ea89 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -4561,22 +4561,13 @@ static ssize_t proc_read( struct file *file,
                          size_t len,
                          loff_t *offset )
 {
-        loff_t pos = *offset;
+        struct proc_data *priv = file->private_data;
-        struct proc_data *priv = (struct proc_data*)file->private_data;
        if (!priv->rbuffer)
                return -EINVAL;
-        if (pos < 0)
+        return simple_read_from_buffer(buffer, len, offset, priv->rbuffer,
-                return -EINVAL;
+                                        priv->readlen);
-        if (pos >= priv->readlen)
-                return 0;
-        if (len > priv->readlen - pos)
-                len = priv->readlen - pos;
-        if (copy_to_user(buffer, priv->rbuffer + pos, len))
-                return -EFAULT;
-        *offset = pos + len;
-        return len;
 }
 /*
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 5f3e849043f7..a382c0078923 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -14,15 +14,6 @@ config IWLWIFI_LEDS
        bool
        default n
-config IWLWIFI_RUN_TIME_CALIB
-        bool
-        depends on IWLCORE
-        default n
-        ---help---
-          This option will enable run time calibration for the iwlwifi driver.
-          These calibrations are Sensitivity and Chain Noise.
 config IWLWIFI_RFKILL
        boolean "IWLWIFI RF kill support"
        depends on IWLCORE
@@ -54,14 +45,6 @@ config IWL4965
          say M here and read <file:Documentation/kbuild/modules.txt>.  The
          module will be called iwl4965.ko.
-config IWL4965_HT
-        bool "Enable 802.11n HT features in iwl4965 driver"
-        depends on EXPERIMENTAL
-        depends on IWL4965
-        ---help---
-          This option enables IEEE 802.11n High Throughput features
-          for the iwl4965 driver.
 config IWL4965_LEDS
        bool "Enable LEDS features in iwl4965 driver"
        depends on IWL4965
@@ -76,15 +59,6 @@ config IWL4965_SPECTRUM_MEASUREMENT
        ---help---
          This option will enable spectrum measurement for the iwl4965 driver.
-config IWL4965_RUN_TIME_CALIB
-        bool "Enable run time Calibration for 4965 NIC"
-        select IWLWIFI_RUN_TIME_CALIB
-        depends on IWL4965
-        default y
-        ---help---
-          This option will enable run time calibration for the iwl4965 driver.
-          These calibrations are Sensitivity and Chain Noise. If unsure, say yes
 config IWLWIFI_DEBUG
        bool "Enable full debugging output in iwl4965 driver"
        depends on IWL4965
@@ -118,16 +92,6 @@ config IWL5000
          This option enables support for Intel Wireless WiFi Link 5000AGN Family
          Dependency on 4965 is temporary
-config IWL5000_RUN_TIME_CALIB
-        bool "Enable run time Calibration for 5000 NIC"
-        select IWLWIFI_RUN_TIME_CALIB
-        depends on IWL5000
-        default y
-        ---help---
-          This option will enable run time calibration for the iwl5000 driver.
-          These calibrations are Sensitivity and Chain Noise. If unsure, say yes
 config IWLWIFI_DEBUGFS
        bool "Iwlwifi debugfs support"
        depends on IWLCORE && IWLWIFI_DEBUG && MAC80211_DEBUGFS
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index 5c73eede7193..1f52b92f08b5 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,10 +1,10 @@
 obj-$(CONFIG_IWLCORE)   += iwlcore.o
 iwlcore-objs            := iwl-core.o iwl-eeprom.o iwl-hcmd.o iwl-power.o
-iwlcore-objs            += iwl-rx.o iwl-tx.o iwl-sta.o
+iwlcore-objs            += iwl-rx.o iwl-tx.o iwl-sta.o iwl-calib.o
+iwlcore-objs            += iwl-scan.o
 iwlcore-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
 iwlcore-$(CONFIG_IWLWIFI_LEDS) += iwl-led.o
 iwlcore-$(CONFIG_IWLWIFI_RFKILL) += iwl-rfkill.o
-iwlcore-$(CONFIG_IWLWIFI_RUN_TIME_CALIB) += iwl-calib.o
 obj-$(CONFIG_IWL3945)   += iwl3945.o
 iwl3945-objs            := iwl3945-base.o iwl-3945.o iwl-3945-rs.o
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
index 0ba6889dfd41..63f20370032d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -388,7 +388,7 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
        u32 print_dump = 0;     /* set to 1 to dump all frames' contents */
        u32 hundred = 0;
        u32 dataframe = 0;
-        u16 fc;
+        __le16 fc;
        u16 seq_ctl;
        u16 channel;
        u16 phy_flags;
@@ -407,7 +407,7 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
        u8 *data = IWL_RX_DATA(pkt);
        /* MAC header */
-        fc = le16_to_cpu(header->frame_control);
+        fc = header->frame_control;
        seq_ctl = le16_to_cpu(header->seq_ctrl);
        /* metadata */
@@ -431,8 +431,8 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
        /* if data frame is to us and all is good,
         *   (optionally) print summary for only 1 out of every 100 */
-        if (to_us && (fc & ~IEEE80211_FCTL_PROTECTED) ==
+        if (to_us && (fc & ~cpu_to_le16(IEEE80211_FCTL_PROTECTED)) ==
-            (IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
+            cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
                dataframe = 1;
                if (!group100)
                        print_summary = 1;      /* print each frame */
@@ -455,13 +455,13 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
                if (hundred)
                        title = "100Frames";
-                else if (fc & IEEE80211_FCTL_RETRY)
+                else if (ieee80211_has_retry(fc))
                        title = "Retry";
-                else if (ieee80211_is_assoc_response(fc))
+                else if (ieee80211_is_assoc_resp(fc))
                        title = "AscRsp";
-                else if (ieee80211_is_reassoc_response(fc))
+                else if (ieee80211_is_reassoc_resp(fc))
                        title = "RasRsp";
-                else if (ieee80211_is_probe_response(fc)) {
+                else if (ieee80211_is_probe_resp(fc)) {
                        title = "PrbRsp";
                        print_dump = 1; /* dump frame contents */
                } else if (ieee80211_is_beacon(fc)) {
@@ -490,14 +490,14 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
                if (dataframe)
                        IWL_DEBUG_RX("%s: mhd=0x%04x, dst=0x%02x, "
                                     "len=%u, rssi=%d, chnl=%d, rate=%u, \n",
-                                     title, fc, header->addr1[5],
+                                     title, le16_to_cpu(fc), header->addr1[5],
                                     length, rssi, channel, rate);
                else {
                        /* src/dst addresses assume managed mode */
                        IWL_DEBUG_RX("%s: 0x%04x, dst=0x%02x, "
                                     "src=0x%02x, rssi=%u, tim=%lu usec, "
                                     "phy=0x%02x, chnl=%d\n",
-                                     title, fc, header->addr1[5],
+                                     title, le16_to_cpu(fc), header->addr1[5],
                                     header->addr3[5], rssi,
                                     tsf_low - priv->scan_start_tsf,
                                     phy_flags, channel);
@@ -971,7 +971,7 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
        u8 rts_retry_limit;
        u8 data_retry_limit;
        __le32 tx_flags;
-        u16 fc = le16_to_cpu(hdr->frame_control);
+        __le16 fc = hdr->frame_control;
        rate = iwl3945_rates[rate_index].plcp;
        tx_flags = cmd->cmd.tx.tx_flags;
@@ -996,7 +996,7 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
        else
                rts_retry_limit = 7;
-        if (ieee80211_is_probe_response(fc)) {
+        if (ieee80211_is_probe_resp(fc)) {
                data_retry_limit = 3;
                if (data_retry_limit < rts_retry_limit)
                        rts_retry_limit = data_retry_limit;
@@ -1006,12 +1006,12 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
        if (priv->data_retry_limit != -1)
                data_retry_limit = priv->data_retry_limit;
-        if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+        if (ieee80211_is_mgmt(fc)) {
-                switch (fc & IEEE80211_FCTL_STYPE) {
+                switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-                case IEEE80211_STYPE_AUTH:
+                case cpu_to_le16(IEEE80211_STYPE_AUTH):
-                case IEEE80211_STYPE_DEAUTH:
+                case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
-                case IEEE80211_STYPE_ASSOC_REQ:
+                case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
-                case IEEE80211_STYPE_REASSOC_REQ:
+                case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
                        if (tx_flags & TX_CMD_FLG_RTS_MSK) {
                                tx_flags &= ~TX_CMD_FLG_RTS_MSK;
                                tx_flags |= TX_CMD_FLG_CTS_MSK;
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
index fc118335b60f..10f630e1afa6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
@@ -82,7 +82,7 @@
 */
 #define IWL_CMD_QUEUE_NUM       4
 #define IWL_CMD_FIFO_NUM        4
-#define IWL_BACK_QUEUE_FIRST_ID 7
+#define IWL49_FIRST_AMPDU_QUEUE 7
 /* Tx rates */
 #define IWL_CCK_RATES 4
@@ -793,19 +793,6 @@ enum {
 /********************* END TXPOWER *****************************************/
-static inline u8 iwl4965_hw_get_rate(__le32 rate_n_flags)
-{
-        return le32_to_cpu(rate_n_flags) & 0xFF;
-}
-static inline u32 iwl4965_hw_get_rate_n_flags(__le32 rate_n_flags)
-{
-        return le32_to_cpu(rate_n_flags) & 0x1FFFF;
-}
-static inline __le32 iwl4965_hw_set_rate_n_flags(u8 rate, u16 flags)
-{
-        return cpu_to_le32(flags|(u16)rate);
-}
 /**
 * Tx/Rx Queues
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
index d8f2b4d33fd9..202303117285 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
@@ -38,6 +38,7 @@
 #include "../net/mac80211/rate.h"
 #include "iwl-dev.h"
+#include "iwl-sta.h"
 #include "iwl-core.h"
 #include "iwl-helpers.h"
@@ -105,8 +106,6 @@ struct iwl4965_scale_tbl_info {
        struct iwl4965_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
 };
-#ifdef CONFIG_IWL4965_HT
 struct iwl4965_traffic_load {
        unsigned long time_stamp;       /* age of the oldest statistics */
        u32 packet_count[TID_QUEUE_MAX_SIZE];   /* packet count in this time
@@ -118,8 +117,6 @@ struct iwl4965_traffic_load {
        u8 head;                        /* start of the circular buffer */
 };
-#endif /* CONFIG_IWL4965_HT */
 /**
 * struct iwl4965_lq_sta -- driver's rate scaling private structure
 *
@@ -157,16 +154,12 @@ struct iwl4965_lq_sta {
        struct iwl_link_quality_cmd lq;
        struct iwl4965_scale_tbl_info lq_info[LQ_SIZE]; /* "active", "search" */
-#ifdef CONFIG_IWL4965_HT
        struct iwl4965_traffic_load load[TID_MAX_LOAD_COUNT];
        u8 tx_agg_tid_en;
-#endif
 #ifdef CONFIG_MAC80211_DEBUGFS
        struct dentry *rs_sta_dbgfs_scale_table_file;
        struct dentry *rs_sta_dbgfs_stats_table_file;
-#ifdef CONFIG_IWL4965_HT
        struct dentry *rs_sta_dbgfs_tx_agg_tid_en_file;
-#endif
        u32 dbg_fixed_rate;
 #endif
        struct iwl_priv *drv;
@@ -256,7 +249,6 @@ static inline u8 rs_is_valid_ant(u8 valid_antenna, u8 ant_type)
        return ((ant_type & valid_antenna) == ant_type);
 }
-#ifdef CONFIG_IWL4965_HT
 /*
 *      removes the old data from the statistics. All data that is older than
 *      TID_MAX_TIME_DIFF, will be deleted.
@@ -282,21 +274,21 @@ 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,
+static u8 rs_tl_add_packet(struct iwl4965_lq_sta *lq_data,
-                             struct ieee80211_hdr *hdr)
+                           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);
+        __le16 fc = hdr->frame_control;
        u8 tid;
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                u8 *qc = ieee80211_get_qos_ctl(hdr);
                tid = qc[0] & 0xf;
        } else
-                return;
+                return MAX_TID_COUNT;
        tl = &lq_data->load[tid];
@@ -309,7 +301,7 @@ static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data,
                tl->queue_count = 1;
                tl->head = 0;
                tl->packet_count[0] = 1;
-                return;
+                return MAX_TID_COUNT;
        }
        time_diff = TIME_WRAP_AROUND(tl->time_stamp, curr_time);
@@ -326,6 +318,8 @@ static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data,
        if ((index + 1) > tl->queue_count)
                tl->queue_count = index + 1;
+        return tid;
 }
 /*
@@ -389,8 +383,6 @@ static void rs_tl_turn_on_agg(struct iwl_priv *priv, u8 tid,
                        rs_tl_turn_on_agg_for_tid(priv, lq_data, tid, sta);
 }
-#endif /* CONFIG_IWLWIFI_HT */
 static inline int get_num_of_ant_from_rate(u32 rate_n_flags)
 {
        return (!!(rate_n_flags & RATE_MCS_ANT_A_MSK) +
@@ -545,7 +537,7 @@ static int rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
        u8 num_of_ant = get_num_of_ant_from_rate(rate_n_flags);
        u8 mcs;
-        *rate_idx = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
+        *rate_idx = iwl_hwrate_to_plcp_idx(rate_n_flags);
        if (*rate_idx  == IWL_RATE_INVALID) {
                *rate_idx = -1;
@@ -626,7 +618,6 @@ static int rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
 /* FIXME:RS: in 4965 we don't use greenfield at all */
 /* FIXME:RS: don't use greenfield for now in TX */
-/* #ifdef CONFIG_IWL4965_HT */
 #if 0
 static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
 {
@@ -634,12 +625,11 @@ static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf
                priv->current_ht_config.is_green_field &&
                !priv->current_ht_config.non_GF_STA_present);
 }
-#else
+#endif
 static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
 {
        return 0;
 }
-#endif  /* CONFIG_IWL4965_HT */
 /**
 * rs_get_supported_rates - get the available rates
@@ -804,7 +794,7 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
        struct iwl4965_scale_tbl_info tbl_type;
        struct iwl4965_scale_tbl_info *curr_tbl, *search_tbl;
        u8 active_index = 0;
-        u16 fc = le16_to_cpu(hdr->frame_control);
+        __le16 fc = hdr->frame_control;
        s32 tpt = 0;
        IWL_DEBUG_RATE_LIMIT("get frame ack response, update rate scale window\n");
@@ -1050,7 +1040,6 @@ static void rs_set_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
                tbl->expected_tpt = expected_tpt_G;
 }
-#ifdef CONFIG_IWL4965_HT
 /*
 * Find starting rate for new "search" high-throughput mode of modulation.
 * Goal is to find lowest expected rate (under perfect conditions) that is
@@ -1152,12 +1141,10 @@ static s32 rs_get_best_rate(struct iwl_priv *priv,
        return new_rate;
 }
-#endif                          /* CONFIG_IWL4965_HT */
 /*
 * Set up search table for MIMO
 */
-#ifdef CONFIG_IWL4965_HT
 static int rs_switch_to_mimo2(struct iwl_priv *priv,
                             struct iwl4965_lq_sta *lq_sta,
                             struct ieee80211_conf *conf,
@@ -1221,16 +1208,6 @@ static int rs_switch_to_mimo2(struct iwl_priv *priv,
                     tbl->current_rate, is_green);
        return 0;
 }
-#else
-static int rs_switch_to_mimo2(struct iwl_priv *priv,
-                             struct iwl4965_lq_sta *lq_sta,
-                             struct ieee80211_conf *conf,
-                             struct sta_info *sta,
-                             struct iwl4965_scale_tbl_info *tbl, int index)
-{
-        return -1;
-}
-#endif  /*CONFIG_IWL4965_HT */
 /*
 * Set up search table for SISO
@@ -1241,7 +1218,6 @@ static int rs_switch_to_siso(struct iwl_priv *priv,
                             struct sta_info *sta,
                             struct iwl4965_scale_tbl_info *tbl, int index)
 {
-#ifdef CONFIG_IWL4965_HT
        u16 rate_mask;
        u8 is_green = lq_sta->is_green;
        s32 rate;
@@ -1291,9 +1267,6 @@ static int rs_switch_to_siso(struct iwl_priv *priv,
        IWL_DEBUG_RATE("LQ: Switch to new mcs %X index is green %X\n",
                     tbl->current_rate, is_green);
        return 0;
-#else
-        return -1;
-#endif  /*CONFIG_IWL4965_HT */
 }
 /*
@@ -1678,7 +1651,8 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
        int high_tpt = IWL_INVALID_VALUE;
        u32 fail_count;
        s8 scale_action = 0;
-        u16 fc, rate_mask;
+        __le16 fc;
+        u16 rate_mask;
        u8 update_lq = 0;
        struct iwl4965_lq_sta *lq_sta;
        struct iwl4965_scale_tbl_info *tbl, *tbl1;
@@ -1689,13 +1663,11 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
        u8 done_search = 0;
        u16 high_low;
        s32 sr;
-#ifdef CONFIG_IWL4965_HT
        u8 tid = MAX_TID_COUNT;
-#endif
        IWL_DEBUG_RATE("rate scale calculate new rate for skb\n");
-        fc = le16_to_cpu(hdr->frame_control);
+        fc = hdr->frame_control;
        if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
                /* Send management frames and broadcast/multicast data using
                 * lowest rate. */
@@ -1712,9 +1684,8 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
        }
        lq_sta = (struct iwl4965_lq_sta *)sta->rate_ctrl_priv;
-#ifdef CONFIG_IWL4965_HT
+        tid = rs_tl_add_packet(lq_sta, hdr);
-        rs_tl_add_packet(lq_sta, hdr);
-#endif
        /*
         * Select rate-scale / modulation-mode table to work with in
         * the rest of this function:  "search" if searching for better
@@ -1842,8 +1813,7 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
                        tbl = &(lq_sta->lq_info[active_tbl]);
                        /* Revert to "active" rate and throughput info */
-                        index = iwl4965_hwrate_to_plcp_idx(
+                        index = iwl_hwrate_to_plcp_idx(tbl->current_rate);
-                                                        tbl->current_rate);
                        current_tpt = lq_sta->last_tpt;
                        /* Need to set up a new rate table in uCode */
@@ -1997,8 +1967,7 @@ lq_update:
                                rs_rate_scale_clear_window(&(tbl->win[i]));
                        /* Use new "search" start rate */
-                        index = iwl4965_hwrate_to_plcp_idx(
+                        index = iwl_hwrate_to_plcp_idx(tbl->current_rate);
-                                                        tbl->current_rate);
                        IWL_DEBUG_RATE("Switch current  mcs: %X index: %d\n",
                                     tbl->current_rate, index);
@@ -2013,9 +1982,7 @@ lq_update:
                 * before next round of mode comparisons. */
                tbl1 = &(lq_sta->lq_info[lq_sta->active_tbl]);
                if (is_legacy(tbl1->lq_type) &&
-#ifdef CONFIG_IWL4965_HT
                   (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE)) &&
-#endif
                    (lq_sta->action_counter >= 1)) {
                        lq_sta->action_counter = 0;
                        IWL_DEBUG_RATE("LQ: STAY in legacy table\n");
@@ -2027,14 +1994,12 @@ lq_update:
                 * mode for a while before next round of mode comparisons. */
                if (lq_sta->enable_counter &&
                    (lq_sta->action_counter >= IWL_ACTION_LIMIT)) {
-#ifdef CONFIG_IWL4965_HT
                        if ((lq_sta->last_tpt > IWL_AGG_TPT_THREHOLD) &&
                            (lq_sta->tx_agg_tid_en & (1 << tid)) &&
                            (tid != MAX_TID_COUNT)) {
                                IWL_DEBUG_RATE("try to aggregate tid %d\n", tid);
                                rs_tl_turn_on_agg(priv, tid, lq_sta, sta);
                        }
-#endif /*CONFIG_IWL4965_HT */
                        lq_sta->action_counter = 0;
                        rs_set_stay_in_table(priv, 0, lq_sta);
                }
@@ -2136,7 +2101,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
        struct ieee80211_conf *conf = &local->hw.conf;
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
        struct sta_info *sta;
-        u16 fc;
+        __le16 fc;
        struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
        struct iwl4965_lq_sta *lq_sta;
@@ -2148,7 +2113,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
        /* Send management frames and broadcast/multicast data using lowest
         * rate. */
-        fc = le16_to_cpu(hdr->frame_control);
+        fc = hdr->frame_control;
        if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1) ||
            !sta || !sta->rate_ctrl_priv) {
                sel->rate_idx = rate_lowest_index(local, sband, sta);
@@ -2279,30 +2244,23 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
        lq_sta->active_legacy_rate = priv->active_rate & ~(0x1000);
        lq_sta->active_rate_basic = priv->active_rate_basic;
        lq_sta->band = priv->band;
-#ifdef CONFIG_IWL4965_HT
        /*
         * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
         * supp_rates[] does not; shift to convert format, force 9 MBits off.
         */
-        lq_sta->active_siso_rate =
+        lq_sta->active_siso_rate = conf->ht_conf.supp_mcs_set[0] << 1;
-                priv->current_ht_config.supp_mcs_set[0] << 1;
+        lq_sta->active_siso_rate |= conf->ht_conf.supp_mcs_set[0] & 0x1;
-        lq_sta->active_siso_rate |=
-                priv->current_ht_config.supp_mcs_set[0] & 0x1;
        lq_sta->active_siso_rate &= ~((u16)0x2);
        lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
        /* Same here */
-        lq_sta->active_mimo2_rate =
+        lq_sta->active_mimo2_rate = conf->ht_conf.supp_mcs_set[1] << 1;
-                priv->current_ht_config.supp_mcs_set[1] << 1;
+        lq_sta->active_mimo2_rate |= conf->ht_conf.supp_mcs_set[1] & 0x1;
-        lq_sta->active_mimo2_rate |=
-                priv->current_ht_config.supp_mcs_set[1] & 0x1;
        lq_sta->active_mimo2_rate &= ~((u16)0x2);
        lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
-        lq_sta->active_mimo3_rate =
+        lq_sta->active_mimo3_rate = conf->ht_conf.supp_mcs_set[2] << 1;
-                priv->current_ht_config.supp_mcs_set[2] << 1;
+        lq_sta->active_mimo3_rate |= conf->ht_conf.supp_mcs_set[2] & 0x1;
-        lq_sta->active_mimo3_rate |=
-                priv->current_ht_config.supp_mcs_set[2] & 0x1;
        lq_sta->active_mimo3_rate &= ~((u16)0x2);
        lq_sta->active_mimo3_rate <<= IWL_FIRST_OFDM_RATE;
@@ -2317,7 +2275,6 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
        /* as default allow aggregation for all tids */
        lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
-#endif /*CONFIG_IWL4965_HT*/
 #ifdef CONFIG_MAC80211_DEBUGFS
        lq_sta->drv = priv;
 #endif
@@ -2635,11 +2592,9 @@ static void rs_add_debugfs(void *priv, void *priv_sta,
        lq_sta->rs_sta_dbgfs_stats_table_file =
                debugfs_create_file("rate_stats_table", 0600, dir,
                        lq_sta, &rs_sta_dbgfs_stats_table_ops);
-#ifdef CONFIG_IWL4965_HT
        lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file =
                debugfs_create_u8("tx_agg_tid_enable", 0600, dir,
                &lq_sta->tx_agg_tid_en);
-#endif
 }
@@ -2648,9 +2603,7 @@ static void rs_remove_debugfs(void *priv, void *priv_sta)
        struct iwl4965_lq_sta *lq_sta = priv_sta;
        debugfs_remove(lq_sta->rs_sta_dbgfs_scale_table_file);
        debugfs_remove(lq_sta->rs_sta_dbgfs_stats_table_file);
-#ifdef CONFIG_IWL4965_HT
        debugfs_remove(lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file);
-#endif
 }
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
index 1dd4124227a5..cbd126418490 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
@@ -286,8 +286,6 @@ static inline u8 iwl4965_get_prev_ieee_rate(u8 rate_index)
        return rate;
 }
-extern int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags);
 /**
 * iwl4965_fill_rs_info - Fill an output text buffer with the rate representation
 *
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index aee7014bcb94..ab5027345a01 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -46,6 +46,9 @@
 #include "iwl-calib.h"
 #include "iwl-sta.h"
+static int iwl4965_send_tx_power(struct iwl_priv *priv);
+static int iwl4965_hw_get_temperature(const struct iwl_priv *priv);
 /* module parameters */
 static struct iwl_mod_params iwl4965_mod_params = {
        .num_of_queues = IWL49_NUM_QUEUES,
@@ -281,7 +284,7 @@ static void iwl4965_init_alive_start(struct iwl_priv *priv)
        }
        /* Calculate temperature */
-        priv->temperature = iwl4965_get_temperature(priv);
+        priv->temperature = iwl4965_hw_get_temperature(priv);
        /* Send pointers to protocol/runtime uCode image ... init code will
         * load and launch runtime uCode, which will send us another "Alive"
@@ -305,60 +308,6 @@ static int is_fat_channel(__le32 rxon_flags)
                (rxon_flags & RXON_FLG_CHANNEL_MODE_MIXED_MSK);
 }
-int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
-{
-        int idx = 0;
-        /* 4965 HT rate format */
-        if (rate_n_flags & RATE_MCS_HT_MSK) {
-                idx = (rate_n_flags & 0xff);
-                if (idx >= IWL_RATE_MIMO2_6M_PLCP)
-                        idx = idx - IWL_RATE_MIMO2_6M_PLCP;
-                idx += IWL_FIRST_OFDM_RATE;
-                /* skip 9M not supported in ht*/
-                if (idx >= IWL_RATE_9M_INDEX)
-                        idx += 1;
-                if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
-                        return idx;
-        /* 4965 legacy rate format, search for match in table */
-        } else {
-                for (idx = 0; idx < ARRAY_SIZE(iwl_rates); idx++)
-                        if (iwl_rates[idx].plcp == (rate_n_flags & 0xFF))
-                                return idx;
-        }
-        return -1;
-}
-/**
- * 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_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_TX_CTL_OFDM_HT;
-        if (rate_n_flags & RATE_MCS_GF_MSK)
-                control->flags |= IEEE80211_TX_CTL_GREEN_FIELD;
-        if (rate_n_flags & RATE_MCS_FAT_MSK)
-                control->flags |= IEEE80211_TX_CTL_40_MHZ_WIDTH;
-        if (rate_n_flags & RATE_MCS_DUP_MSK)
-                control->flags |= IEEE80211_TX_CTL_DUP_DATA;
-        if (rate_n_flags & RATE_MCS_SGI_MSK)
-                control->flags |= IEEE80211_TX_CTL_SHORT_GI;
-        rate_index = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
-        if (control->band == IEEE80211_BAND_5GHZ)
-                rate_index -= IWL_FIRST_OFDM_RATE;
-        control->tx_rate_idx = rate_index;
-}
 /*
 * EEPROM handlers
 */
@@ -608,52 +557,6 @@ out:
 #define REG_RECALIB_PERIOD (60)
-/**
- * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
- *
- * This callback is provided in order to send a statistics request.
- *
- * This timer function is continually reset to execute within
- * REG_RECALIB_PERIOD seconds since the last STATISTICS_NOTIFICATION
- * was received.  We need to ensure we receive the statistics in order
- * to update the temperature used for calibrating the TXPOWER.
- */
-static void iwl4965_bg_statistics_periodic(unsigned long data)
-{
-        struct iwl_priv *priv = (struct iwl_priv *)data;
-        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-                return;
-        iwl_send_statistics_request(priv, CMD_ASYNC);
-}
-void iwl4965_rf_kill_ct_config(struct iwl_priv *priv)
-{
-        struct iwl4965_ct_kill_config cmd;
-        unsigned long flags;
-        int ret = 0;
-        spin_lock_irqsave(&priv->lock, flags);
-        iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
-                    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
-        spin_unlock_irqrestore(&priv->lock, flags);
-        cmd.critical_temperature_R =
-                cpu_to_le32(priv->hw_params.ct_kill_threshold);
-        ret = iwl_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
-                               sizeof(cmd), &cmd);
-        if (ret)
-                IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
-        else
-                IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded, "
-                        "critical temperature is %d\n",
-                        cmd.critical_temperature_R);
-}
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
 /* Reset differential Rx gains in NIC to prepare for chain noise calibration.
 * Called after every association, but this runs only once!
 *  ... once chain noise is calibrated the first time, it's good forever.  */
@@ -741,30 +644,6 @@ static void iwl4965_gain_computation(struct iwl_priv *priv,
        data->beacon_count = 0;
 }
-static void iwl4965_bg_sensitivity_work(struct work_struct *work)
-{
-        struct iwl_priv *priv = container_of(work, struct iwl_priv,
-                        sensitivity_work);
-        mutex_lock(&priv->mutex);
-        if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
-            test_bit(STATUS_SCANNING, &priv->status)) {
-                mutex_unlock(&priv->mutex);
-                return;
-        }
-        if (priv->start_calib) {
-                iwl_chain_noise_calibration(priv, &priv->statistics);
-                iwl_sensitivity_calibration(priv, &priv->statistics);
-        }
-        mutex_unlock(&priv->mutex);
-        return;
-}
-#endif /*CONFIG_IWL4965_RUN_TIME_CALIB*/
 static void iwl4965_bg_txpower_work(struct work_struct *work)
 {
        struct iwl_priv *priv = container_of(work, struct iwl_priv,
@@ -783,7 +662,7 @@ static void iwl4965_bg_txpower_work(struct work_struct *work)
        /* Regardless of if we are assocaited, we must reconfigure the
         * TX power since frames can be sent on non-radar channels while
         * not associated */
-        iwl4965_hw_reg_send_txpower(priv);
+        iwl4965_send_tx_power(priv);
        /* Update last_temperature to keep is_calib_needed from running
         * when it isn't needed... */
@@ -843,7 +722,7 @@ static const u16 default_queue_to_tx_fifo[] = {
        IWL_TX_FIFO_HCCA_2
 };
-int iwl4965_alive_notify(struct iwl_priv *priv)
+static int iwl4965_alive_notify(struct iwl_priv *priv)
 {
        u32 a;
        int i = 0;
@@ -920,7 +799,6 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
        return ret;
 }
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
 static struct iwl_sensitivity_ranges iwl4965_sensitivity = {
        .min_nrg_cck = 97,
        .max_nrg_cck = 0,
@@ -943,14 +821,13 @@ static struct iwl_sensitivity_ranges iwl4965_sensitivity = {
        .nrg_th_cck = 100,
        .nrg_th_ofdm = 100,
 };
-#endif
 /**
 * iwl4965_hw_set_hw_params
 *
 * Called when initializing driver
 */
-int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
+static int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
 {
        if ((priv->cfg->mod_params->num_of_queues > IWL49_NUM_QUEUES) ||
@@ -961,6 +838,7 @@ 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.first_ampdu_q = IWL49_FIRST_AMPDU_QUEUE;
        priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
        priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
        priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
@@ -983,9 +861,7 @@ int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
        priv->hw_params.valid_rx_ant = ANT_A | ANT_B;
        priv->hw_params.ct_kill_threshold = CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD);
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
        priv->hw_params.sens = &iwl4965_sensitivity;
-#endif
        return 0;
 }
@@ -1001,11 +877,6 @@ static int iwl4965_set_power(struct iwl_priv *priv,
                                    cmd, NULL);
        return ret;
 }
-int iwl4965_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
-{
-        IWL_ERROR("TODO: Implement iwl4965_hw_reg_set_txpower!\n");
-        return -EINVAL;
-}
 static s32 iwl4965_math_div_round(s32 num, s32 denom, s32 *res)
 {
@@ -1056,20 +927,6 @@ static s32 iwl4965_get_voltage_compensation(s32 eeprom_voltage,
        return comp;
 }
-static const struct iwl_channel_info *
-iwl4965_get_channel_txpower_info(struct iwl_priv *priv,
-                                 enum ieee80211_band band, u16 channel)
-{
-        const struct iwl_channel_info *ch_info;
-        ch_info = iwl_get_channel_info(priv, band, channel);
-        if (!is_channel_valid(ch_info))
-                return NULL;
-        return ch_info;
-}
 static s32 iwl4965_get_tx_atten_grp(u16 channel)
 {
        if (channel >= CALIB_IWL_TX_ATTEN_GR5_FCH &&
@@ -1493,30 +1350,17 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
        s32 factory_actual_pwr[2];
        s32 power_index;
-        /* Sanity check requested level (dBm) */
-        if (priv->user_txpower_limit < IWL_TX_POWER_TARGET_POWER_MIN) {
-                IWL_WARNING("Requested user TXPOWER %d below limit.\n",
-                            priv->user_txpower_limit);
-                return -EINVAL;
-        }
-        if (priv->user_txpower_limit > IWL_TX_POWER_TARGET_POWER_MAX) {
-                IWL_WARNING("Requested user TXPOWER %d above limit.\n",
-                            priv->user_txpower_limit);
-                return -EINVAL;
-        }
        /* user_txpower_limit is in dBm, convert to half-dBm (half-dB units
         *   are used for indexing into txpower table) */
-        user_target_power = 2 * priv->user_txpower_limit;
+        user_target_power = 2 * priv->tx_power_user_lmt;
        /* Get current (RXON) channel, band, width */
-        ch_info =
-                iwl4965_get_channel_txpower_info(priv, priv->band, channel);
        IWL_DEBUG_TXPOWER("chan %d band %d is_fat %d\n", channel, band,
                          is_fat);
-        if (!ch_info)
+        ch_info = iwl_get_channel_info(priv, priv->band, channel);
+        if (!is_channel_valid(ch_info))
                return -EINVAL;
        /* get txatten group, used to select 1) thermal txpower adjustment
@@ -1717,12 +1561,12 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 }
 /**
- * iwl4965_hw_reg_send_txpower - Configure the TXPOWER level user limit
+ * iwl4965_send_tx_power - Configure the TXPOWER level user limit
 *
 * Uses the active RXON for channel, band, and characteristics (fat, high)
- * The power limit is taken from priv->user_txpower_limit.
+ * The power limit is taken from priv->tx_power_user_lmt.
 */
-int iwl4965_hw_reg_send_txpower(struct iwl_priv *priv)
+static int iwl4965_send_tx_power(struct iwl_priv *priv)
 {
        struct iwl4965_txpowertable_cmd cmd = { 0 };
        int ret;
@@ -1848,11 +1692,6 @@ static int iwl4965_shared_mem_rx_idx(struct iwl_priv *priv)
        return le32_to_cpu(s->rb_closed) & 0xFFF;
 }
-int iwl4965_hw_get_temperature(struct iwl_priv *priv)
-{
-        return priv->temperature;
-}
 unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
                          struct iwl_frame *frame, u8 rate)
 {
@@ -1875,10 +1714,10 @@ unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
        if ((rate == IWL_RATE_1M_PLCP) || (rate >= IWL_RATE_2M_PLCP))
                tx_beacon_cmd->tx.rate_n_flags =
-                        iwl4965_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
+                        iwl_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
        else
                tx_beacon_cmd->tx.rate_n_flags =
-                        iwl4965_hw_set_rate_n_flags(rate, 0);
+                        iwl_hw_set_rate_n_flags(rate, 0);
        tx_beacon_cmd->tx.tx_flags = (TX_CMD_FLG_SEQ_CTL_MSK |
                                TX_CMD_FLG_TSF_MSK | TX_CMD_FLG_STA_RATE_MSK);
@@ -1950,12 +1789,12 @@ static s32 sign_extend(u32 oper, int index)
 }
 /**
- * iwl4965_get_temperature - return the calibrated temperature (in Kelvin)
+ * iwl4965_hw_get_temperature - return the calibrated temperature (in Kelvin)
 * @statistics: Provides the temperature reading from the uCode
 *
 * A return of <0 indicates bogus data in the statistics
 */
-int iwl4965_get_temperature(const struct iwl_priv *priv)
+static int iwl4965_hw_get_temperature(const struct iwl_priv *priv)
 {
        s32 temperature;
        s32 vt;
@@ -1990,8 +1829,7 @@ int iwl4965_get_temperature(const struct iwl_priv *priv)
                vt = sign_extend(
                        le32_to_cpu(priv->statistics.general.temperature), 23);
-        IWL_DEBUG_TEMP("Calib values R[1-3]: %d %d %d R4: %d\n",
+        IWL_DEBUG_TEMP("Calib values R[1-3]: %d %d %d R4: %d\n", R1, R2, R3, vt);
-                       R1, R2, R3, vt);
        if (R3 == R1) {
                IWL_ERROR("Calibration conflict R1 == R3\n");
@@ -2002,11 +1840,10 @@ int iwl4965_get_temperature(const struct iwl_priv *priv)
         * Add offset to center the adjustment around 0 degrees Centigrade. */
        temperature = TEMPERATURE_CALIB_A_VAL * (vt - R2);
        temperature /= (R3 - R1);
-        temperature = (temperature * 97) / 100 +
+        temperature = (temperature * 97) / 100 + TEMPERATURE_CALIB_KELVIN_OFFSET;
-            TEMPERATURE_CALIB_KELVIN_OFFSET;
-        IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n", temperature,
+        IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n",
-            KELVIN_TO_CELSIUS(temperature));
+                        temperature, KELVIN_TO_CELSIUS(temperature));
        return temperature;
 }
@@ -2123,9 +1960,7 @@ void iwl4965_hw_rx_statistics(struct iwl_priv *priv,
        if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
            (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
                iwl4965_rx_calc_noise(priv);
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
+                queue_work(priv->workqueue, &priv->run_time_calib_work);
-                queue_work(priv->workqueue, &priv->sensitivity_work);
-#endif
        }
        iwl_leds_background(priv);
@@ -2136,7 +1971,7 @@ void iwl4965_hw_rx_statistics(struct iwl_priv *priv,
        if (!change)
                return;
-        temp = iwl4965_get_temperature(priv);
+        temp = iwl4965_hw_get_temperature(priv);
        if (temp < 0)
                return;
@@ -2155,8 +1990,9 @@ void iwl4965_hw_rx_statistics(struct iwl_priv *priv,
        priv->temperature = temp;
        set_bit(STATUS_TEMPERATURE, &priv->status);
-        if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+        if (!priv->disable_tx_power_cal &&
-                     iwl4965_is_temp_calib_needed(priv))
+             unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+             iwl4965_is_temp_calib_needed(priv))
                queue_work(priv->workqueue, &priv->txpower_work);
 }
@@ -2552,7 +2388,7 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
        u32 print_dump = 0;     /* set to 1 to dump all frames' contents */
        u32 hundred = 0;
        u32 dataframe = 0;
-        u16 fc;
+        __le16 fc;
        u16 seq_ctl;
        u16 channel;
        u16 phy_flags;
@@ -2575,7 +2411,7 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
                return;
        /* MAC header */
-        fc = le16_to_cpu(header->frame_control);
+        fc = header->frame_control;
        seq_ctl = le16_to_cpu(header->seq_ctrl);
        /* metadata */
@@ -2600,8 +2436,8 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
        /* if data frame is to us and all is good,
         *   (optionally) print summary for only 1 out of every 100 */
-        if (to_us && (fc & ~IEEE80211_FCTL_PROTECTED) ==
+        if (to_us && (fc & ~cpu_to_le16(IEEE80211_FCTL_PROTECTED)) ==
-            (IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
+            cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
                dataframe = 1;
                if (!group100)
                        print_summary = 1;      /* print each frame */
@@ -2625,13 +2461,13 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
                if (hundred)
                        title = "100Frames";
-                else if (fc & IEEE80211_FCTL_RETRY)
+                else if (ieee80211_has_retry(fc))
                        title = "Retry";
-                else if (ieee80211_is_assoc_response(fc))
+                else if (ieee80211_is_assoc_resp(fc))
                        title = "AscRsp";
-                else if (ieee80211_is_reassoc_response(fc))
+                else if (ieee80211_is_reassoc_resp(fc))
                        title = "RasRsp";
-                else if (ieee80211_is_probe_response(fc)) {
+                else if (ieee80211_is_probe_resp(fc)) {
                        title = "PrbRsp";
                        print_dump = 1; /* dump frame contents */
                } else if (ieee80211_is_beacon(fc)) {
@@ -2648,7 +2484,7 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
                else
                        title = "Frame";
-                rate_idx = iwl4965_hwrate_to_plcp_idx(rate_sym);
+                rate_idx = iwl_hwrate_to_plcp_idx(rate_sym);
                if (unlikely(rate_idx == -1))
                        bitrate = 0;
                else
@@ -2660,14 +2496,14 @@ static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
                if (dataframe)
                        IWL_DEBUG_RX("%s: mhd=0x%04x, dst=0x%02x, "
                                     "len=%u, rssi=%d, chnl=%d, rate=%u, \n",
-                                     title, fc, header->addr1[5],
+                                     title, le16_to_cpu(fc), header->addr1[5],
                                     length, rssi, channel, bitrate);
                else {
                        /* src/dst addresses assume managed mode */
                        IWL_DEBUG_RX("%s: 0x%04x, dst=0x%02x, "
                                     "src=0x%02x, rssi=%u, tim=%lu usec, "
                                     "phy=0x%02x, chnl=%d\n",
-                                     title, fc, header->addr1[5],
+                                     title, le16_to_cpu(fc), header->addr1[5],
                                     header->addr3[5], rssi,
                                     tsf_low - priv->scan_start_tsf,
                                     phy_flags, channel);
@@ -2713,7 +2549,7 @@ void iwl4965_rx_reply_rx(struct iwl_priv *priv,
        rx_status.band = (rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
                                IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
        rx_status.rate_idx =
-                iwl4965_hwrate_to_plcp_idx(le32_to_cpu(rx_start->rate_n_flags));
+                iwl_hwrate_to_plcp_idx(le32_to_cpu(rx_start->rate_n_flags));
        if (rx_status.band == IEEE80211_BAND_5GHZ)
                rx_status.rate_idx -= IWL_FIRST_OFDM_RATE;
@@ -2819,7 +2655,6 @@ void iwl4965_rx_reply_rx(struct iwl_priv *priv,
                break;
        case IEEE80211_FTYPE_CTL:
-#ifdef CONFIG_IWL4965_HT
                switch (fc & IEEE80211_FCTL_STYPE) {
                case IEEE80211_STYPE_BACK_REQ:
                        IWL_DEBUG_HT("IEEE80211_STYPE_BACK_REQ arrived\n");
@@ -2829,7 +2664,6 @@ void iwl4965_rx_reply_rx(struct iwl_priv *priv,
                default:
                        break;
                }
-#endif
                break;
        case IEEE80211_FTYPE_DATA: {
@@ -2863,8 +2697,6 @@ void iwl4965_rx_reply_rx(struct iwl_priv *priv,
        }
 }
-#ifdef CONFIG_IWL4965_HT
 /**
 * iwl4965_tx_status_reply_compressed_ba - Update tx status from block-ack
 *
@@ -2926,7 +2758,7 @@ static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
        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_hwrate_to_tx_control(priv, agg->rate_n_flags, info);
        IWL_DEBUG_TX_REPLY("Bitmap %llx\n", (unsigned long long)bitmap);
@@ -2948,7 +2780,7 @@ static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv,
 }
 /**
- * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
+ * txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE
 * priv->lock must be held by the caller
 */
 static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
@@ -2956,9 +2788,9 @@ static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
 {
        int ret = 0;
-        if (IWL_BACK_QUEUE_FIRST_ID > txq_id) {
+        if (IWL49_FIRST_AMPDU_QUEUE > txq_id) {
                IWL_WARNING("queue number too small: %d, must be > %d\n",
-                                txq_id, IWL_BACK_QUEUE_FIRST_ID);
+                                txq_id, IWL49_FIRST_AMPDU_QUEUE);
                return -EINVAL;
        }
@@ -3046,7 +2878,7 @@ static void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
        if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff)) {
                /* calculate mac80211 ampdu sw queue to wake */
                int ampdu_q =
-                   scd_flow - IWL_BACK_QUEUE_FIRST_ID + priv->hw->queues;
+                   scd_flow - priv->hw_params.first_ampdu_q + priv->hw->queues;
                int freed = iwl_tx_queue_reclaim(priv, scd_flow, index);
                priv->stations[ba_resp->sta_id].
                        tid[ba_resp->tid].tfds_in_queue -= freed;
@@ -3091,7 +2923,7 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
 /**
 * iwl4965_tx_queue_agg_enable - Set up & enable aggregation for selected queue
 *
- * NOTE:  txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID,
+ * NOTE:  txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE,
 *        i.e. it must be one of the higher queues used for aggregation
 */
 static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
@@ -3101,9 +2933,9 @@ static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
        int ret;
        u16 ra_tid;
-        if (IWL_BACK_QUEUE_FIRST_ID > txq_id)
+        if (IWL49_FIRST_AMPDU_QUEUE > txq_id)
                IWL_WARNING("queue number too small: %d, must be > %d\n",
-                        txq_id, IWL_BACK_QUEUE_FIRST_ID);
+                        txq_id, IWL49_FIRST_AMPDU_QUEUE);
        ra_tid = BUILD_RAxTID(sta_id, tid);
@@ -3154,10 +2986,6 @@ static int iwl4965_txq_agg_enable(struct iwl_priv *priv, int txq_id,
        return 0;
 }
-#endif /* CONFIG_IWL4965_HT */
-#ifdef CONFIG_IWL4965_HT
 static int iwl4965_rx_agg_start(struct iwl_priv *priv,
                                const u8 *addr, int tid, u16 ssn)
 {
@@ -3231,8 +3059,6 @@ int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
        }
        return 0;
 }
-#endif /* CONFIG_IWL4965_HT */
 static u16 iwl4965_get_hcmd_size(u8 cmd_id, u16 len)
 {
@@ -3262,13 +3088,9 @@ 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 +
+        return le32_to_cpup(&tx_resp->u.status + tx_resp->frame_count) & MAX_SN;
-                                tx_resp->frame_count);
-        return le32_to_cpu(*scd_ssn) & MAX_SN;
 }
 /**
@@ -3276,32 +3098,29 @@ static inline u32 iwl4965_get_scd_ssn(struct iwl4965_tx_resp *tx_resp)
 */
 static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
                                      struct iwl_ht_agg *agg,
-                                      struct iwl4965_tx_resp_agg *tx_resp,
+                                      struct iwl4965_tx_resp *tx_resp,
-                                      u16 start_idx)
+                                      int txq_id, u16 start_idx)
 {
        u16 status;
-        struct agg_tx_status *frame_status = &tx_resp->status;
+        struct agg_tx_status *frame_status = tx_resp->u.agg_status;
        struct ieee80211_tx_info *info = NULL;
        struct ieee80211_hdr *hdr = NULL;
-        int i, sh;
+        u32 rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
-        int txq_id, idx;
+        int i, sh, 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->rate_n_flags = 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 = start_idx;
-                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",
@@ -3312,15 +3131,12 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
                info->flags &= ~IEEE80211_TX_CTL_AMPDU;
                info->flags |= iwl_is_tx_success(status)?
                        IEEE80211_TX_STAT_ACK : 0;
-                iwl4965_hwrate_to_tx_control(priv,
+                iwl_hwrate_to_tx_control(priv, rate_n_flags, info);
-                                             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",
+                IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n", rate_n_flags);
-                        iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
                agg->wait_for_ba = 0;
        } else {
@@ -3378,7 +3194,6 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
                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);
@@ -3388,7 +3203,6 @@ static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
        }
        return 0;
 }
-#endif
 /**
 * iwl4965_rx_reply_tx - Handle standard (non-aggregation) Tx response
@@ -3403,13 +3217,11 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
        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);
+        u32  status = le32_to_cpu(tx_resp->u.status);
-#ifdef CONFIG_IWL4965_HT
        int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
-        u16 fc;
+        __le16 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 "
@@ -3422,11 +3234,10 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
        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);
+        fc = hdr->frame_control;
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                qc = ieee80211_get_qos_ctl(hdr);
                tid = qc[0] & 0xf;
        }
@@ -3445,8 +3256,7 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
                agg = &priv->stations[sta_id].tid[tid].agg;
-                iwl4965_tx_status_reply_tx(priv, agg,
+                iwl4965_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
-                                (struct iwl4965_tx_resp_agg *)tx_resp, index);
                if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status)) {
                        /* TODO: send BAR */
@@ -3464,7 +3274,7 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
                            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 +
+                                ampdu_q = txq_id - IWL49_FIRST_AMPDU_QUEUE +
                                          priv->hw->queues;
                                if (agg->state == IWL_AGG_OFF)
                                        ieee80211_wake_queue(priv->hw, txq_id);
@@ -3474,32 +3284,32 @@ static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
                        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 |=
-        info->status.retry_count = tx_resp->failure_frame;
+                        iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
-        info->flags |= iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+                iwl_hwrate_to_tx_control(priv,
-        iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
+                                        le32_to_cpu(tx_resp->rate_n_flags),
-                                     info);
+                                        info);
-        IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
+                IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags "
-                     "retries %d\n", txq_id, iwl_get_tx_fail_reason(status),
+                             "0x%x retries %d\n", txq_id,
-                     status, le32_to_cpu(tx_resp->rate_n_flags),
+                                iwl_get_tx_fail_reason(status),
-                     tx_resp->failure_frame);
+                                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
+                IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
-        if (index != -1) {
-                int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                if (index != -1) {
-                if (tid != MAX_TID_COUNT)
+                    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 &&
+                    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)
+                    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");
@@ -3513,28 +3323,18 @@ static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
        priv->rx_handlers[REPLY_RX] = iwl4965_rx_reply_rx;
        /* Tx response */
        priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
+        /* block ack */
-#ifdef CONFIG_IWL4965_HT
        priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
-#endif /* CONFIG_IWL4965_HT */
 }
-void iwl4965_hw_setup_deferred_work(struct iwl_priv *priv)
+static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
 {
        INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
-        INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
-#endif
-        init_timer(&priv->statistics_periodic);
-        priv->statistics_periodic.data = (unsigned long)priv;
-        priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
 }
-void iwl4965_hw_cancel_deferred_work(struct iwl_priv *priv)
+static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
 {
-        del_timer_sync(&priv->statistics_periodic);
+        cancel_work_sync(&priv->txpower_work);
-        cancel_delayed_work(&priv->init_alive_start);
 }
@@ -3545,10 +3345,8 @@ static struct iwl_hcmd_ops iwl4965_hcmd = {
 static struct iwl_hcmd_utils_ops iwl4965_hcmd_utils = {
        .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,
        .gain_computation = iwl4965_gain_computation,
-#endif
 };
 static struct iwl_lib_ops iwl4965_lib = {
@@ -3558,11 +3356,11 @@ static struct iwl_lib_ops iwl4965_lib = {
        .shared_mem_rx_idx = iwl4965_shared_mem_rx_idx,
        .txq_update_byte_cnt_tbl = iwl4965_txq_update_byte_cnt_tbl,
        .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,
+        .setup_deferred_work = iwl4965_setup_deferred_work,
+        .cancel_deferred_work = iwl4965_cancel_deferred_work,
        .is_valid_rtc_data_addr = iwl4965_hw_valid_rtc_data_addr,
        .alive_notify = iwl4965_alive_notify,
        .init_alive_start = iwl4965_init_alive_start,
@@ -3590,8 +3388,8 @@ static struct iwl_lib_ops iwl4965_lib = {
                .check_version = iwl4965_eeprom_check_version,
                .query_addr = iwlcore_eeprom_query_addr,
        },
-        .radio_kill_sw = iwl4965_radio_kill_sw,
        .set_power = iwl4965_set_power,
+        .send_tx_power  = iwl4965_send_tx_power,
        .update_chain_flags = iwl4965_update_chain_flags,
 };
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000-hw.h b/drivers/net/wireless/iwlwifi/iwl-5000-hw.h
index 9e557ce315b7..4efe0c06b5b2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-5000-hw.h
@@ -81,7 +81,7 @@
 #define IWL50_QUEUE_SIZE                 256
 #define IWL50_CMD_FIFO_NUM                 7
 #define IWL50_NUM_QUEUES                  20
-#define IWL50_BACK_QUEUE_FIRST_ID         10
+#define IWL50_FIRST_AMPDU_QUEUE           10
 #define IWL_sta_id_POS 12
 #define IWL_sta_id_LEN 4
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
index 7e525ad45135..438c3812c390 100644
--- a/drivers/net/wireless/iwlwifi/iwl-5000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -41,6 +41,7 @@
 #include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-io.h"
+#include "iwl-sta.h"
 #include "iwl-helpers.h"
 #include "iwl-5000-hw.h"
@@ -300,8 +301,6 @@ err:
 }
-#ifdef CONFIG_IWL5000_RUN_TIME_CALIB
 static void iwl5000_gain_computation(struct iwl_priv *priv,
                u32 average_noise[NUM_RX_CHAINS],
                u16 min_average_noise_antenna_i,
@@ -354,7 +353,6 @@ static void iwl5000_gain_computation(struct iwl_priv *priv,
        data->beacon_count = 0;
 }
 static void iwl5000_chain_noise_reset(struct iwl_priv *priv)
 {
        struct iwl_chain_noise_data *data = &priv->chain_noise_data;
@@ -393,10 +391,6 @@ static struct iwl_sensitivity_ranges iwl5000_sensitivity = {
        .nrg_th_ofdm = 95,
 };
-#endif /* CONFIG_IWL5000_RUN_TIME_CALIB */
 static const u8 *iwl5000_eeprom_query_addr(const struct iwl_priv *priv,
                                           size_t offset)
 {
@@ -832,6 +826,7 @@ 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.first_ampdu_q = IWL50_FIRST_AMPDU_QUEUE;
        priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
        priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
        priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
@@ -847,9 +842,7 @@ static int iwl5000_hw_set_hw_params(struct iwl_priv *priv)
        priv->hw_params.max_bsm_size = BSM_SRAM_SIZE;
        priv->hw_params.fat_channel =  BIT(IEEE80211_BAND_2GHZ) |
                                        BIT(IEEE80211_BAND_5GHZ);
-#ifdef CONFIG_IWL5000_RUN_TIME_CALIB
        priv->hw_params.sens = &iwl5000_sensitivity;
-#endif
        switch (priv->hw_rev & CSR_HW_REV_TYPE_MSK) {
        case CSR_HW_REV_TYPE_5100:
@@ -984,6 +977,135 @@ static void iwl5000_txq_inval_byte_cnt_tbl(struct iwl_priv *priv,
        }
 }
+static int iwl5000_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
+                                        u16 txq_id)
+{
+        u32 tbl_dw_addr;
+        u32 tbl_dw;
+        u16 scd_q2ratid;
+        scd_q2ratid = ra_tid & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+        tbl_dw_addr = priv->scd_base_addr +
+                        IWL50_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
+        tbl_dw = iwl_read_targ_mem(priv, tbl_dw_addr);
+        if (txq_id & 0x1)
+                tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
+        else
+                tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);
+        iwl_write_targ_mem(priv, tbl_dw_addr, tbl_dw);
+        return 0;
+}
+static void iwl5000_tx_queue_stop_scheduler(struct iwl_priv *priv, u16 txq_id)
+{
+        /* Simply stop the queue, but don't change any configuration;
+         * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
+        iwl_write_prph(priv,
+                IWL50_SCD_QUEUE_STATUS_BITS(txq_id),
+                (0 << IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+                (1 << IWL50_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
+static int iwl5000_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 ret;
+        u16 ra_tid;
+        if (IWL50_FIRST_AMPDU_QUEUE > txq_id)
+                IWL_WARNING("queue number too small: %d, must be > %d\n",
+                        txq_id, IWL50_FIRST_AMPDU_QUEUE);
+        ra_tid = BUILD_RAxTID(sta_id, tid);
+        /* Modify device's station table to Tx this TID */
+        iwl_sta_modify_enable_tid_tx(priv, sta_id, tid);
+        spin_lock_irqsave(&priv->lock, flags);
+        ret = iwl_grab_nic_access(priv);
+        if (ret) {
+                spin_unlock_irqrestore(&priv->lock, flags);
+                return ret;
+        }
+        /* Stop this Tx queue before configuring it */
+        iwl5000_tx_queue_stop_scheduler(priv, txq_id);
+        /* Map receiver-address / traffic-ID to this queue */
+        iwl5000_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
+        /* Set this queue as a chain-building queue */
+        iwl_set_bits_prph(priv, IWL50_SCD_QUEUECHAIN_SEL, (1<<txq_id));
+        /* enable aggregations for the queue */
+        iwl_set_bits_prph(priv, IWL50_SCD_AGGR_SEL, (1<<txq_id));
+        /* Place first TFD at index corresponding to start sequence number.
+         * Assumes that ssn_idx is valid (!= 0xFFF) */
+        priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+        priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+        iwl5000_set_wr_ptrs(priv, txq_id, ssn_idx);
+        /* Set up Tx window size and frame limit for this queue */
+        iwl_write_targ_mem(priv, priv->scd_base_addr +
+                        IWL50_SCD_CONTEXT_QUEUE_OFFSET(txq_id) +
+                        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_set_bits_prph(priv, IWL50_SCD_INTERRUPT_MASK, (1 << txq_id));
+        /* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
+        iwl5000_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
+        iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+        return 0;
+}
+static int iwl5000_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
+                                   u16 ssn_idx, u8 tx_fifo)
+{
+        int ret;
+        if (IWL50_FIRST_AMPDU_QUEUE > txq_id) {
+                IWL_WARNING("queue number too small: %d, must be > %d\n",
+                                txq_id, IWL50_FIRST_AMPDU_QUEUE);
+                return -EINVAL;
+        }
+        ret = iwl_grab_nic_access(priv);
+        if (ret)
+                return ret;
+        iwl5000_tx_queue_stop_scheduler(priv, txq_id);
+        iwl_clear_bits_prph(priv, IWL50_SCD_AGGR_SEL, (1 << txq_id));
+        priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+        priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+        /* supposes that ssn_idx is valid (!= 0xFFF) */
+        iwl5000_set_wr_ptrs(priv, txq_id, ssn_idx);
+        iwl_clear_bits_prph(priv, IWL50_SCD_INTERRUPT_MASK, (1 << txq_id));
+        iwl_txq_ctx_deactivate(priv, txq_id);
+        iwl5000_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
+        iwl_release_nic_access(priv);
+        return 0;
+}
 static u16 iwl5000_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 {
        u16 size = (u16)sizeof(struct iwl_addsta_cmd);
@@ -1004,23 +1126,21 @@ static void iwl5000_txq_set_sched(struct iwl_priv *priv, u32 mask)
 static inline u32 iwl5000_get_scd_ssn(struct iwl5000_tx_resp *tx_resp)
 {
-        __le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
+        return le32_to_cpup((__le32*)&tx_resp->status +
-                                tx_resp->frame_count);
+                            tx_resp->frame_count) & MAX_SN;
-        return le32_to_cpu(*scd_ssn) & MAX_SN;
 }
 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)
+                                      int txq_id, 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;
+        u32 rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
-        int txq_id, idx;
+        int i, sh, idx;
        u16 seq;
        if (agg->wait_for_ba)
@@ -1028,16 +1148,14 @@ static int iwl5000_tx_status_reply_tx(struct iwl_priv *priv,
        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->rate_n_flags = 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 = start_idx;
-                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",
@@ -1048,15 +1166,13 @@ static int iwl5000_tx_status_reply_tx(struct iwl_priv *priv,
                info->flags &= ~IEEE80211_TX_CTL_AMPDU;
                info->flags |= iwl_is_tx_success(status)?
                        IEEE80211_TX_STAT_ACK : 0;
-                iwl4965_hwrate_to_tx_control(priv,
+                iwl_hwrate_to_tx_control(priv, rate_n_flags, info);
-                                             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",
+                IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n", rate_n_flags);
-                        iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
                agg->wait_for_ba = 0;
        } else {
@@ -1114,7 +1230,6 @@ static int iwl5000_tx_status_reply_tx(struct iwl_priv *priv,
                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);
@@ -1136,12 +1251,9 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
        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 "
@@ -1154,11 +1266,9 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
        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_data_qos(hdr->frame_control)) {
-        if (ieee80211_is_qos_data(fc)) {
+                qc = ieee80211_get_qos_ctl(hdr);
-                qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
                tid = qc[0] & 0xf;
        }
@@ -1177,7 +1287,7 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
                agg = &priv->stations[sta_id].tid[tid].agg;
-                iwl5000_tx_status_reply_tx(priv, agg, tx_resp, index);
+                iwl5000_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
                if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status)) {
                        /* TODO: send BAR */
@@ -1195,7 +1305,7 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
                            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 +
+                                ampdu_q = txq_id - IWL50_FIRST_AMPDU_QUEUE +
                                          priv->hw->queues;
                                if (agg->state == IWL_AGG_OFF)
                                        ieee80211_wake_queue(priv->hw, txq_id);
@@ -1205,32 +1315,31 @@ static void iwl5000_rx_reply_tx(struct iwl_priv *priv,
                        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 =
-        info->status.retry_count = tx_resp->failure_frame;
+                        iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
-        info->flags = iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+                iwl_hwrate_to_tx_control(priv,
-        iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
+                                        le32_to_cpu(tx_resp->rate_n_flags),
-                                     info);
+                                        info);
-        IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
+                IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags "
-                     "retries %d\n", txq_id, iwl_get_tx_fail_reason(status),
+                             "0x%x retries %d\n", txq_id,
-                     status, le32_to_cpu(tx_resp->rate_n_flags),
+                                iwl_get_tx_fail_reason(status),
-                     tx_resp->failure_frame);
+                                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
+                IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
-        if (index != -1) {
+                if (index != -1) {
-                int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+                    int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
-                if (tid != MAX_TID_COUNT)
+                    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 &&
+                    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)
+                    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");
@@ -1242,6 +1351,12 @@ static u16 iwl5000_get_hcmd_size(u8 cmd_id, u16 len)
        return len;
 }
+static void iwl5000_setup_deferred_work(struct iwl_priv *priv)
+{
+        /* in 5000 the tx power calibration is done in uCode */
+        priv->disable_tx_power_cal = 1;
+}
 static void iwl5000_rx_handler_setup(struct iwl_priv *priv)
 {
        /* init calibration handlers */
@@ -1305,6 +1420,19 @@ static int iwl5000_send_rxon_assoc(struct iwl_priv *priv)
        return ret;
 }
+static int  iwl5000_send_tx_power(struct iwl_priv *priv)
+{
+        struct iwl5000_tx_power_dbm_cmd tx_power_cmd;
+        /* half dBm need to multiply */
+        tx_power_cmd.global_lmt = (s8)(2 * priv->tx_power_user_lmt);
+        tx_power_cmd.flags = 0;
+        tx_power_cmd.srv_chan_lmt = IWL50_TX_POWER_AUTO;
+        return  iwl_send_cmd_pdu_async(priv, REPLY_TX_POWER_DBM_CMD,
+                                       sizeof(tx_power_cmd), &tx_power_cmd,
+                                       NULL);
+}
 static struct iwl_hcmd_ops iwl5000_hcmd = {
        .rxon_assoc = iwl5000_send_rxon_assoc,
@@ -1313,10 +1441,8 @@ static struct iwl_hcmd_ops iwl5000_hcmd = {
 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,
        .chain_noise_reset = iwl5000_chain_noise_reset,
-#endif
 };
 static struct iwl_lib_ops iwl5000_lib = {
@@ -1327,11 +1453,15 @@ static struct iwl_lib_ops iwl5000_lib = {
        .txq_update_byte_cnt_tbl = iwl5000_txq_update_byte_cnt_tbl,
        .txq_inval_byte_cnt_tbl = iwl5000_txq_inval_byte_cnt_tbl,
        .txq_set_sched = iwl5000_txq_set_sched,
+        .txq_agg_enable = iwl5000_txq_agg_enable,
+        .txq_agg_disable = iwl5000_txq_agg_disable,
        .rx_handler_setup = iwl5000_rx_handler_setup,
+        .setup_deferred_work = iwl5000_setup_deferred_work,
        .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,
+        .send_tx_power = iwl5000_send_tx_power,
        .apm_ops = {
                .init = iwl5000_apm_init,
                .reset = iwl5000_apm_reset,
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.c b/drivers/net/wireless/iwlwifi/iwl-calib.c
index a6c7f0d9a414..48f58000b64b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-calib.c
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.c
@@ -60,13 +60,11 @@
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/
-#include <linux/kernel.h>
 #include <net/mac80211.h>
 #include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-calib.h"
-#include "iwl-eeprom.h"
 /* "false alarms" are signals that our DSP tries to lock onto,
 *   but then determines that they are either noise, or transmissions
@@ -435,8 +433,6 @@ void iwl_init_sensitivity(struct iwl_priv *priv)
        data = &(priv->sensitivity_data);
        if (ranges == NULL)
-                /* can happen if IWLWIFI_RUN_TIME_CALIB is selected
-                 * but no IWLXXXX_RUN_TIME_CALIB for specific is selected */
                return;
        memset(data, 0, sizeof(struct iwl_sensitivity_data));
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.h b/drivers/net/wireless/iwlwifi/iwl-calib.h
index b8e57c59eac8..5524a29e22d8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-calib.h
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.h
@@ -62,16 +62,10 @@
 #ifndef __iwl_calib_h__
 #define __iwl_calib_h__
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/version.h>
-#include <net/mac80211.h>
-#include "iwl-eeprom.h"
-#include "iwl-core.h"
 #include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-commands.h"
-#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
 void iwl_chain_noise_calibration(struct iwl_priv *priv,
                                struct iwl4965_notif_statistics *stat_resp);
 void iwl_sensitivity_calibration(struct iwl_priv *priv,
@@ -86,24 +80,5 @@ static inline void iwl_chain_noise_reset(struct iwl_priv *priv)
            priv->cfg->ops->utils->chain_noise_reset)
                priv->cfg->ops->utils->chain_noise_reset(priv);
 }
-#else
-static inline void iwl_chain_noise_calibration(struct iwl_priv *priv,
-                                struct iwl4965_notif_statistics *stat_resp)
-{
-}
-static inline void iwl_sensitivity_calibration(struct iwl_priv *priv,
-                                struct iwl4965_notif_statistics *resp)
-{
-}
-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 fb6f5ffb9f1d..920dfc1b2db2 100644
--- a/drivers/net/wireless/iwlwifi/iwl-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -126,6 +126,7 @@ enum {
        /* Miscellaneous commands */
        QUIET_NOTIFICATION = 0x96,              /* not used */
        REPLY_TX_PWR_TABLE_CMD = 0x97,
+        REPLY_TX_POWER_DBM_CMD = 0x98,
        MEASURE_ABORT_NOTIFICATION = 0x99,      /* not used */
        /* Bluetooth device coexistance config command */
@@ -280,16 +281,7 @@ struct iwl_cmd_header {
 #define RATE_MCS_ANT_C_MSK    0x10000
 #define RATE_MCS_ANT_ABC_MSK  0x1C000
+#define RATE_MCS_ANT_INIT_IND   1
-/**
- * struct iwl4965_tx_power - txpower format used in REPLY_SCAN_CMD
- *
- * Scan uses only one transmitter, so only one analog/dsp gain pair is needed.
- */
-struct iwl4965_tx_power {
-        u8 tx_gain;             /* gain for analog radio */
-        u8 dsp_atten;           /* gain for DSP */
-} __attribute__ ((packed));
 #define POWER_TABLE_NUM_ENTRIES                 33
 #define POWER_TABLE_NUM_HT_OFDM_ENTRIES         32
@@ -339,6 +331,17 @@ struct iwl4965_tx_power_db {
        struct tx_power_dual_stream power_tbl[POWER_TABLE_NUM_ENTRIES];
 } __attribute__ ((packed));
+/**
+ * Commad REPLY_TX_POWER_DBM_CMD = 0x98
+ * struct iwl5000_tx_power_dbm_cmd
+ */
+#define IWL50_TX_POWER_AUTO 0x7f
+struct iwl5000_tx_power_dbm_cmd {
+        s8 global_lmt; /*in half-dBm (e.g. 30 = 15 dBm) */
+        u8 flags;
+        s8 srv_chan_lmt; /*in half-dBm (e.g. 30 = 15 dBm) */
+        u8 reserved;
+} __attribute__ ((packed));
 /******************************************************************************
 * (0a)
@@ -990,6 +993,7 @@ struct iwl_wep_cmd {
 #define WEP_KEY_WEP_TYPE 1
 #define WEP_KEYS_MAX 4
 #define WEP_INVALID_OFFSET 0xff
+#define WEP_KEY_LEN_64 5
 #define WEP_KEY_LEN_128 13
 /******************************************************************************
@@ -1481,21 +1485,10 @@ struct iwl4965_tx_resp {
         *                   table entry used for all frames in the new agg.
         *           31-16:  Sequence # for this frame's Tx cmd (not SSN!)
         */
-        __le32 status;  /* TX status (for aggregation status of 1st frame) */
+        union {
-} __attribute__ ((packed));
+                __le32 status;
+                struct agg_tx_status agg_status[0]; /* for each agg frame */
-struct iwl4965_tx_resp_agg {
+        } u;
-        u8 frame_count;         /* 1 no aggregation, >1 aggregation */
-        u8 reserved1;
-        u8 failure_rts;
-        u8 failure_frame;
-        __le32 rate_n_flags;
-        __le16 wireless_media_time;
-        __le16 reserved3;
-        __le32 pa_power1;
-        __le32 pa_power2;
-        struct agg_tx_status status;    /* TX status (for aggregation status */
-                                        /* of 1st frame) */
 } __attribute__ ((packed));
 struct iwl5000_tx_resp {
@@ -2085,7 +2078,7 @@ struct iwl4965_card_state_notif {
 #define RF_CARD_DISABLED   0x04
 #define RXON_CARD_DISABLED 0x10
-struct iwl4965_ct_kill_config {
+struct iwl_ct_kill_config {
        __le32   reserved;
        __le32   critical_temperature_M;
        __le32   critical_temperature_R;
@@ -2098,7 +2091,7 @@ struct iwl4965_ct_kill_config {
 *****************************************************************************/
 /**
- * struct iwl4965_scan_channel - entry in REPLY_SCAN_CMD channel table
+ * struct iwl_scan_channel - entry in REPLY_SCAN_CMD channel table
 *
 * One for each channel in the scan list.
 * Each channel can independently select:
@@ -2108,7 +2101,7 @@ struct iwl4965_ct_kill_config {
 *     quiet_plcp_th, good_CRC_th)
 *
 * To avoid uCode errors, make sure the following are true (see comments
- * under struct iwl4965_scan_cmd about max_out_time and quiet_time):
+ * under struct iwl_scan_cmd about max_out_time and quiet_time):
 * 1)  If using passive_dwell (i.e. passive_dwell != 0):
 *     active_dwell <= passive_dwell (< max_out_time if max_out_time != 0)
 * 2)  quiet_time <= active_dwell
@@ -2116,7 +2109,7 @@ struct iwl4965_ct_kill_config {
 *     passive_dwell < max_out_time
 *     active_dwell < max_out_time
 */
-struct iwl4965_scan_channel {
+struct iwl_scan_channel {
        /*
         * type is defined as:
         * 0:0 1 = active, 0 = passive
@@ -2126,19 +2119,20 @@ struct iwl4965_scan_channel {
         */
        u8 type;
        u8 channel;     /* band is selected by iwl4965_scan_cmd "flags" field */
-        struct iwl4965_tx_power tpc;
+        u8 tx_gain;             /* gain for analog radio */
+        u8 dsp_atten;           /* gain for DSP */
        __le16 active_dwell;    /* in 1024-uSec TU (time units), typ 5-50 */
        __le16 passive_dwell;   /* in 1024-uSec TU (time units), typ 20-500 */
 } __attribute__ ((packed));
 /**
- * struct iwl4965_ssid_ie - directed scan network information element
+ * struct iwl_ssid_ie - directed scan network information element
 *
 * Up to 4 of these may appear in REPLY_SCAN_CMD, selected by "type" field
 * in struct iwl4965_scan_channel; each channel may select different ssids from
 * among the 4 entries.  SSID IEs get transmitted in reverse order of entry.
 */
-struct iwl4965_ssid_ie {
+struct iwl_ssid_ie {
        u8 id;
        u8 len;
        u8 ssid[32];
@@ -2199,9 +2193,9 @@ struct iwl4965_ssid_ie {
 * Driver must use separate scan commands for 2.4 vs. 5 GHz bands.
 *
 * To avoid uCode errors, see timing restrictions described under
- * struct iwl4965_scan_channel.
+ * struct iwl_scan_channel.
 */
-struct iwl4965_scan_cmd {
+struct iwl_scan_cmd {
        __le16 len;
        u8 reserved0;
        u8 channel_count;       /* # channels in channel list */
@@ -2225,7 +2219,7 @@ struct iwl4965_scan_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];
+        struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX];
        /*
         * Probe request frame, followed by channel list.
@@ -2253,14 +2247,14 @@ struct iwl4965_scan_cmd {
 /*
 * REPLY_SCAN_CMD = 0x80 (response)
 */
-struct iwl4965_scanreq_notification {
+struct iwl_scanreq_notification {
        __le32 status;          /* 1: okay, 2: cannot fulfill request */
 } __attribute__ ((packed));
 /*
 * SCAN_START_NOTIFICATION = 0x82 (notification only, not a command)
 */
-struct iwl4965_scanstart_notification {
+struct iwl_scanstart_notification {
        __le32 tsf_low;
        __le32 tsf_high;
        __le32 beacon_timer;
@@ -2277,7 +2271,7 @@ struct iwl4965_scanstart_notification {
 /*
 * SCAN_RESULTS_NOTIFICATION = 0x83 (notification only, not a command)
 */
-struct iwl4965_scanresults_notification {
+struct iwl_scanresults_notification {
        u8 channel;
        u8 band;
        u8 reserved[2];
@@ -2289,7 +2283,7 @@ struct iwl4965_scanresults_notification {
 /*
 * SCAN_COMPLETE_NOTIFICATION = 0x84 (notification only, not a command)
 */
-struct iwl4965_scancomplete_notification {
+struct iwl_scancomplete_notification {
        u8 scanned_channels;
        u8 status;
        u8 reserved;
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 61716ba90427..eb74a40a62eb 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -85,6 +85,63 @@ const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT] = {
 };
 EXPORT_SYMBOL(iwl_rates);
+/**
+ * translate ucode response to mac80211 tx status control values
+ */
+void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+                                  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_TX_CTL_OFDM_HT;
+        if (rate_n_flags & RATE_MCS_GF_MSK)
+                control->flags |= IEEE80211_TX_CTL_GREEN_FIELD;
+        if (rate_n_flags & RATE_MCS_FAT_MSK)
+                control->flags |= IEEE80211_TX_CTL_40_MHZ_WIDTH;
+        if (rate_n_flags & RATE_MCS_DUP_MSK)
+                control->flags |= IEEE80211_TX_CTL_DUP_DATA;
+        if (rate_n_flags & RATE_MCS_SGI_MSK)
+                control->flags |= IEEE80211_TX_CTL_SHORT_GI;
+        rate_index = iwl_hwrate_to_plcp_idx(rate_n_flags);
+        if (control->band == IEEE80211_BAND_5GHZ)
+                rate_index -= IWL_FIRST_OFDM_RATE;
+        control->tx_rate_idx = rate_index;
+}
+EXPORT_SYMBOL(iwl_hwrate_to_tx_control);
+int iwl_hwrate_to_plcp_idx(u32 rate_n_flags)
+{
+        int idx = 0;
+        /* HT rate format */
+        if (rate_n_flags & RATE_MCS_HT_MSK) {
+                idx = (rate_n_flags & 0xff);
+                if (idx >= IWL_RATE_MIMO2_6M_PLCP)
+                        idx = idx - IWL_RATE_MIMO2_6M_PLCP;
+                idx += IWL_FIRST_OFDM_RATE;
+                /* skip 9M not supported in ht*/
+                if (idx >= IWL_RATE_9M_INDEX)
+                        idx += 1;
+                if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
+                        return idx;
+        /* legacy rate format, search for match in table */
+        } else {
+                for (idx = 0; idx < ARRAY_SIZE(iwl_rates); idx++)
+                        if (iwl_rates[idx].plcp == (rate_n_flags & 0xFF))
+                                return idx;
+        }
+        return -1;
+}
+EXPORT_SYMBOL(iwl_hwrate_to_plcp_idx);
 const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 EXPORT_SYMBOL(iwl_bcast_addr);
@@ -321,7 +378,6 @@ 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,
@@ -374,13 +430,6 @@ static void iwlcore_init_ht_hw_capab(const struct iwl_priv *priv,
                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,
-                              struct ieee80211_ht_info *ht_info,
-                              enum ieee80211_band band)
-{
-}
-#endif /* CONFIG_IWL4965_HT */
 static void iwlcore_init_hw_rates(struct iwl_priv *priv,
                              struct ieee80211_rate *rates)
@@ -486,28 +535,10 @@ 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) {
+                        geo_ch->flags |= 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)
+                        if (ch->max_power_avg > priv->tx_power_channel_lmt)
-                                priv->max_channel_txpower_limit =
+                                priv->tx_power_channel_lmt = ch->max_power_avg;
-                                    ch->max_power_avg;
                } else {
                        geo_ch->flags |= IEEE80211_CHAN_DISABLED;
                }
@@ -515,7 +546,7 @@ static int iwlcore_init_geos(struct iwl_priv *priv)
                /* Save flags for reg domain usage */
                geo_ch->orig_flags = geo_ch->flags;
-                IWL_DEBUG_INFO("Channel %d Freq=%d[%sGHz] %s flag=0%X\n",
+                IWL_DEBUG_INFO("Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
                                ch->channel, geo_ch->center_freq,
                                is_channel_a_band(ch) ?  "5.2" : "2.4",
                                geo_ch->flags & IEEE80211_CHAN_DISABLED ?
@@ -553,7 +584,6 @@ static void iwlcore_free_geos(struct iwl_priv *priv)
        clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
 }
-#ifdef CONFIG_IWL4965_HT
 static u8 is_single_rx_stream(struct iwl_priv *priv)
 {
        return !priv->current_ht_config.is_ht ||
@@ -561,6 +591,7 @@ static u8 is_single_rx_stream(struct iwl_priv *priv)
                (priv->current_ht_config.supp_mcs_set[2] == 0)) ||
               priv->ps_mode == IWL_MIMO_PS_STATIC;
 }
 static u8 iwl_is_channel_extension(struct iwl_priv *priv,
                                   enum ieee80211_band band,
                                   u16 channel, u8 extension_chan_offset)
@@ -571,12 +602,12 @@ static u8 iwl_is_channel_extension(struct iwl_priv *priv,
        if (!is_channel_valid(ch_info))
                return 0;
-        if (extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_NONE)
+        if (extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_ABOVE)
-                return 0;
+                return !(ch_info->fat_extension_channel &
+                                        IEEE80211_CHAN_NO_FAT_ABOVE);
-        if ((ch_info->fat_extension_channel == extension_chan_offset) ||
+        else if (extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_BELOW)
-            (ch_info->fat_extension_channel == HT_IE_EXT_CHANNEL_MAX))
+                return !(ch_info->fat_extension_channel &
-                return 1;
+                                        IEEE80211_CHAN_NO_FAT_BELOW);
        return 0;
 }
@@ -588,7 +619,7 @@ u8 iwl_is_fat_tx_allowed(struct iwl_priv *priv,
        if ((!iwl_ht_conf->is_ht) ||
           (iwl_ht_conf->supported_chan_width != IWL_CHANNEL_WIDTH_40MHZ) ||
-           (iwl_ht_conf->extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_NONE))
+           (iwl_ht_conf->extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_NONE))
                return 0;
        if (sta_ht_inf) {
@@ -622,19 +653,18 @@ void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info)
                IWL_DEBUG_ASSOC("control diff than current %d %d\n",
                                le16_to_cpu(rxon->channel),
                                ht_info->control_channel);
-                rxon->channel = cpu_to_le16(ht_info->control_channel);
                return;
        }
        /* Note: control channel is opposite of extension channel */
        switch (ht_info->extension_chan_offset) {
-        case IWL_EXT_CHANNEL_OFFSET_ABOVE:
+        case IEEE80211_HT_IE_CHA_SEC_ABOVE:
                rxon->flags &= ~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
                break;
-        case IWL_EXT_CHANNEL_OFFSET_BELOW:
+        case IEEE80211_HT_IE_CHA_SEC_BELOW:
                rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
                break;
-        case IWL_EXT_CHANNEL_OFFSET_NONE:
+        case IEEE80211_HT_IE_CHA_SEC_NONE:
        default:
                rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
                break;
@@ -660,13 +690,6 @@ void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info)
 }
 EXPORT_SYMBOL(iwl_set_rxon_ht);
-#else
-static inline u8 is_single_rx_stream(struct iwl_priv *priv)
-{
-        return 1;
-}
-#endif  /*CONFIG_IWL4965_HT */
 /*
 * Determine how many receiver/antenna chains to use.
 * More provides better reception via diversity.  Fewer saves power.
@@ -791,10 +814,8 @@ int iwl_setup_mac(struct iwl_priv *priv)
                    IEEE80211_HW_NOISE_DBM;
        /* Default value; 4 EDCA QOS priorities */
        hw->queues = 4;
-#ifdef CONFIG_IWL4965_HT
        /* Enhanced value; more queues, to support 11n aggregation */
        hw->ampdu_queues = 12;
-#endif /* CONFIG_IWL4965_HT */
        hw->conf.beacon_int = 100;
@@ -853,6 +874,7 @@ int iwl_init_drv(struct iwl_priv *priv)
        /* Choose which receivers/antennas to use */
        iwl_set_rxon_chain(priv);
+        iwl_init_scan_params(priv);
        if (priv->cfg->mod_params->enable_qos)
                priv->qos_data.qos_enable = 1;
@@ -867,7 +889,7 @@ int iwl_init_drv(struct iwl_priv *priv)
        priv->rates_mask = IWL_RATES_MASK;
        /* If power management is turned on, default to AC mode */
        priv->power_mode = IWL_POWER_AC;
-        priv->user_txpower_limit = IWL_DEFAULT_TX_POWER;
+        priv->tx_power_user_lmt = IWL_TX_POWER_TARGET_POWER_MAX;
        ret = iwl_init_channel_map(priv);
        if (ret) {
@@ -906,6 +928,34 @@ void iwl_free_calib_results(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwl_free_calib_results);
+int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
+{
+        int ret = 0;
+        if (tx_power < IWL_TX_POWER_TARGET_POWER_MIN) {
+                IWL_WARNING("Requested user TXPOWER %d below limit.\n",
+                            priv->tx_power_user_lmt);
+                return -EINVAL;
+        }
+        if (tx_power > IWL_TX_POWER_TARGET_POWER_MAX) {
+                IWL_WARNING("Requested user TXPOWER %d above limit.\n",
+                            priv->tx_power_user_lmt);
+                return -EINVAL;
+        }
+        if (priv->tx_power_user_lmt != tx_power)
+                force = true;
+        priv->tx_power_user_lmt = tx_power;
+        if (force && priv->cfg->ops->lib->send_tx_power)
+                ret = priv->cfg->ops->lib->send_tx_power(priv);
+        return ret;
+}
+EXPORT_SYMBOL(iwl_set_tx_power);
 void iwl_uninit_drv(struct iwl_priv *priv)
 {
        iwl_free_calib_results(priv);
@@ -915,35 +965,6 @@ void iwl_uninit_drv(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwl_uninit_drv);
-/* Low level driver call this function to update iwlcore with
- * driver status.
- */
-int iwlcore_low_level_notify(struct iwl_priv *priv,
-                              enum iwlcore_card_notify notify)
-{
-        int ret;
-        switch (notify) {
-        case IWLCORE_INIT_EVT:
-                ret = iwl_rfkill_init(priv);
-                if (ret)
-                        IWL_ERROR("Unable to initialize RFKILL system. "
-                                  "Ignoring error: %d\n", ret);
-                iwl_power_initialize(priv);
-                break;
-        case IWLCORE_START_EVT:
-                iwl_power_update_mode(priv, 1);
-                break;
-        case IWLCORE_STOP_EVT:
-                break;
-        case IWLCORE_REMOVE_EVT:
-                iwl_rfkill_unregister(priv);
-                break;
-        }
-        return 0;
-}
-EXPORT_SYMBOL(iwlcore_low_level_notify);
 int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags)
 {
        u32 stat_flags = 0;
@@ -1204,12 +1225,14 @@ void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
                ptr += sizeof(u32);
                time = iwl_read_targ_mem(priv, ptr);
                ptr += sizeof(u32);
-                if (mode == 0)
+                if (mode == 0) {
-                        IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
+                        /* data, ev */
-                else {
+                        IWL_ERROR("EVT_LOG:0x%08x:%04u\n", time, ev);
+                } else {
                        data = iwl_read_targ_mem(priv, ptr);
                        ptr += sizeof(u32);
-                        IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
+                        IWL_ERROR("EVT_LOGT:%010u:0x%08x:%04u\n",
+                                        time, data, ev);
                }
        }
 }
@@ -1272,4 +1295,114 @@ void iwl_dump_nic_event_log(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwl_dump_nic_event_log);
+void iwl_rf_kill_ct_config(struct iwl_priv *priv)
+{
+        struct iwl_ct_kill_config cmd;
+        unsigned long flags;
+        int ret = 0;
+        spin_lock_irqsave(&priv->lock, flags);
+        iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+                    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
+        spin_unlock_irqrestore(&priv->lock, flags);
+        cmd.critical_temperature_R =
+                cpu_to_le32(priv->hw_params.ct_kill_threshold);
+        ret = iwl_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
+                               sizeof(cmd), &cmd);
+        if (ret)
+                IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
+        else
+                IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded, "
+                        "critical temperature is %d\n",
+                        cmd.critical_temperature_R);
+}
+EXPORT_SYMBOL(iwl_rf_kill_ct_config);
+/*
+ * CARD_STATE_CMD
+ *
+ * Use: Sets the device's internal card state to enable, disable, or halt
+ *
+ * When in the 'enable' state the card operates as normal.
+ * When in the 'disable' state, the card enters into a low power mode.
+ * When in the 'halt' state, the card is shut down and must be fully
+ * restarted to come back on.
+ */
+static int iwl_send_card_state(struct iwl_priv *priv, u32 flags, u8 meta_flag)
+{
+        struct iwl_host_cmd cmd = {
+                .id = REPLY_CARD_STATE_CMD,
+                .len = sizeof(u32),
+                .data = &flags,
+                .meta.flags = meta_flag,
+        };
+        return iwl_send_cmd(priv, &cmd);
+}
+void iwl_radio_kill_sw_disable_radio(struct iwl_priv *priv)
+{
+        unsigned long flags;
+        if (test_bit(STATUS_RF_KILL_SW, &priv->status))
+                return;
+        IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO OFF\n");
+        iwl_scan_cancel(priv);
+        /* FIXME: This is a workaround for AP */
+        if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
+                spin_lock_irqsave(&priv->lock, flags);
+                iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+                            CSR_UCODE_SW_BIT_RFKILL);
+                spin_unlock_irqrestore(&priv->lock, flags);
+                /* call the host command only if no hw rf-kill set */
+                if (!test_bit(STATUS_RF_KILL_HW, &priv->status) &&
+                    iwl_is_ready(priv))
+                        iwl_send_card_state(priv,
+                                CARD_STATE_CMD_DISABLE, 0);
+                set_bit(STATUS_RF_KILL_SW, &priv->status);
+                        /* make sure mac80211 stop sending Tx frame */
+                if (priv->mac80211_registered)
+                        ieee80211_stop_queues(priv->hw);
+        }
+}
+EXPORT_SYMBOL(iwl_radio_kill_sw_disable_radio);
+int iwl_radio_kill_sw_enable_radio(struct iwl_priv *priv)
+{
+        unsigned long flags;
+        if (!test_bit(STATUS_RF_KILL_SW, &priv->status))
+                return 0;
+        IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO ON\n");
+        spin_lock_irqsave(&priv->lock, flags);
+        iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+        clear_bit(STATUS_RF_KILL_SW, &priv->status);
+        spin_unlock_irqrestore(&priv->lock, flags);
+        /* wake up ucode */
+        msleep(10);
+        spin_lock_irqsave(&priv->lock, flags);
+        iwl_read32(priv, CSR_UCODE_DRV_GP1);
+        if (!iwl_grab_nic_access(priv))
+                iwl_release_nic_access(priv);
+        spin_unlock_irqrestore(&priv->lock, flags);
+        if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+                IWL_DEBUG_RF_KILL("Can not turn radio back on - "
+                                  "disabled by HW switch\n");
+                return 0;
+        }
+        if (priv->is_open)
+                queue_work(priv->workqueue, &priv->restart);
+        return 1;
+}
+EXPORT_SYMBOL(iwl_radio_kill_sw_enable_radio);
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index 6b5af7afbb25..2838093b4459 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -88,13 +88,11 @@ struct iwl_hcmd_ops {
 struct iwl_hcmd_utils_ops {
        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,
                        u32 *average_noise,
                        u16 min_average_noise_antennat_i,
                        u32 min_average_noise);
        void (*chain_noise_reset)(struct iwl_priv *priv);
-#endif
 };
 struct iwl_lib_ops {
@@ -111,15 +109,17 @@ struct iwl_lib_ops {
        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);
+        /* setup deferred work */
+        void (*setup_deferred_work)(struct iwl_priv *priv);
+        /* cancel deferred work */
+        void (*cancel_deferred_work)(struct iwl_priv *priv);
        /* alive notification after init uCode load */
        void (*init_alive_start)(struct iwl_priv *priv);
        /* alive notification */
@@ -128,8 +128,6 @@ struct iwl_lib_ops {
        int (*is_valid_rtc_data_addr)(u32 addr);
        /* 1st ucode load */
        int (*load_ucode)(struct iwl_priv *priv);
-        /* rfkill */
-        void (*radio_kill_sw)(struct iwl_priv *priv, int disable_radio);
         /* power management */
        struct {
                int (*init)(struct iwl_priv *priv);
@@ -140,6 +138,7 @@ struct iwl_lib_ops {
        } apm_ops;
        /* power */
        int (*set_power)(struct iwl_priv *priv, void *cmd);
+        int (*send_tx_power) (struct iwl_priv *priv);
        void (*update_chain_flags)(struct iwl_priv *priv);
        /* eeprom operations (as defined in iwl-eeprom.h) */
        struct iwl_eeprom_ops eeprom_ops;
@@ -233,11 +232,53 @@ 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
+/*****************************************************
+ * TX power
+ ****************************************************/
+int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force);
+/*****************************************************
+ * RF -Kill - here and not in iwl-rfkill.h to be available when
+ * RF-kill subsystem is not compiled.
+ ****************************************************/
+void iwl_radio_kill_sw_disable_radio(struct iwl_priv *priv);
+int iwl_radio_kill_sw_enable_radio(struct iwl_priv *priv);
+/*******************************************************************************
+ * Rate
+ ******************************************************************************/
+void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+                              struct ieee80211_tx_info *info);
+int iwl_hwrate_to_plcp_idx(u32 rate_n_flags);
+static inline u8 iwl_hw_get_rate(__le32 rate_n_flags)
+{
+        return le32_to_cpu(rate_n_flags) & 0xFF;
+}
+static inline u32 iwl_hw_get_rate_n_flags(__le32 rate_n_flags)
+{
+        return le32_to_cpu(rate_n_flags) & 0x1FFFF;
+}
+static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u32 flags)
+{
+        return cpu_to_le32(flags|(u32)rate);
+}
+/*******************************************************************************
+ * Scanning
+ ******************************************************************************/
+void iwl_init_scan_params(struct iwl_priv *priv);
+int iwl_scan_cancel(struct iwl_priv *priv);
+int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms);
+const char *iwl_escape_essid(const char *essid, u8 essid_len);
+int iwl_scan_initiate(struct iwl_priv *priv);
+void iwl_setup_rx_scan_handlers(struct iwl_priv *priv);
+void iwl_setup_scan_deferred_work(struct iwl_priv *priv);
 /*****************************************************
 *   S e n d i n g     H o s t     C o m m a n d s   *
@@ -286,6 +327,7 @@ void iwl_dump_nic_event_log(struct iwl_priv *priv);
 #define STATUS_POWER_PMI        16
 #define STATUS_FW_ERROR         17
 #define STATUS_CONF_PENDING     18
+#define STATUS_MODE_PENDING     19
 static inline int iwl_is_ready(struct iwl_priv *priv)
@@ -322,19 +364,10 @@ static inline int iwl_is_ready_rf(struct iwl_priv *priv)
        return iwl_is_ready(priv);
 }
+extern void iwl_rf_kill_ct_config(struct iwl_priv *priv);
-enum iwlcore_card_notify {
-        IWLCORE_INIT_EVT = 0,
-        IWLCORE_START_EVT = 1,
-        IWLCORE_STOP_EVT = 2,
-        IWLCORE_REMOVE_EVT = 3,
-};
-int iwlcore_low_level_notify(struct iwl_priv *priv,
-                             enum iwlcore_card_notify notify);
 extern int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags);
 extern int iwl_verify_ucode(struct iwl_priv *priv);
-int iwl_send_lq_cmd(struct iwl_priv *priv,
+extern int iwl_send_lq_cmd(struct iwl_priv *priv,
                    struct iwl_link_quality_cmd *lq, u8 flags);
 static inline int iwl_send_rxon_assoc(struct iwl_priv *priv)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index 11de561c7bf8..58384805a494 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -55,10 +55,9 @@ struct iwl_debugfs {
                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 */
+                struct dentry *file_disable_tx_power;
        } 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 29e16ba69cdb..ed948dc59b3d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -255,21 +255,18 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
                        pos += scnprintf(buf + pos, bufsz - pos, "tid data:\n");
                        pos += scnprintf(buf + pos, bufsz - pos,
                                        "seq_num\t\ttxq_id");
-#ifdef CONFIG_IWL4965_HT
                        pos += scnprintf(buf + pos, bufsz - pos,
                                        "\tframe_count\twait_for_ba\t");
                        pos += scnprintf(buf + pos, bufsz - pos,
                                        "start_idx\tbitmap0\t");
                        pos += scnprintf(buf + pos, bufsz - pos,
                                        "bitmap1\trate_n_flags");
-#endif
                        pos += scnprintf(buf + pos, bufsz - pos, "\n");
                        for (j = 0; j < MAX_TID_COUNT; j++) {
                                pos += scnprintf(buf + pos, bufsz - pos,
                                                "[%d]:\t\t%u", j,
                                                station->tid[j].seq_number);
-#ifdef CONFIG_IWL4965_HT
                                pos += scnprintf(buf + pos, bufsz - pos,
                                                "\t%u\t\t%u\t\t%u\t\t",
                                                station->tid[j].agg.txq_id,
@@ -280,7 +277,6 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
                                                station->tid[j].agg.start_idx,
                                                (unsigned long long)station->tid[j].agg.bitmap,
                                                station->tid[j].agg.rate_n_flags);
-#endif
                                pos += scnprintf(buf + pos, bufsz - pos, "\n");
                        }
                        pos += scnprintf(buf + pos, bufsz - pos, "\n");
@@ -389,11 +385,10 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
        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 */
+        DEBUGFS_ADD_BOOL(disable_tx_power, rf, &priv->disable_tx_power_cal);
        return 0;
 err:
@@ -419,10 +414,9 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
        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->dbgfs_rf_files.file_disable_tx_power);
        DEBUGFS_REMOVE(priv->dbgfs->dir_rf);
        DEBUGFS_REMOVE(priv->dbgfs->dir_drv);
        kfree(priv->dbgfs);
diff --git a/drivers/net/wireless/iwlwifi/iwl-dev.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 802f1a12b1aa..81ff4c2c6a5a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-dev.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -29,8 +29,8 @@
 * Please use iwl-4965-hw.h for hardware-related definitions.
 */
-#ifndef __iwl_4965_h__
+#ifndef __iwl_dev_h__
-#define __iwl_4965_h__
+#define __iwl_dev_h__
 #include <linux/pci.h> /* for struct pci_device_id */
 #include <linux/kernel.h>
@@ -157,44 +157,11 @@ struct iwl4965_channel_tgh_info {
        s64 last_radar_time;
 };
-/* current Tx power values to use, one for each rate for each channel.
- * requested power is limited by:
- * -- regulatory EEPROM limits for this channel
- * -- hardware capabilities (clip-powers)
- * -- spectrum management
- * -- user preference (e.g. iwconfig)
- * when requested power is set, base power index must also be set. */
-struct iwl4965_channel_power_info {
-        struct iwl4965_tx_power tpc;    /* actual radio and DSP gain settings */
-        s8 power_table_index;   /* actual (compenst'd) index into gain table */
-        s8 base_power_index;    /* gain index for power at factory temp. */
-        s8 requested_power;     /* power (dBm) requested for this chnl/rate */
-};
-/* current scan Tx power values to use, one for each scan rate for each
- * channel. */
-struct iwl4965_scan_power_info {
-        struct iwl4965_tx_power tpc;    /* actual radio and DSP gain settings */
-        s8 power_table_index;   /* actual (compenst'd) index into gain table */
-        s8 requested_power;     /* scan pwr (dBm) requested for chnl/rate */
-};
-/* For fat_extension_channel */
-enum {
-        HT_IE_EXT_CHANNEL_NONE = 0,
-        HT_IE_EXT_CHANNEL_ABOVE,
-        HT_IE_EXT_CHANNEL_INVALID,
-        HT_IE_EXT_CHANNEL_BELOW,
-        HT_IE_EXT_CHANNEL_MAX
-};
 /*
 * One for each channel, holds all channel setup data
 * Some of the fields (e.g. eeprom and flags/max_power_avg) are redundant
 *     with one another!
 */
-#define IWL4965_MAX_RATE (33)
 struct iwl_channel_info {
        struct iwl4965_channel_tgd_info tgd;
        struct iwl4965_channel_tgh_info tgh;
@@ -213,11 +180,6 @@ struct iwl_channel_info {
        u8 band_index;    /* 0-4, maps channel to band1/2/3/4/5 */
        enum ieee80211_band band;
-        /* Radio/DSP gain settings for each "normal" data Tx rate.
-         * These include, in addition to RF and DSP gain, a few fields for
-         *   remembering/modifying gain settings (indexes). */
-        struct iwl4965_channel_power_info power_info[IWL4965_MAX_RATE];
        /* FAT channel info */
        s8 fat_max_power_avg;   /* (dBm) regul. eeprom, normal Tx, any rate */
        s8 fat_curr_txpow;      /* (dBm) regulatory/spectrum/user (not h/w) */
@@ -225,9 +187,6 @@ struct iwl_channel_info {
        s8 fat_scan_power;      /* (dBm) eeprom, direct scans, any rate */
        u8 fat_flags;           /* flags copied from EEPROM */
        u8 fat_extension_channel; /* HT_IE_EXT_CHANNEL_* */
-        /* Radio/DSP gain settings for each scan rate, for directed scans. */
-        struct iwl4965_scan_power_info scan_pwr_info[IWL_NUM_SCAN_RATES];
 };
 struct iwl4965_clip_group {
@@ -401,7 +360,6 @@ struct iwl_rx_queue {
 #define IWL_INVALID_RATE     0xFF
 #define IWL_INVALID_VALUE    -1
-#ifdef CONFIG_IWL4965_HT
 /**
 * struct iwl_ht_agg -- aggregation status while waiting for block-ack
 * @txq_id: Tx queue used for Tx attempt
@@ -430,14 +388,11 @@ struct iwl_ht_agg {
        u8 state;
 };
-#endif /* CONFIG_IWL4965_HT */
 struct iwl_tid_data {
        u16 seq_number;
        u16 tfds_in_queue;
-#ifdef CONFIG_IWL4965_HT
        struct iwl_ht_agg agg;
-#endif /* CONFIG_IWL4965_HT */
 };
 struct iwl_hw_key {
@@ -525,7 +480,7 @@ struct fw_desc {
 };
 /* uCode file layout */
-struct iwl4965_ucode {
+struct iwl_ucode {
        __le32 ver;             /* major/minor/subminor */
        __le32 inst_size;       /* bytes of runtime instructions */
        __le32 data_size;       /* bytes of runtime data */
@@ -587,6 +542,7 @@ struct iwl_sensitivity_ranges {
 * @max_xxx_size: for ucode uses
 * @ct_kill_threshold: temperature threshold
 * @struct iwl_sensitivity_ranges: range of sensitivity values
+ * @first_ampdu_q: first HW queue available for ampdu
 */
 struct iwl_hw_params {
        u16 max_txq_num;
@@ -606,9 +562,8 @@ struct iwl_hw_params {
        u32 max_data_size;
        u32 max_bsm_size;
        u32 ct_kill_threshold; /* value in hw-dependent units */
-#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
        const struct iwl_sensitivity_ranges *sens;
-#endif
+        u8 first_ampdu_q;
 };
 #define HT_SHORT_GI_20MHZ       (1 << 0)
@@ -638,15 +593,8 @@ 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);
-extern void iwl4965_handle_data_packet_monitor(struct iwl_priv *priv,
-                                           struct iwl_rx_mem_buffer *rxb,
-                                           void *data, short len,
-                                           struct ieee80211_rx_status *stats,
-                                           u16 phy_flags);
 extern int iwl4965_is_duplicate_packet(struct iwl_priv *priv,
                                       struct ieee80211_hdr *header);
-extern int iwl4965_calc_db_from_ratio(int sig_ratio);
 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,
@@ -654,18 +602,7 @@ extern unsigned int iwl4965_fill_beacon_frame(struct iwl_priv *priv,
 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 iwl_bcast_addr[ETH_ALEN];
-int iwl4965_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
-/*
- * Currently used by iwl-3945-rs... look at restructuring so that it doesn't
- * call this... todo... fix that.
-*/
-extern u8 iwl4965_sync_station(struct iwl_priv *priv, int sta_id,
-                           u16 tx_rate, u8 flags);
 /******************************************************************************
 *
@@ -683,38 +620,16 @@ extern u8 iwl4965_sync_station(struct iwl_priv *priv, int sta_id,
 * iwl4965_mac_     <-- mac80211 callback
 *
 ****************************************************************************/
-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_set_hw_params(struct iwl_priv *priv);
 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 iwl_frame *frame, u8 rate);
-extern void iwl4965_hw_build_tx_cmd_rate(struct iwl_priv *priv,
-                                     struct iwl_cmd *cmd,
-                                     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);
-extern int iwl4965_hw_reg_set_txpower(struct iwl_priv *priv, s8 power);
 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
- * @bssid: MAC address of station ID to find
- *
- * NOTE:  This should not be hardware specific but the code has
- * not yet been merged into a single common layer for managing the
- * station tables.
- */
-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 iwl_queue_space(const struct iwl_queue *q);
 static inline int iwl_queue_used(const struct iwl_queue *q, int i)
@@ -738,37 +653,17 @@ static inline u8 get_cmd_index(struct iwl_queue *q, u32 index, int is_huge)
 struct iwl_priv;
-extern void iwl4965_radio_kill_sw(struct iwl_priv *priv, int disable_radio);
 /*
 * Forward declare iwl-4965.c functions for iwl-base.c
 */
-extern int iwl4965_tx_queue_update_wr_ptr(struct iwl_priv *priv,
-                                          struct iwl_tx_queue *txq,
-                                          u16 byte_cnt);
-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_info *info);
-#ifdef CONFIG_IWL4965_HT
-extern void iwl4965_init_ht_hw_capab(const struct iwl_priv *priv,
-                                struct ieee80211_ht_info *ht_info,
-                                enum ieee80211_band band);
-void iwl4965_set_rxon_ht(struct iwl_priv *priv,
-                         struct iwl_ht_info *ht_info);
 int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
                                    enum ieee80211_ampdu_mlme_action action,
                                    const u8 *addr, u16 tid, u16 *ssn);
 int iwl4965_check_empty_hw_queue(struct iwl_priv *priv, int sta_id,
                                        u8 tid, int txq_id);
-#else
-static inline void iwl4965_init_ht_hw_capab(const struct iwl_priv *priv,
-                                            struct ieee80211_ht_info *ht_info,
-                                            enum ieee80211_band band) {}
-#endif /*CONFIG_IWL4965_HT */
 /* Structures, enum, and defines specific to the 4965 */
 #define IWL_KW_SIZE 0x1000      /*4k */
@@ -792,11 +687,6 @@ struct iwl_kw {
 #define IWL_OPERATION_MODE_MIXED    2
 #define IWL_OPERATION_MODE_20MHZ    3
-#define IWL_EXT_CHANNEL_OFFSET_NONE      0
-#define IWL_EXT_CHANNEL_OFFSET_ABOVE     1
-#define IWL_EXT_CHANNEL_OFFSET_RESERVE1  2
-#define IWL_EXT_CHANNEL_OFFSET_BELOW     3
 #define IWL_TX_CRC_SIZE 4
 #define IWL_TX_DELIMITER_SIZE 4
@@ -892,7 +782,6 @@ enum ucode_type {
        UCODE_RT
 };
-#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
 /* Sensitivity calib data */
 struct iwl_sensitivity_data {
        u32 auto_corr_ofdm;
@@ -934,7 +823,6 @@ struct iwl_chain_noise_data {
        u8 delta_gain_code[NUM_RX_CHAINS];
        u8 radio_write;
 };
-#endif /* CONFIG_IWLWIFI_RUN_TIME_CALIB */
 #define EEPROM_SEM_TIMEOUT 10           /* milliseconds */
 #define EEPROM_SEM_RETRY_LIMIT 1000     /* number of attempts (not time) */
@@ -1006,7 +894,8 @@ struct iwl_priv {
        int one_direct_scan;
        u8 direct_ssid_len;
        u8 direct_ssid[IW_ESSID_MAX_SIZE];
-        struct iwl4965_scan_cmd *scan;
+        struct iwl_scan_cmd *scan;
+        u32 scan_tx_ant[IEEE80211_NUM_BANDS];
        /* spinlock */
        spinlock_t lock;        /* protect general shared data */
@@ -1067,15 +956,11 @@ struct iwl_priv {
        u8 assoc_station_added;
        u8 use_ant_b_for_management_frame;      /* Tx antenna selection */
        u8 start_calib;
-#ifdef CONFIG_IWLWIFI_RUN_TIME_CALIB
        struct iwl_sensitivity_data sensitivity_data;
        struct iwl_chain_noise_data chain_noise_data;
        __le16 sensitivity_tbl[HD_TABLE_SIZE];
-#endif /*CONFIG_IWLWIFI_RUN_TIME_CALIB*/
-#ifdef CONFIG_IWL4965_HT
        struct iwl_ht_info current_ht_config;
-#endif
        u8 last_phy_res[100];
        /* Rate scaling data */
@@ -1197,15 +1082,11 @@ struct iwl_priv {
        struct delayed_work init_alive_start;
        struct delayed_work alive_start;
-        struct delayed_work activity_timer;
-        struct delayed_work thermal_periodic;
-        struct delayed_work gather_stats;
        struct delayed_work scan_check;
        struct delayed_work post_associate;
+        /* TX Power */
-#define IWL_DEFAULT_TX_POWER 0x0F
+        s8 tx_power_user_lmt;
-        s8 user_txpower_limit;
+        s8 tx_power_channel_lmt;
-        s8 max_channel_txpower_limit;
 #ifdef CONFIG_PM
        u32 pm_state[16];
@@ -1223,13 +1104,10 @@ 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 */
+        u32 disable_tx_power_cal;
-#ifdef CONFIG_IWL4965_RUN_TIME_CALIB
+        struct work_struct run_time_calib_work;
-        struct work_struct sensitivity_work;
-#endif /* CONFIG_IWL4965_RUN_TIME_CALIB */
        struct timer_list statistics_periodic;
 }; /*iwl_priv */
@@ -1250,18 +1128,6 @@ 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)
 {
@@ -1270,7 +1136,6 @@ static inline struct ieee80211_hdr *iwl_tx_queue_get_hdr(struct iwl_priv *priv,
                                txb[idx].skb[0]->data;
        return NULL;
 }
-#endif
 static inline int iwl_is_associated(struct iwl_priv *priv)
@@ -1332,4 +1197,4 @@ extern const struct iwl_channel_info *iwl_get_channel_info(
 /* Requires full declaration of iwl_priv before including */
-#endif                          /* __iwl4965_4965_h__ */
+#endif                          /* __iwl_dev_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
index 11f9d9557a0e..4a08a1b50979 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
@@ -382,8 +382,8 @@ static int iwl_set_fat_chan_info(struct iwl_priv *priv,
        if (!is_channel_valid(ch_info))
                return -1;
-        IWL_DEBUG_INFO("FAT Ch. %d [%sGHz] %s%s%s%s%s(0x%02x"
+        IWL_DEBUG_INFO("FAT Ch. %d [%sGHz] %s%s%s%s%s(0x%02x %ddBm):"
-                        " %ddBm): Ad-Hoc %ssupported\n",
+                        " Ad-Hoc %ssupported\n",
                        ch_info->channel,
                        is_channel_a_band(ch_info) ?
                        "5.2" : "2.4",
@@ -470,6 +470,11 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                        /* Copy the run-time flags so they are there even on
                         * invalid channels */
                        ch_info->flags = eeprom_ch_info[ch].flags;
+                        /* First write that fat is not enabled, and then enable
+                         * one by one */
+                        ch_info->fat_extension_channel =
+                                (IEEE80211_CHAN_NO_FAT_ABOVE |
+                                 IEEE80211_CHAN_NO_FAT_BELOW);
                        if (!(is_channel_valid(ch_info))) {
                                IWL_DEBUG_INFO("Ch. %d Flags %x [%sGHz] - "
@@ -488,8 +493,8 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                        ch_info->scan_power = eeprom_ch_info[ch].max_power_avg;
                        ch_info->min_power = 0;
-                        IWL_DEBUG_INFO("Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x"
+                        IWL_DEBUG_INFO("Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x %ddBm):"
-                                       " %ddBm): Ad-Hoc %ssupported\n",
+                                       " Ad-Hoc %ssupported\n",
                                       ch_info->channel,
                                       is_channel_a_band(ch_info) ?
                                       "5.2" : "2.4",
@@ -510,8 +515,8 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                        /* Set the user_txpower_limit to the highest power
                         * supported by any channel */
                        if (eeprom_ch_info[ch].max_power_avg >
-                            priv->user_txpower_limit)
+                                                priv->tx_power_user_lmt)
-                                priv->user_txpower_limit =
+                                priv->tx_power_user_lmt =
                                    eeprom_ch_info[ch].max_power_avg;
                        ch_info++;
@@ -534,12 +539,14 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                for (ch = 0; ch < eeprom_ch_count; ch++) {
                        if ((band == 6) &&
-                            ((eeprom_ch_index[ch] == 5) ||
+                                ((eeprom_ch_index[ch] == 5) ||
-                            (eeprom_ch_index[ch] == 6) ||
+                                 (eeprom_ch_index[ch] == 6) ||
-                            (eeprom_ch_index[ch] == 7)))
+                                 (eeprom_ch_index[ch] == 7)))
-                               fat_extension_chan = HT_IE_EXT_CHANNEL_MAX;
+                                /* both are allowed: above and below */
+                                fat_extension_chan = 0;
                        else
-                                fat_extension_chan = HT_IE_EXT_CHANNEL_ABOVE;
+                                fat_extension_chan =
+                                        IEEE80211_CHAN_NO_FAT_BELOW;
                        /* Set up driver's info for lower half */
                        iwl_set_fat_chan_info(priv, ieeeband,
@@ -551,7 +558,7 @@ int iwl_init_channel_map(struct iwl_priv *priv)
                        iwl_set_fat_chan_info(priv, ieeeband,
                                                (eeprom_ch_index[ch] + 4),
                                                &(eeprom_ch_info[ch]),
-                                                HT_IE_EXT_CHANNEL_BELOW);
+                                                IEEE80211_CHAN_NO_FAT_ABOVE);
                }
        }
diff --git a/drivers/net/wireless/iwlwifi/iwl-helpers.h b/drivers/net/wireless/iwlwifi/iwl-helpers.h
index dedefa06ad8f..41eed6793328 100644
--- a/drivers/net/wireless/iwlwifi/iwl-helpers.h
+++ b/drivers/net/wireless/iwlwifi/iwl-helpers.h
@@ -139,123 +139,12 @@ static inline void iwl_set_bits16(__le16 *dst, u8 pos, u8 len, int val)
 #define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
-#define IEEE80211_CHAN_W_RADAR_DETECT 0x00000010
 static inline struct ieee80211_conf *ieee80211_get_hw_conf(
        struct ieee80211_hw *hw)
 {
        return &hw->conf;
 }
-#define QOS_CONTROL_LEN 2
-static inline int ieee80211_is_management(u16 fc)
-{
-        return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT;
-}
-static inline int ieee80211_is_control(u16 fc)
-{
-        return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL;
-}
-static inline int ieee80211_is_data(u16 fc)
-{
-        return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA;
-}
-static inline int ieee80211_is_back_request(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BACK_REQ);
-}
-static inline int ieee80211_is_probe_response(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_RESP);
-}
-static inline int ieee80211_is_probe_request(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_REQ);
-}
-static inline int ieee80211_is_beacon(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON);
-}
-static inline int ieee80211_is_atim(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ATIM);
-}
-static inline int ieee80211_is_assoc_request(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
-}
-static inline int ieee80211_is_assoc_response(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_RESP);
-}
-static inline int ieee80211_is_auth(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
-}
-static inline int ieee80211_is_deauth(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
-}
-static inline int ieee80211_is_disassoc(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
-}
-static inline int ieee80211_is_reassoc_request(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ);
-}
-static inline int ieee80211_is_reassoc_response(u16 fc)
-{
-        return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-               ((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-rfkill.c b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
index 59c8a716bd96..5f098747cf95 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rfkill.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
@@ -55,13 +55,13 @@ static int iwl_rfkill_soft_rf_kill(void *data, enum rfkill_state state)
        switch (state) {
        case RFKILL_STATE_ON:
-                priv->cfg->ops->lib->radio_kill_sw(priv, 0);
+                iwl_radio_kill_sw_enable_radio(priv);
                /* if HW rf-kill is set dont allow ON state */
                if (iwl_is_rfkill(priv))
                        err = -EBUSY;
                break;
        case RFKILL_STATE_OFF:
-                priv->cfg->ops->lib->radio_kill_sw(priv, 1);
+                iwl_radio_kill_sw_disable_radio(priv);
                if (!iwl_is_rfkill(priv))
                        err = -EBUSY;
                break;
diff --git a/drivers/net/wireless/iwlwifi/iwl-rfkill.h b/drivers/net/wireless/iwlwifi/iwl-rfkill.h
index a7f04b855403..b3c04dba45cf 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rfkill.h
+++ b/drivers/net/wireless/iwlwifi/iwl-rfkill.h
@@ -33,7 +33,6 @@ struct iwl_priv;
 #include <linux/rfkill.h>
 #include <linux/input.h>
 #ifdef CONFIG_IWLWIFI_RFKILL
 struct iwl_rfkill_mngr {
        struct rfkill *rfkill;
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
index cc61c937320f..c24844802a88 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -451,7 +451,6 @@ 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;
@@ -465,6 +464,5 @@ void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
                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-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
new file mode 100644
index 000000000000..5ca181f7125d
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -0,0 +1,921 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * Tomas Winkler <tomas.winkler@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+/* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
+ * sending probe req.  This should be set long enough to hear probe responses
+ * from more than one AP.  */
+#define IWL_ACTIVE_DWELL_TIME_24    (20)       /* all times in msec */
+#define IWL_ACTIVE_DWELL_TIME_52    (10)
+/* For faster active scanning, scan will move to the next channel if fewer than
+ * PLCP_QUIET_THRESH packets are heard on this channel within
+ * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
+ * time if it's a quiet channel (nothing responded to our probe, and there's
+ * no other traffic).
+ * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
+#define IWL_PLCP_QUIET_THRESH       __constant_cpu_to_le16(1)  /* packets */
+#define IWL_ACTIVE_QUIET_TIME       __constant_cpu_to_le16(5)  /* msec */
+/* For passive scan, listen PASSIVE_DWELL_TIME (msec) on each channel.
+ * Must be set longer than active dwell time.
+ * For the most reliable scan, set > AP beacon interval (typically 100msec). */
+#define IWL_PASSIVE_DWELL_TIME_24   (20)       /* all times in msec */
+#define IWL_PASSIVE_DWELL_TIME_52   (10)
+#define IWL_PASSIVE_DWELL_BASE      (100)
+#define IWL_CHANNEL_TUNE_TIME       5
+static int scan_tx_ant[3] = {
+        RATE_MCS_ANT_A_MSK, RATE_MCS_ANT_B_MSK, RATE_MCS_ANT_C_MSK
+};
+static int iwl_is_empty_essid(const char *essid, int essid_len)
+{
+        /* Single white space is for Linksys APs */
+        if (essid_len == 1 && essid[0] == ' ')
+                return 1;
+        /* Otherwise, if the entire essid is 0, we assume it is hidden */
+        while (essid_len) {
+                essid_len--;
+                if (essid[essid_len] != '\0')
+                        return 0;
+        }
+        return 1;
+}
+const char *iwl_escape_essid(const char *essid, u8 essid_len)
+{
+        static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+        const char *s = essid;
+        char *d = escaped;
+        if (iwl_is_empty_essid(essid, essid_len)) {
+                memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+                return escaped;
+        }
+        essid_len = min(essid_len, (u8) IW_ESSID_MAX_SIZE);
+        while (essid_len--) {
+                if (*s == '\0') {
+                        *d++ = '\\';
+                        *d++ = '0';
+                        s++;
+                } else
+                        *d++ = *s++;
+        }
+        *d = '\0';
+        return escaped;
+}
+EXPORT_SYMBOL(iwl_escape_essid);
+/**
+ * iwl_scan_cancel - Cancel any currently executing HW scan
+ *
+ * NOTE: priv->mutex is not required before calling this function
+ */
+int iwl_scan_cancel(struct iwl_priv *priv)
+{
+        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+                clear_bit(STATUS_SCANNING, &priv->status);
+                return 0;
+        }
+        if (test_bit(STATUS_SCANNING, &priv->status)) {
+                if (!test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+                        IWL_DEBUG_SCAN("Queuing scan abort.\n");
+                        set_bit(STATUS_SCAN_ABORTING, &priv->status);
+                        queue_work(priv->workqueue, &priv->abort_scan);
+                } else
+                        IWL_DEBUG_SCAN("Scan abort already in progress.\n");
+                return test_bit(STATUS_SCANNING, &priv->status);
+        }
+        return 0;
+}
+EXPORT_SYMBOL(iwl_scan_cancel);
+/**
+ * iwl_scan_cancel_timeout - Cancel any currently executing HW scan
+ * @ms: amount of time to wait (in milliseconds) for scan to abort
+ *
+ * NOTE: priv->mutex must be held before calling this function
+ */
+int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
+{
+        unsigned long now = jiffies;
+        int ret;
+        ret = iwl_scan_cancel(priv);
+        if (ret && ms) {
+                mutex_unlock(&priv->mutex);
+                while (!time_after(jiffies, now + msecs_to_jiffies(ms)) &&
+                                test_bit(STATUS_SCANNING, &priv->status))
+                        msleep(1);
+                mutex_lock(&priv->mutex);
+                return test_bit(STATUS_SCANNING, &priv->status);
+        }
+        return ret;
+}
+EXPORT_SYMBOL(iwl_scan_cancel_timeout);
+static int iwl_send_scan_abort(struct iwl_priv *priv)
+{
+        int ret = 0;
+        struct iwl_rx_packet *res;
+        struct iwl_host_cmd cmd = {
+                .id = REPLY_SCAN_ABORT_CMD,
+                .meta.flags = CMD_WANT_SKB,
+        };
+        /* If there isn't a scan actively going on in the hardware
+         * then we are in between scan bands and not actually
+         * actively scanning, so don't send the abort command */
+        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+                return 0;
+        }
+        ret = iwl_send_cmd_sync(priv, &cmd);
+        if (ret) {
+                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+                return ret;
+        }
+        res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+        if (res->u.status != CAN_ABORT_STATUS) {
+                /* The scan abort will return 1 for success or
+                 * 2 for "failure".  A failure condition can be
+                 * due to simply not being in an active scan which
+                 * can occur if we send the scan abort before we
+                 * the microcode has notified us that a scan is
+                 * completed. */
+                IWL_DEBUG_INFO("SCAN_ABORT returned %d.\n", res->u.status);
+                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+                clear_bit(STATUS_SCAN_HW, &priv->status);
+        }
+        dev_kfree_skb_any(cmd.meta.u.skb);
+        return ret;
+}
+/* Service response to REPLY_SCAN_CMD (0x80) */
+static void iwl_rx_reply_scan(struct iwl_priv *priv,
+                              struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        struct iwl_scanreq_notification *notif =
+            (struct iwl_scanreq_notification *)pkt->u.raw;
+        IWL_DEBUG_RX("Scan request status = 0x%x\n", notif->status);
+#endif
+}
+/* Service SCAN_START_NOTIFICATION (0x82) */
+static void iwl_rx_scan_start_notif(struct iwl_priv *priv,
+                                    struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        struct iwl_scanstart_notification *notif =
+            (struct iwl_scanstart_notification *)pkt->u.raw;
+        priv->scan_start_tsf = le32_to_cpu(notif->tsf_low);
+        IWL_DEBUG_SCAN("Scan start: "
+                       "%d [802.11%s] "
+                       "(TSF: 0x%08X:%08X) - %d (beacon timer %u)\n",
+                       notif->channel,
+                       notif->band ? "bg" : "a",
+                       notif->tsf_high,
+                       notif->tsf_low, notif->status, notif->beacon_timer);
+}
+/* Service SCAN_RESULTS_NOTIFICATION (0x83) */
+static void iwl_rx_scan_results_notif(struct iwl_priv *priv,
+                                      struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        struct iwl_scanresults_notification *notif =
+            (struct iwl_scanresults_notification *)pkt->u.raw;
+        IWL_DEBUG_SCAN("Scan ch.res: "
+                       "%d [802.11%s] "
+                       "(TSF: 0x%08X:%08X) - %d "
+                       "elapsed=%lu usec (%dms since last)\n",
+                       notif->channel,
+                       notif->band ? "bg" : "a",
+                       le32_to_cpu(notif->tsf_high),
+                       le32_to_cpu(notif->tsf_low),
+                       le32_to_cpu(notif->statistics[0]),
+                       le32_to_cpu(notif->tsf_low) - priv->scan_start_tsf,
+                       jiffies_to_msecs(elapsed_jiffies
+                                        (priv->last_scan_jiffies, jiffies)));
+#endif
+        priv->last_scan_jiffies = jiffies;
+        priv->next_scan_jiffies = 0;
+}
+/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
+static void iwl_rx_scan_complete_notif(struct iwl_priv *priv,
+                                       struct iwl_rx_mem_buffer *rxb)
+{
+        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+        struct iwl_scancomplete_notification *scan_notif = (void *)pkt->u.raw;
+        IWL_DEBUG_SCAN("Scan complete: %d channels (TSF 0x%08X:%08X) - %d\n",
+                       scan_notif->scanned_channels,
+                       scan_notif->tsf_low,
+                       scan_notif->tsf_high, scan_notif->status);
+        /* The HW is no longer scanning */
+        clear_bit(STATUS_SCAN_HW, &priv->status);
+        /* The scan completion notification came in, so kill that timer... */
+        cancel_delayed_work(&priv->scan_check);
+        IWL_DEBUG_INFO("Scan pass on %sGHz took %dms\n",
+                       (priv->scan_bands == 2) ? "2.4" : "5.2",
+                       jiffies_to_msecs(elapsed_jiffies
+                                        (priv->scan_pass_start, jiffies)));
+        /* Remove this scanned band from the list
+         * of pending bands to scan */
+        priv->scan_bands--;
+        /* If a request to abort was given, or the scan did not succeed
+         * then we reset the scan state machine and terminate,
+         * re-queuing another scan if one has been requested */
+        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+                IWL_DEBUG_INFO("Aborted scan completed.\n");
+                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+        } else {
+                /* If there are more bands on this scan pass reschedule */
+                if (priv->scan_bands > 0)
+                        goto reschedule;
+        }
+        priv->last_scan_jiffies = jiffies;
+        priv->next_scan_jiffies = 0;
+        IWL_DEBUG_INFO("Setting scan to off\n");
+        clear_bit(STATUS_SCANNING, &priv->status);
+        IWL_DEBUG_INFO("Scan took %dms\n",
+                jiffies_to_msecs(elapsed_jiffies(priv->scan_start, jiffies)));
+        queue_work(priv->workqueue, &priv->scan_completed);
+        return;
+reschedule:
+        priv->scan_pass_start = jiffies;
+        queue_work(priv->workqueue, &priv->request_scan);
+}
+void iwl_setup_rx_scan_handlers(struct iwl_priv *priv)
+{
+        /* scan handlers */
+        priv->rx_handlers[REPLY_SCAN_CMD] = iwl_rx_reply_scan;
+        priv->rx_handlers[SCAN_START_NOTIFICATION] = iwl_rx_scan_start_notif;
+        priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
+                                        iwl_rx_scan_results_notif;
+        priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
+                                        iwl_rx_scan_complete_notif;
+}
+EXPORT_SYMBOL(iwl_setup_rx_scan_handlers);
+static inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv,
+                                                enum ieee80211_band band)
+{
+        if (band == IEEE80211_BAND_5GHZ)
+                return IWL_ACTIVE_DWELL_TIME_52;
+        else
+                return IWL_ACTIVE_DWELL_TIME_24;
+}
+static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
+                                          enum ieee80211_band band)
+{
+        u16 active = iwl_get_active_dwell_time(priv, band);
+        u16 passive = (band != IEEE80211_BAND_5GHZ) ?
+            IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_24 :
+            IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_52;
+        if (iwl_is_associated(priv)) {
+                /* If we're associated, we clamp the maximum passive
+                 * dwell time to be 98% of the beacon interval (minus
+                 * 2 * channel tune time) */
+                passive = priv->beacon_int;
+                if ((passive > IWL_PASSIVE_DWELL_BASE) || !passive)
+                        passive = IWL_PASSIVE_DWELL_BASE;
+                passive = (passive * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+        }
+        if (passive <= active)
+                passive = active + 1;
+        return passive;
+}
+static int iwl_get_channels_for_scan(struct iwl_priv *priv,
+                                     enum ieee80211_band band,
+                                     u8 is_active, u8 direct_mask,
+                                     struct iwl_scan_channel *scan_ch)
+{
+        const struct ieee80211_channel *channels = NULL;
+        const struct ieee80211_supported_band *sband;
+        const struct iwl_channel_info *ch_info;
+        u16 passive_dwell = 0;
+        u16 active_dwell = 0;
+        int added, i;
+        sband = iwl_get_hw_mode(priv, band);
+        if (!sband)
+                return 0;
+        channels = sband->channels;
+        active_dwell = iwl_get_active_dwell_time(priv, band);
+        passive_dwell = iwl_get_passive_dwell_time(priv, band);
+        for (i = 0, added = 0; i < sband->n_channels; i++) {
+                if (channels[i].flags & IEEE80211_CHAN_DISABLED)
+                        continue;
+                scan_ch->channel =
+                        ieee80211_frequency_to_channel(channels[i].center_freq);
+                ch_info = iwl_get_channel_info(priv, band, scan_ch->channel);
+                if (!is_channel_valid(ch_info)) {
+                        IWL_DEBUG_SCAN("Channel %d is INVALID for this SKU.\n",
+                                       scan_ch->channel);
+                        continue;
+                }
+                if (!is_active || is_channel_passive(ch_info) ||
+                    (channels[i].flags & IEEE80211_CHAN_PASSIVE_SCAN))
+                        scan_ch->type = 0;
+                else
+                        scan_ch->type = 1;
+                if (scan_ch->type & 1)
+                        scan_ch->type |= (direct_mask << 1);
+                scan_ch->active_dwell = cpu_to_le16(active_dwell);
+                scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+                /* Set txpower levels to defaults */
+                scan_ch->dsp_atten = 110;
+                if (band == IEEE80211_BAND_5GHZ)
+                        scan_ch->tx_gain = ((1 << 5) | (3 << 3)) | 3;
+                else {
+                        scan_ch->tx_gain = ((1 << 5) | (5 << 3));
+                        /* NOTE: if we were doing 6Mb OFDM for scans we'd use
+                         * power level:
+                         * scan_ch->tx_gain = ((1 << 5) | (2 << 3)) | 3;
+                         */
+                }
+                IWL_DEBUG_SCAN("Scanning %d [%s %d]\n",
+                               scan_ch->channel,
+                               (scan_ch->type & 1) ? "ACTIVE" : "PASSIVE",
+                               (scan_ch->type & 1) ?
+                               active_dwell : passive_dwell);
+                scan_ch++;
+                added++;
+        }
+        IWL_DEBUG_SCAN("total channels to scan %d \n", added);
+        return added;
+}
+void iwl_init_scan_params(struct iwl_priv *priv)
+{
+        if (!priv->scan_tx_ant[IEEE80211_BAND_5GHZ])
+                priv->scan_tx_ant[IEEE80211_BAND_5GHZ] = RATE_MCS_ANT_INIT_IND;
+        if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
+                priv->scan_tx_ant[IEEE80211_BAND_2GHZ] = RATE_MCS_ANT_INIT_IND;
+}
+int iwl_scan_initiate(struct iwl_priv *priv)
+{
+        if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+                IWL_ERROR("APs don't scan.\n");
+                return 0;
+        }
+        if (!iwl_is_ready_rf(priv)) {
+                IWL_DEBUG_SCAN("Aborting scan due to not ready.\n");
+                return -EIO;
+        }
+        if (test_bit(STATUS_SCANNING, &priv->status)) {
+                IWL_DEBUG_SCAN("Scan already in progress.\n");
+                return -EAGAIN;
+        }
+        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+                IWL_DEBUG_SCAN("Scan request while abort pending.  "
+                               "Queuing.\n");
+                return -EAGAIN;
+        }
+        IWL_DEBUG_INFO("Starting scan...\n");
+        priv->scan_bands = 2;
+        set_bit(STATUS_SCANNING, &priv->status);
+        priv->scan_start = jiffies;
+        priv->scan_pass_start = priv->scan_start;
+        queue_work(priv->workqueue, &priv->request_scan);
+        return 0;
+}
+EXPORT_SYMBOL(iwl_scan_initiate);
+#define IWL_SCAN_CHECK_WATCHDOG (7 * HZ)
+static void iwl_bg_scan_check(struct work_struct *data)
+{
+        struct iwl_priv *priv =
+            container_of(data, struct iwl_priv, scan_check.work);
+        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+                return;
+        mutex_lock(&priv->mutex);
+        if (test_bit(STATUS_SCANNING, &priv->status) ||
+            test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+                IWL_DEBUG(IWL_DL_SCAN, "Scan completion watchdog resetting "
+                        "adapter (%dms)\n",
+                        jiffies_to_msecs(IWL_SCAN_CHECK_WATCHDOG));
+                if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+                        iwl_send_scan_abort(priv);
+        }
+        mutex_unlock(&priv->mutex);
+}
+/**
+ * iwl_supported_rate_to_ie - fill in the supported rate in IE field
+ *
+ * return : set the bit for each supported rate insert in ie
+ */
+static u16 iwl_supported_rate_to_ie(u8 *ie, u16 supported_rate,
+                                    u16 basic_rate, int *left)
+{
+        u16 ret_rates = 0, bit;
+        int i;
+        u8 *cnt = ie;
+        u8 *rates = ie + 1;
+        for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
+                if (bit & supported_rate) {
+                        ret_rates |= bit;
+                        rates[*cnt] = iwl_rates[i].ieee |
+                                ((bit & basic_rate) ? 0x80 : 0x00);
+                        (*cnt)++;
+                        (*left)--;
+                        if ((*left <= 0) ||
+                            (*cnt >= IWL_SUPPORTED_RATES_IE_LEN))
+                                break;
+                }
+        }
+        return ret_rates;
+}
+static void iwl_ht_cap_to_ie(const struct ieee80211_supported_band *sband,
+                             u8 *pos, int *left)
+{
+        struct ieee80211_ht_cap *ht_cap;
+        if (!sband || !sband->ht_info.ht_supported)
+                return;
+        if (*left < sizeof(struct ieee80211_ht_cap))
+                return;
+        *pos++ = sizeof(struct ieee80211_ht_cap);
+        ht_cap = (struct ieee80211_ht_cap *) pos;
+        ht_cap->cap_info = cpu_to_le16(sband->ht_info.cap);
+        memcpy(ht_cap->supp_mcs_set, sband->ht_info.supp_mcs_set, 16);
+        ht_cap->ampdu_params_info =
+                (sband->ht_info.ampdu_factor & IEEE80211_HT_CAP_AMPDU_FACTOR) |
+                ((sband->ht_info.ampdu_density << 2) &
+                        IEEE80211_HT_CAP_AMPDU_DENSITY);
+        *left -= sizeof(struct ieee80211_ht_cap);
+}
+/**
+ * iwl_fill_probe_req - fill in all required fields and IE for probe request
+ */
+static u16 iwl_fill_probe_req(struct iwl_priv *priv,
+                                  enum ieee80211_band band,
+                                  struct ieee80211_mgmt *frame,
+                                  int left)
+{
+        int len = 0;
+        u8 *pos = NULL;
+        u16 active_rates, ret_rates, cck_rates, active_rate_basic;
+        const struct ieee80211_supported_band *sband =
+                                                iwl_get_hw_mode(priv, band);
+        /* Make sure there is enough space for the probe request,
+         * two mandatory IEs and the data */
+        left -= 24;
+        if (left < 0)
+                return 0;
+        frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+        memcpy(frame->da, iwl_bcast_addr, ETH_ALEN);
+        memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
+        memcpy(frame->bssid, iwl_bcast_addr, ETH_ALEN);
+        frame->seq_ctrl = 0;
+        len += 24;
+        /* ...next IE... */
+        pos = &frame->u.probe_req.variable[0];
+        /* fill in our indirect SSID IE */
+        left -= 2;
+        if (left < 0)
+                return 0;
+        *pos++ = WLAN_EID_SSID;
+        *pos++ = 0;
+        len += 2;
+        /* fill in supported rate */
+        left -= 2;
+        if (left < 0)
+                return 0;
+        *pos++ = WLAN_EID_SUPP_RATES;
+        *pos = 0;
+        /* exclude 60M rate */
+        active_rates = priv->rates_mask;
+        active_rates &= ~IWL_RATE_60M_MASK;
+        active_rate_basic = active_rates & IWL_BASIC_RATES_MASK;
+        cck_rates = IWL_CCK_RATES_MASK & active_rates;
+        ret_rates = iwl_supported_rate_to_ie(pos, cck_rates,
+                                             active_rate_basic, &left);
+        active_rates &= ~ret_rates;
+        ret_rates = iwl_supported_rate_to_ie(pos, active_rates,
+                                             active_rate_basic, &left);
+        active_rates &= ~ret_rates;
+        len += 2 + *pos;
+        pos += (*pos) + 1;
+        if (active_rates == 0)
+                goto fill_end;
+        /* fill in supported extended rate */
+        /* ...next IE... */
+        left -= 2;
+        if (left < 0)
+                return 0;
+        /* ... fill it in... */
+        *pos++ = WLAN_EID_EXT_SUPP_RATES;
+        *pos = 0;
+        iwl_supported_rate_to_ie(pos, active_rates, active_rate_basic, &left);
+        if (*pos > 0) {
+                len += 2 + *pos;
+                pos += (*pos) + 1;
+        } else {
+                pos--;
+        }
+ fill_end:
+        left -= 2;
+        if (left < 0)
+                return 0;
+        *pos++ = WLAN_EID_HT_CAPABILITY;
+        *pos = 0;
+        iwl_ht_cap_to_ie(sband, pos, &left);
+        if (*pos > 0)
+                len += 2 + *pos;
+        return (u16)len;
+}
+static u32 iwl_scan_tx_ant(struct iwl_priv *priv, enum ieee80211_band band)
+{
+        int i, ind;
+        ind = priv->scan_tx_ant[band];
+        for (i = 0; i < priv->hw_params.tx_chains_num; i++) {
+                ind = (ind+1) >= priv->hw_params.tx_chains_num ? 0 : ind+1;
+                if (priv->hw_params.valid_tx_ant & (1 << ind)) {
+                        priv->scan_tx_ant[band] = ind;
+                        break;
+                }
+        }
+        return scan_tx_ant[ind];
+}
+static void iwl_bg_request_scan(struct work_struct *data)
+{
+        struct iwl_priv *priv =
+            container_of(data, struct iwl_priv, request_scan);
+        struct iwl_host_cmd cmd = {
+                .id = REPLY_SCAN_CMD,
+                .len = sizeof(struct iwl_scan_cmd),
+                .meta.flags = CMD_SIZE_HUGE,
+        };
+        struct iwl_scan_cmd *scan;
+        struct ieee80211_conf *conf = NULL;
+        int ret = 0;
+        u32 tx_ant;
+        u16 cmd_len;
+        enum ieee80211_band band;
+        u8 direct_mask;
+        u8 rx_chain = 0x7; /* bitmap: ABC chains */
+        conf = ieee80211_get_hw_conf(priv->hw);
+        mutex_lock(&priv->mutex);
+        if (!iwl_is_ready(priv)) {
+                IWL_WARNING("request scan called when driver not ready.\n");
+                goto done;
+        }
+        /* Make sure the scan wasn't cancelled before this queued work
+         * was given the chance to run... */
+        if (!test_bit(STATUS_SCANNING, &priv->status))
+                goto done;
+        /* This should never be called or scheduled if there is currently
+         * a scan active in the hardware. */
+        if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+                IWL_DEBUG_INFO("Multiple concurrent scan requests in parallel. "
+                               "Ignoring second request.\n");
+                ret = -EIO;
+                goto done;
+        }
+        if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+                IWL_DEBUG_SCAN("Aborting scan due to device shutdown\n");
+                goto done;
+        }
+        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+                IWL_DEBUG_HC("Scan request while abort pending.  Queuing.\n");
+                goto done;
+        }
+        if (iwl_is_rfkill(priv)) {
+                IWL_DEBUG_HC("Aborting scan due to RF Kill activation\n");
+                goto done;
+        }
+        if (!test_bit(STATUS_READY, &priv->status)) {
+                IWL_DEBUG_HC("Scan request while uninitialized.  Queuing.\n");
+                goto done;
+        }
+        if (!priv->scan_bands) {
+                IWL_DEBUG_HC("Aborting scan due to no requested bands\n");
+                goto done;
+        }
+        if (!priv->scan) {
+                priv->scan = kmalloc(sizeof(struct iwl_scan_cmd) +
+                                     IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+                if (!priv->scan) {
+                        ret = -ENOMEM;
+                        goto done;
+                }
+        }
+        scan = priv->scan;
+        memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+        scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
+        scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
+        if (iwl_is_associated(priv)) {
+                u16 interval = 0;
+                u32 extra;
+                u32 suspend_time = 100;
+                u32 scan_suspend_time = 100;
+                unsigned long flags;
+                IWL_DEBUG_INFO("Scanning while associated...\n");
+                spin_lock_irqsave(&priv->lock, flags);
+                interval = priv->beacon_int;
+                spin_unlock_irqrestore(&priv->lock, flags);
+                scan->suspend_time = 0;
+                scan->max_out_time = cpu_to_le32(200 * 1024);
+                if (!interval)
+                        interval = suspend_time;
+                extra = (suspend_time / interval) << 22;
+                scan_suspend_time = (extra |
+                    ((suspend_time % interval) * 1024));
+                scan->suspend_time = cpu_to_le32(scan_suspend_time);
+                IWL_DEBUG_SCAN("suspend_time 0x%X beacon interval %d\n",
+                               scan_suspend_time, interval);
+        }
+        /* We should add the ability for user to lock to PASSIVE ONLY */
+        if (priv->one_direct_scan) {
+                IWL_DEBUG_SCAN("Start direct scan for '%s'\n",
+                                iwl_escape_essid(priv->direct_ssid,
+                                priv->direct_ssid_len));
+                scan->direct_scan[0].id = WLAN_EID_SSID;
+                scan->direct_scan[0].len = priv->direct_ssid_len;
+                memcpy(scan->direct_scan[0].ssid,
+                       priv->direct_ssid, priv->direct_ssid_len);
+                direct_mask = 1;
+        } else if (!iwl_is_associated(priv) && priv->essid_len) {
+                IWL_DEBUG_SCAN("Start direct scan for '%s' (not associated)\n",
+                                iwl_escape_essid(priv->essid, priv->essid_len));
+                scan->direct_scan[0].id = WLAN_EID_SSID;
+                scan->direct_scan[0].len = priv->essid_len;
+                memcpy(scan->direct_scan[0].ssid, priv->essid, priv->essid_len);
+                direct_mask = 1;
+        } else {
+                IWL_DEBUG_SCAN("Start indirect scan.\n");
+                direct_mask = 0;
+        }
+        scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+        scan->tx_cmd.sta_id = priv->hw_params.bcast_sta_id;
+        scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+        switch (priv->scan_bands) {
+        case 2:
+                band = IEEE80211_BAND_2GHZ;
+                scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
+                tx_ant = iwl_scan_tx_ant(priv, band);
+                if (priv->active_rxon.flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK)
+                        scan->tx_cmd.rate_n_flags =
+                                iwl_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
+                                                        tx_ant);
+                else
+                        scan->tx_cmd.rate_n_flags =
+                                iwl_hw_set_rate_n_flags(IWL_RATE_1M_PLCP,
+                                                        tx_ant |
+                                                        RATE_MCS_CCK_MSK);
+                scan->good_CRC_th = 0;
+                break;
+        case 1:
+                band = IEEE80211_BAND_5GHZ;
+                tx_ant = iwl_scan_tx_ant(priv, band);
+                scan->tx_cmd.rate_n_flags =
+                                iwl_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
+                                                        tx_ant);
+                scan->good_CRC_th = IWL_GOOD_CRC_TH;
+                /* Force use of chains B and C (0x6) for scan Rx for 4965
+                 * Avoid A (0x1) because of its off-channel reception on A-band.
+                 * MIMO is not used here, but value is required */
+                if ((priv->hw_rev & CSR_HW_REV_TYPE_MSK) == CSR_HW_REV_TYPE_4965)
+                        rx_chain = 0x6;
+                break;
+        default:
+                IWL_WARNING("Invalid scan band count\n");
+                goto done;
+        }
+        scan->rx_chain = RXON_RX_CHAIN_DRIVER_FORCE_MSK |
+                                cpu_to_le16((0x7 << RXON_RX_CHAIN_VALID_POS) |
+                                (rx_chain << RXON_RX_CHAIN_FORCE_SEL_POS) |
+                                (0x7 << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS));
+        cmd_len = iwl_fill_probe_req(priv, band,
+                                     (struct ieee80211_mgmt *)scan->data,
+                                     IWL_MAX_SCAN_SIZE - sizeof(*scan));
+        scan->tx_cmd.len = cpu_to_le16(cmd_len);
+        if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR)
+                scan->filter_flags = RXON_FILTER_PROMISC_MSK;
+        scan->filter_flags |= (RXON_FILTER_ACCEPT_GRP_MSK |
+                               RXON_FILTER_BCON_AWARE_MSK);
+        if (direct_mask)
+                scan->channel_count =
+                        iwl_get_channels_for_scan(priv, band, 1, /* active */
+                                                  direct_mask,
+                                (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
+        else
+                scan->channel_count =
+                        iwl_get_channels_for_scan(priv, band, 0, /* passive */
+                                                  direct_mask,
+                                (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
+        if (scan->channel_count == 0) {
+                IWL_DEBUG_SCAN("channel count %d\n", scan->channel_count);
+                goto done;
+        }
+        cmd.len += le16_to_cpu(scan->tx_cmd.len) +
+            scan->channel_count * sizeof(struct iwl_scan_channel);
+        cmd.data = scan;
+        scan->len = cpu_to_le16(cmd.len);
+        set_bit(STATUS_SCAN_HW, &priv->status);
+        ret = iwl_send_cmd_sync(priv, &cmd);
+        if (ret)
+                goto done;
+        queue_delayed_work(priv->workqueue, &priv->scan_check,
+                           IWL_SCAN_CHECK_WATCHDOG);
+        mutex_unlock(&priv->mutex);
+        return;
+ done:
+        /* inform mac80211 scan aborted */
+        queue_work(priv->workqueue, &priv->scan_completed);
+        mutex_unlock(&priv->mutex);
+}
+static void iwl_bg_abort_scan(struct work_struct *work)
+{
+        struct iwl_priv *priv = container_of(work, struct iwl_priv, abort_scan);
+        if (!iwl_is_ready(priv))
+                return;
+        mutex_lock(&priv->mutex);
+        set_bit(STATUS_SCAN_ABORTING, &priv->status);
+        iwl_send_scan_abort(priv);
+        mutex_unlock(&priv->mutex);
+}
+void iwl_setup_scan_deferred_work(struct iwl_priv *priv)
+{
+        /*  FIXME: move here when resolved PENDING
+         *  INIT_WORK(&priv->scan_completed, iwl_bg_scan_completed); */
+        INIT_WORK(&priv->request_scan, iwl_bg_request_scan);
+        INIT_WORK(&priv->abort_scan, iwl_bg_abort_scan);
+        INIT_DELAYED_WORK(&priv->scan_check, iwl_bg_scan_check);
+}
+EXPORT_SYMBOL(iwl_setup_scan_deferred_work);
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.c b/drivers/net/wireless/iwlwifi/iwl-sta.c
index 983f10760fb0..f874e7d7b225 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.c
@@ -30,11 +30,9 @@
 #include <net/mac80211.h>
 #include <linux/etherdevice.h>
-#include "iwl-eeprom.h"
 #include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-sta.h"
-#include "iwl-io.h"
 #include "iwl-helpers.h"
@@ -74,6 +72,17 @@ u8 iwl_find_station(struct iwl_priv *priv, const u8 *addr)
 }
 EXPORT_SYMBOL(iwl_find_station);
+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);
+        }
+}
+EXPORT_SYMBOL(iwl_get_ra_sta_id);
 static int iwl_add_sta_callback(struct iwl_priv *priv,
                                   struct iwl_cmd *cmd, struct sk_buff *skb)
 {
@@ -105,8 +114,6 @@ static int iwl_add_sta_callback(struct iwl_priv *priv,
        return 1;
 }
 int iwl_send_add_sta(struct iwl_priv *priv,
                     struct iwl_addsta_cmd *sta, u8 flags)
 {
@@ -156,8 +163,6 @@ 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)
 {
@@ -202,12 +207,6 @@ static void iwl_set_ht_add_station(struct iwl_priv *priv, u8 index,
 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
@@ -280,7 +279,6 @@ u8 iwl_add_station_flags(struct iwl_priv *priv, const u8 *addr, int is_ap,
 }
 EXPORT_SYMBOL(iwl_add_station_flags);
 static int iwl_sta_ucode_deactivate(struct iwl_priv *priv, const char *addr)
 {
        unsigned long flags;
@@ -384,9 +382,9 @@ static int iwl_send_remove_station(struct iwl_priv *priv, const u8 *addr,
        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)
 {
@@ -426,7 +424,7 @@ out:
        return 0;
 }
 EXPORT_SYMBOL(iwl_remove_station);
-int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
+static int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
 {
        int i;
@@ -496,6 +494,8 @@ int iwl_remove_default_wep_key(struct iwl_priv *priv,
        priv->default_wep_key--;
        memset(&priv->wep_keys[keyconf->keyidx], 0, sizeof(priv->wep_keys[0]));
        ret = iwl_send_static_wepkey_cmd(priv, 1);
+        IWL_DEBUG_WEP("Remove default WEP key: idx=%d ret=%d\n",
+                      keyconf->keyidx, ret);
        spin_unlock_irqrestore(&priv->sta_lock, flags);
        return ret;
@@ -508,6 +508,12 @@ int iwl_set_default_wep_key(struct iwl_priv *priv,
        int ret;
        unsigned long flags;
+        if (keyconf->keylen != WEP_KEY_LEN_128 &&
+            keyconf->keylen != WEP_KEY_LEN_64) {
+                IWL_DEBUG_WEP("Bad WEP key length %d\n", keyconf->keylen);
+                return -EINVAL;
+        }
        keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
        keyconf->hw_key_idx = HW_KEY_DEFAULT;
        priv->stations[IWL_AP_ID].keyinfo.alg = ALG_WEP;
@@ -524,6 +530,8 @@ int iwl_set_default_wep_key(struct iwl_priv *priv,
                                                        keyconf->keylen);
        ret = iwl_send_static_wepkey_cmd(priv, 0);
+        IWL_DEBUG_WEP("Set default WEP key: len=%d idx=%d ret=%d\n",
+                keyconf->keylen, keyconf->keyidx, ret);
        spin_unlock_irqrestore(&priv->sta_lock, flags);
        return ret;
@@ -670,6 +678,9 @@ int iwl_remove_dynamic_key(struct iwl_priv *priv,
        key_flags = le16_to_cpu(priv->stations[sta_id].sta.key.key_flags);
        keyidx = (key_flags >> STA_KEY_FLG_KEYID_POS) & 0x3;
+        IWL_DEBUG_WEP("Remove dynamic key: idx=%d sta=%d\n",
+                      keyconf->keyidx, sta_id);
        if (keyconf->keyidx != keyidx) {
                /* We need to remove a key with index different that the one
                 * in the uCode. This means that the key we need to remove has
@@ -694,7 +705,6 @@ int iwl_remove_dynamic_key(struct iwl_priv *priv,
        priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
        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, CMD_ASYNC);
        spin_unlock_irqrestore(&priv->sta_lock, flags);
        return ret;
@@ -724,6 +734,10 @@ int iwl_set_dynamic_key(struct iwl_priv *priv,
                ret = -EINVAL;
        }
+        IWL_DEBUG_WEP("Set dynamic key: alg= %d len=%d idx=%d sta=%d ret=%d\n",
+                      keyconf->alg, keyconf->keylen, keyconf->keyidx,
+                      sta_id, ret);
        return ret;
 }
 EXPORT_SYMBOL(iwl_set_dynamic_key);
@@ -816,7 +830,7 @@ static void iwl_sta_init_lq(struct iwl_priv *priv, const u8 *addr, int is_ap)
                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);
+                        iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
                r = iwl4965_get_prev_ieee_rate(r);
        }
@@ -842,7 +856,6 @@ 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;
@@ -852,7 +865,6 @@ int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
                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);
@@ -863,7 +875,6 @@ int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
 }
 EXPORT_SYMBOL(iwl_rxon_add_station);
 /**
 * iwl_get_sta_id - Find station's index within station table
 *
@@ -921,7 +932,6 @@ int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
 }
 EXPORT_SYMBOL(iwl_get_sta_id);
 /**
 * iwl_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
 */
@@ -940,4 +950,3 @@ void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv, int sta_id, int tid)
 }
 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 3d55716f5301..b6bb209fdd58 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.h
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.h
@@ -32,18 +32,24 @@
 #define HW_KEY_DYNAMIC 0
 #define HW_KEY_DEFAULT 1
-int iwl_get_free_ucode_key_index(struct iwl_priv *priv);
+/**
+ * iwl_find_station - Find station id for a given BSSID
+ * @bssid: MAC address of station ID to find
+ */
+u8 iwl_find_station(struct iwl_priv *priv, const u8 *bssid);
 int iwl_send_static_wepkey_cmd(struct iwl_priv *priv, u8 send_if_empty);
 int iwl_remove_default_wep_key(struct iwl_priv *priv,
-                                struct ieee80211_key_conf *key);
+                               struct ieee80211_key_conf *key);
 int iwl_set_default_wep_key(struct iwl_priv *priv,
-                                struct ieee80211_key_conf *key);
+                            struct ieee80211_key_conf *key);
 int iwl_set_dynamic_key(struct iwl_priv *priv,
-                                struct ieee80211_key_conf *key, u8 sta_id);
+                        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);
+                           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);
+int iwl_get_ra_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr);
 #endif /* __iwl_sta_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
index cfe6f4b233dd..7296e2846ec3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-tx.c
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -36,8 +36,6 @@
 #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,
@@ -58,9 +56,6 @@ static const u16 default_tid_to_tx_fifo[] = {
        IWL_TX_FIFO_AC3
 };
-#endif  /*CONFIG_IWL4965_HT */
 /**
 * iwl_hw_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
@@ -574,15 +569,15 @@ static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
                                  struct ieee80211_hdr *hdr,
                                  int is_unicast, u8 std_id)
 {
-        u16 fc = le16_to_cpu(hdr->frame_control);
+        __le16 fc = 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)
+                if (ieee80211_is_mgmt(fc))
                        tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-                if (ieee80211_is_probe_response(fc) &&
+                if (ieee80211_is_probe_resp(fc) &&
                    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
                        tx_flags |= TX_CMD_FLG_TSF_MSK;
        } else {
@@ -590,16 +585,16 @@ static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
                tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
        }
-        if (ieee80211_is_back_request(fc))
+        if (ieee80211_is_back_req(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))
+        if (ieee80211_has_morefrags(fc))
                tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                u8 *qc = ieee80211_get_qos_ctl(hdr);
                tx_cmd->tid_tspec = qc[0] & 0xf;
                tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
        } else {
@@ -618,9 +613,8 @@ static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
                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 (ieee80211_is_mgmt(fc)) {
-                if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
+                if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
-                    (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);
@@ -639,7 +633,7 @@ static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
 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,
+                              __le16 fc, int sta_id,
                              int is_hcca)
 {
        u8 rts_retry_limit = 0;
@@ -660,7 +654,7 @@ static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
                rate_flags |= RATE_MCS_CCK_MSK;
-        if (ieee80211_is_probe_response(fc)) {
+        if (ieee80211_is_probe_resp(fc)) {
                data_retry_limit = 3;
                if (data_retry_limit < rts_retry_limit)
                        rts_retry_limit = data_retry_limit;
@@ -675,11 +669,11 @@ static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
                tx_cmd->initial_rate_index = 0;
                tx_cmd->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
        } else {
-                switch (fc & IEEE80211_FCTL_STYPE) {
+                switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
-                case IEEE80211_STYPE_AUTH:
+                case cpu_to_le16(IEEE80211_STYPE_AUTH):
-                case IEEE80211_STYPE_DEAUTH:
+                case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
-                case IEEE80211_STYPE_ASSOC_REQ:
+                case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
-                case IEEE80211_STYPE_REASSOC_REQ:
+                case cpu_to_le16(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;
@@ -701,7 +695,7 @@ static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
        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);
+        tx_cmd->rate_n_flags = iwl_hw_set_rate_n_flags(rate_plcp, rate_flags);
 }
 static void iwl_tx_cmd_build_hwcrypto(struct iwl_priv *priv,
@@ -776,7 +770,7 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
        u16 seq_number = 0;
        u8 id, hdr_len, unicast;
        u8 sta_id;
-        u16 fc;
+        __le16 fc;
        u8 wait_write_ptr = 0;
        u8 tid = 0;
        u8 *qc = NULL;
@@ -803,19 +797,19 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
        unicast = !is_multicast_ether_addr(hdr->addr1);
        id = 0;
-        fc = le16_to_cpu(hdr->frame_control);
+        fc = 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))
+        else if (ieee80211_is_assoc_req(fc))
                IWL_DEBUG_TX("Sending ASSOC frame\n");
-        else if (ieee80211_is_reassoc_request(fc))
+        else if (ieee80211_is_reassoc_req(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) &&
+        if (ieee80211_is_data(fc) &&
           (!iwl_is_associated(priv) ||
            ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id) ||
            !priv->assoc_station_added)) {
@@ -825,7 +819,7 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
        spin_unlock_irqrestore(&priv->lock, flags);
-        hdr_len = ieee80211_get_hdrlen(fc);
+        hdr_len = ieee80211_get_hdrlen(le16_to_cpu(fc));
        /* Find (or create) index into station table for destination station */
        sta_id = iwl_get_sta_id(priv, hdr);
@@ -839,8 +833,8 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
        IWL_DEBUG_TX("station Id %d\n", sta_id);
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                qc = ieee80211_get_qos_ctrl(hdr, hdr_len);
+                qc = ieee80211_get_qos_ctl(hdr);
                tid = qc[0] & 0xf;
                seq_number = priv->stations[sta_id].tid[tid].seq_number &
                                IEEE80211_SCTL_SEQ;
@@ -848,12 +842,10 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
                        (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 */
@@ -945,14 +937,14 @@ int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
        /* 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);
+        iwl_update_tx_stats(priv, le16_to_cpu(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)) {
+        if (!ieee80211_has_morefrags(hdr->frame_control)) {
                txq->need_update = 1;
                if (qc)
                        priv->stations[sta_id].tid[tid].seq_number = seq_number;
@@ -1196,8 +1188,6 @@ void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
 }
 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.
@@ -1359,7 +1349,6 @@ int iwl_txq_check_empty(struct iwl_priv *priv, int sta_id, u8 tid, int txq_id)
        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
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index 72279e07fe32..47cf4b997f50 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -2431,15 +2431,15 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
                                  struct ieee80211_hdr *hdr,
                                  int is_unicast, u8 std_id)
 {
-        u16 fc = le16_to_cpu(hdr->frame_control);
+        __le16 fc = hdr->frame_control;
        __le32 tx_flags = cmd->cmd.tx.tx_flags;
        cmd->cmd.tx.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)
+                if (ieee80211_is_mgmt(fc))
                        tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-                if (ieee80211_is_probe_response(fc) &&
+                if (ieee80211_is_probe_resp(fc) &&
                    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
                        tx_flags |= TX_CMD_FLG_TSF_MSK;
        } else {
@@ -2448,11 +2448,11 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
        }
        cmd->cmd.tx.sta_id = std_id;
-        if (ieee80211_get_morefrag(hdr))
+        if (ieee80211_has_morefrags(fc))
                tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                u8 *qc = ieee80211_get_qos_ctrl(hdr, ieee80211_get_hdrlen(fc));
+                u8 *qc = ieee80211_get_qos_ctl(hdr);
                cmd->cmd.tx.tid_tspec = qc[0] & 0xf;
                tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
        } else {
@@ -2471,9 +2471,8 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
                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 (ieee80211_is_mgmt(fc)) {
-                if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
+                if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
-                    (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);
@@ -2564,7 +2563,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
        u8 sta_id;
        u8 tid = 0;
        u16 seq_number = 0;
-        u16 fc;
+        __le16 fc;
        u8 wait_write_ptr = 0;
        u8 *qc = NULL;
        unsigned long flags;
@@ -2589,28 +2588,28 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
        unicast = !is_multicast_ether_addr(hdr->addr1);
        id = 0;
-        fc = le16_to_cpu(hdr->frame_control);
+        fc = hdr->frame_control;
 #ifdef CONFIG_IWL3945_DEBUG
        if (ieee80211_is_auth(fc))
                IWL_DEBUG_TX("Sending AUTH frame\n");
-        else if (ieee80211_is_assoc_request(fc))
+        else if (ieee80211_is_assoc_req(fc))
                IWL_DEBUG_TX("Sending ASSOC frame\n");
-        else if (ieee80211_is_reassoc_request(fc))
+        else if (ieee80211_is_reassoc_req(fc))
                IWL_DEBUG_TX("Sending REASSOC frame\n");
 #endif
        /* drop all data frame if we are not associated */
        if ((!iwl3945_is_associated(priv) ||
             ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id)) &&
-            ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) {
+            ieee80211_is_data(fc)) {
                IWL_DEBUG_DROP("Dropping - !iwl3945_is_associated\n");
                goto drop_unlock;
        }
        spin_unlock_irqrestore(&priv->lock, flags);
-        hdr_len = ieee80211_get_hdrlen(fc);
+        hdr_len = ieee80211_get_hdrlen(le16_to_cpu(fc));
        /* Find (or create) index into station table for destination station */
        sta_id = iwl3945_get_sta_id(priv, hdr);
@@ -2624,8 +2623,8 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
        IWL_DEBUG_RATE("station Id %d\n", sta_id);
-        if (ieee80211_is_qos_data(fc)) {
+        if (ieee80211_is_data_qos(fc)) {
-                qc = ieee80211_get_qos_ctrl(hdr, hdr_len);
+                qc = ieee80211_get_qos_ctl(hdr);
                tid = qc[0] & 0xf;
                seq_number = priv->stations[sta_id].tid[tid].seq_number &
                                IEEE80211_SCTL_SEQ;
@@ -2732,7 +2731,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
        out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_A_MSK;
        out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_B_MSK;
-        if (!ieee80211_get_morefrag(hdr)) {
+        if (!ieee80211_has_morefrags(hdr->frame_control)) {
                txq->need_update = 1;
                if (qc) {
                        priv->stations[sta_id].tid[tid].seq_number = seq_number;
@@ -2746,7 +2745,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
                           sizeof(out_cmd->cmd.tx));
        iwl3945_print_hex_dump(IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
-                           ieee80211_get_hdrlen(fc));
+                           ieee80211_get_hdrlen(le16_to_cpu(fc)));
        /* Tell device the write index *just past* this latest filled TFD */
        q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
@@ -2875,7 +2874,8 @@ static void iwl3945_radio_kill_sw(struct iwl3945_priv *priv, int disable_radio)
                return;
        }
-        queue_work(priv->workqueue, &priv->restart);
+        if (priv->is_open)
+                queue_work(priv->workqueue, &priv->restart);
        return;
 }
diff --git a/drivers/net/wireless/iwlwifi/iwl4965-base.c b/drivers/net/wireless/iwlwifi/iwl4965-base.c
index c71daec8c746..f5911687671b 100644
--- a/drivers/net/wireless/iwlwifi/iwl4965-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl4965-base.c
@@ -87,46 +87,6 @@ MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT);
 MODULE_LICENSE("GPL");
-static int iwl4965_is_empty_essid(const char *essid, int essid_len)
-{
-        /* Single white space is for Linksys APs */
-        if (essid_len == 1 && essid[0] == ' ')
-                return 1;
-        /* Otherwise, if the entire essid is 0, we assume it is hidden */
-        while (essid_len) {
-                essid_len--;
-                if (essid[essid_len] != '\0')
-                        return 0;
-        }
-        return 1;
-}
-static const char *iwl4965_escape_essid(const char *essid, u8 essid_len)
-{
-        static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-        const char *s = essid;
-        char *d = escaped;
-        if (iwl4965_is_empty_essid(essid, essid_len)) {
-                memcpy(escaped, "<hidden>", sizeof("<hidden>"));
-                return escaped;
-        }
-        essid_len = min(essid_len, (u8) IW_ESSID_MAX_SIZE);
-        while (essid_len--) {
-                if (*s == '\0') {
-                        *d++ = '\\';
-                        *d++ = '0';
-                        s++;
-                } else
-                        *d++ = *s++;
-        }
-        *d = '\0';
-        return escaped;
-}
 /*************** STATION TABLE MANAGEMENT ****
 * mac80211 should be examined to determine if sta_info is duplicating
 * the functionality provided here
@@ -367,9 +327,9 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
        /* If we issue a new RXON command which required a tune then we must
         * send a new TXPOWER command or we won't be able to Tx any frames */
-        rc = iwl4965_hw_reg_send_txpower(priv);
+        rc = iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
        if (rc) {
-                IWL_ERROR("Error setting Tx power (%d).\n", rc);
+                IWL_ERROR("Error sending TX power (%d).\n", rc);
                return rc;
        }
@@ -419,69 +379,6 @@ static int iwl4965_send_bt_config(struct iwl_priv *priv)
                                sizeof(struct iwl4965_bt_cmd), &bt_cmd);
 }
-static int iwl4965_send_scan_abort(struct iwl_priv *priv)
-{
-        int ret = 0;
-        struct iwl_rx_packet *res;
-        struct iwl_host_cmd cmd = {
-                .id = REPLY_SCAN_ABORT_CMD,
-                .meta.flags = CMD_WANT_SKB,
-        };
-        /* If there isn't a scan actively going on in the hardware
-         * then we are in between scan bands and not actually
-         * actively scanning, so don't send the abort command */
-        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
-                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
-                return 0;
-        }
-        ret = iwl_send_cmd_sync(priv, &cmd);
-        if (ret) {
-                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
-                return ret;
-        }
-        res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
-        if (res->u.status != CAN_ABORT_STATUS) {
-                /* The scan abort will return 1 for success or
-                 * 2 for "failure".  A failure condition can be
-                 * due to simply not being in an active scan which
-                 * can occur if we send the scan abort before we
-                 * the microcode has notified us that a scan is
-                 * completed. */
-                IWL_DEBUG_INFO("SCAN_ABORT returned %d.\n", res->u.status);
-                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
-                clear_bit(STATUS_SCAN_HW, &priv->status);
-        }
-        dev_kfree_skb_any(cmd.meta.u.skb);
-        return ret;
-}
-/*
- * CARD_STATE_CMD
- *
- * Use: Sets the device's internal card state to enable, disable, or halt
- *
- * When in the 'enable' state the card operates as normal.
- * When in the 'disable' state, the card enters into a low power mode.
- * When in the 'halt' state, the card is shut down and must be fully
- * restarted to come back on.
- */
-static int iwl4965_send_card_state(struct iwl_priv *priv, u32 flags, u8 meta_flag)
-{
-        struct iwl_host_cmd cmd = {
-                .id = REPLY_CARD_STATE_CMD,
-                .len = sizeof(u32),
-                .data = &flags,
-                .meta.flags = meta_flag,
-        };
-        return iwl_send_cmd(priv, &cmd);
-}
 static void iwl_clear_free_frames(struct iwl_priv *priv)
 {
        struct list_head *element;
@@ -605,36 +502,6 @@ static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
 *
 ******************************************************************************/
-/**
- * iwl4965_supported_rate_to_ie - fill in the supported rate in IE field
- *
- * return : set the bit for each supported rate insert in ie
- */
-static u16 iwl4965_supported_rate_to_ie(u8 *ie, u16 supported_rate,
-                                    u16 basic_rate, int *left)
-{
-        u16 ret_rates = 0, bit;
-        int i;
-        u8 *cnt = ie;
-        u8 *rates = ie + 1;
-        for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
-                if (bit & supported_rate) {
-                        ret_rates |= bit;
-                        rates[*cnt] = iwl_rates[i].ieee |
-                                ((bit & basic_rate) ? 0x80 : 0x00);
-                        (*cnt)++;
-                        (*left)--;
-                        if ((*left <= 0) ||
-                            (*cnt >= IWL_SUPPORTED_RATES_IE_LEN))
-                                break;
-                }
-        }
-        return ret_rates;
-}
-#ifdef CONFIG_IWL4965_HT
 static void iwl4965_ht_conf(struct iwl_priv *priv,
                            struct ieee80211_bss_conf *bss_conf)
 {
@@ -665,9 +532,11 @@ static void iwl4965_ht_conf(struct iwl_priv *priv,
        iwl_conf->extension_chan_offset =
                ht_bss_conf->bss_cap & IEEE80211_HT_IE_CHA_SEC_OFFSET;
        /* If no above or below channel supplied disable FAT channel */
-        if (iwl_conf->extension_chan_offset != IWL_EXT_CHANNEL_OFFSET_ABOVE &&
+        if (iwl_conf->extension_chan_offset != IEEE80211_HT_IE_CHA_SEC_ABOVE &&
-            iwl_conf->extension_chan_offset != IWL_EXT_CHANNEL_OFFSET_BELOW)
+            iwl_conf->extension_chan_offset != IEEE80211_HT_IE_CHA_SEC_BELOW) {
+                iwl_conf->extension_chan_offset = IEEE80211_HT_IE_CHA_SEC_NONE;
                iwl_conf->supported_chan_width = 0;
+        }
        iwl_conf->tx_mimo_ps_mode =
                (u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
@@ -685,151 +554,6 @@ static void iwl4965_ht_conf(struct iwl_priv *priv,
        IWL_DEBUG_MAC80211("leave\n");
 }
-static void iwl_ht_cap_to_ie(const struct ieee80211_supported_band *sband,
-                        u8 *pos, int *left)
-{
-        struct ieee80211_ht_cap *ht_cap;
-        if (!sband || !sband->ht_info.ht_supported)
-                return;
-        if (*left < sizeof(struct ieee80211_ht_cap))
-                return;
-        *pos++ = sizeof(struct ieee80211_ht_cap);
-        ht_cap = (struct ieee80211_ht_cap *) pos;
-        ht_cap->cap_info = cpu_to_le16(sband->ht_info.cap);
-        memcpy(ht_cap->supp_mcs_set, sband->ht_info.supp_mcs_set, 16);
-        ht_cap->ampdu_params_info =
-                (sband->ht_info.ampdu_factor & IEEE80211_HT_CAP_AMPDU_FACTOR) |
-                ((sband->ht_info.ampdu_density << 2) &
-                        IEEE80211_HT_CAP_AMPDU_DENSITY);
-        *left -= sizeof(struct ieee80211_ht_cap);
-}
-#else
-static inline void iwl4965_ht_conf(struct iwl_priv *priv,
-                                   struct ieee80211_bss_conf *bss_conf)
-{
-}
-static void iwl_ht_cap_to_ie(const struct ieee80211_supported_band *sband,
-                        u8 *pos, int *left)
-{
-}
-#endif
-/**
- * iwl4965_fill_probe_req - fill in all required fields and IE for probe request
- */
-static u16 iwl4965_fill_probe_req(struct iwl_priv *priv,
-                                  enum ieee80211_band band,
-                                  struct ieee80211_mgmt *frame,
-                                  int left, int is_direct)
-{
-        int len = 0;
-        u8 *pos = NULL;
-        u16 active_rates, ret_rates, cck_rates, active_rate_basic;
-        const struct ieee80211_supported_band *sband =
-                                                iwl_get_hw_mode(priv, band);
-        /* Make sure there is enough space for the probe request,
-         * two mandatory IEs and the data */
-        left -= 24;
-        if (left < 0)
-                return 0;
-        len += 24;
-        frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-        memcpy(frame->da, iwl_bcast_addr, ETH_ALEN);
-        memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
-        memcpy(frame->bssid, iwl_bcast_addr, ETH_ALEN);
-        frame->seq_ctrl = 0;
-        /* fill in our indirect SSID IE */
-        /* ...next IE... */
-        left -= 2;
-        if (left < 0)
-                return 0;
-        len += 2;
-        pos = &(frame->u.probe_req.variable[0]);
-        *pos++ = WLAN_EID_SSID;
-        *pos++ = 0;
-        /* fill in our direct SSID IE... */
-        if (is_direct) {
-                /* ...next IE... */
-                left -= 2 + priv->essid_len;
-                if (left < 0)
-                        return 0;
-                /* ... fill it in... */
-                *pos++ = WLAN_EID_SSID;
-                *pos++ = priv->essid_len;
-                memcpy(pos, priv->essid, priv->essid_len);
-                pos += priv->essid_len;
-                len += 2 + priv->essid_len;
-        }
-        /* fill in supported rate */
-        /* ...next IE... */
-        left -= 2;
-        if (left < 0)
-                return 0;
-        /* ... fill it in... */
-        *pos++ = WLAN_EID_SUPP_RATES;
-        *pos = 0;
-        /* exclude 60M rate */
-        active_rates = priv->rates_mask;
-        active_rates &= ~IWL_RATE_60M_MASK;
-        active_rate_basic = active_rates & IWL_BASIC_RATES_MASK;
-        cck_rates = IWL_CCK_RATES_MASK & active_rates;
-        ret_rates = iwl4965_supported_rate_to_ie(pos, cck_rates,
-                        active_rate_basic, &left);
-        active_rates &= ~ret_rates;
-        ret_rates = iwl4965_supported_rate_to_ie(pos, active_rates,
-                                 active_rate_basic, &left);
-        active_rates &= ~ret_rates;
-        len += 2 + *pos;
-        pos += (*pos) + 1;
-        if (active_rates == 0)
-                goto fill_end;
-        /* fill in supported extended rate */
-        /* ...next IE... */
-        left -= 2;
-        if (left < 0)
-                return 0;
-        /* ... fill it in... */
-        *pos++ = WLAN_EID_EXT_SUPP_RATES;
-        *pos = 0;
-        iwl4965_supported_rate_to_ie(pos, active_rates,
-                                 active_rate_basic, &left);
-        if (*pos > 0)
-                len += 2 + *pos;
- fill_end:
-        /* fill in HT IE */
-        left -= 2;
-        if (left < 0)
-                return 0;
-        *pos++ = WLAN_EID_HT_CAPABILITY;
-        *pos = 0;
-        iwl_ht_cap_to_ie(sband, pos, &left);
-        if (*pos > 0)
-                len += 2 + *pos;
-        return (u16)len;
-}
 /*
 * QoS  support
 */
@@ -862,10 +586,8 @@ static void iwl4965_activate_qos(struct iwl_priv *priv, u8 force)
                priv->qos_data.def_qos_parm.qos_flags |=
                        QOS_PARAM_FLG_UPDATE_EDCA_MSK;
-#ifdef CONFIG_IWL4965_HT
        if (priv->current_ht_config.is_ht)
                priv->qos_data.def_qos_parm.qos_flags |= QOS_PARAM_FLG_TGN_MSK;
-#endif /* CONFIG_IWL4965_HT */
        spin_unlock_irqrestore(&priv->lock, flags);
@@ -909,60 +631,6 @@ int iwl4965_is_network_packet(struct iwl_priv *priv, struct ieee80211_hdr *heade
        return 1;
 }
-/**
- * iwl4965_scan_cancel - Cancel any currently executing HW scan
- *
- * NOTE: priv->mutex is not required before calling this function
- */
-static int iwl4965_scan_cancel(struct iwl_priv *priv)
-{
-        if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
-                clear_bit(STATUS_SCANNING, &priv->status);
-                return 0;
-        }
-        if (test_bit(STATUS_SCANNING, &priv->status)) {
-                if (!test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
-                        IWL_DEBUG_SCAN("Queuing scan abort.\n");
-                        set_bit(STATUS_SCAN_ABORTING, &priv->status);
-                        queue_work(priv->workqueue, &priv->abort_scan);
-                } else
-                        IWL_DEBUG_SCAN("Scan abort already in progress.\n");
-                return test_bit(STATUS_SCANNING, &priv->status);
-        }
-        return 0;
-}
-/**
- * iwl4965_scan_cancel_timeout - Cancel any currently executing HW scan
- * @ms: amount of time to wait (in milliseconds) for scan to abort
- *
- * NOTE: priv->mutex must be held before calling this function
- */
-static int iwl4965_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
-{
-        unsigned long now = jiffies;
-        int ret;
-        ret = iwl4965_scan_cancel(priv);
-        if (ret && ms) {
-                mutex_unlock(&priv->mutex);
-                while (!time_after(jiffies, now + msecs_to_jiffies(ms)) &&
-                                test_bit(STATUS_SCANNING, &priv->status))
-                        msleep(1);
-                mutex_lock(&priv->mutex);
-                return test_bit(STATUS_SCANNING, &priv->status);
-        }
-        return ret;
-}
 static void iwl4965_sequence_reset(struct iwl_priv *priv)
 {
        /* Reset ieee stats */
@@ -974,7 +642,7 @@ static void iwl4965_sequence_reset(struct iwl_priv *priv)
        priv->last_frag_num = -1;
        priv->last_packet_time = 0;
-        iwl4965_scan_cancel(priv);
+        iwl_scan_cancel(priv);
 }
 #define MAX_UCODE_BEACON_INTERVAL       4096
@@ -1049,41 +717,6 @@ static void iwl4965_setup_rxon_timing(struct iwl_priv *priv)
                le16_to_cpu(priv->rxon_timing.atim_window));
 }
-static int iwl4965_scan_initiate(struct iwl_priv *priv)
-{
-        if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
-                IWL_ERROR("APs don't scan.\n");
-                return 0;
-        }
-        if (!iwl_is_ready_rf(priv)) {
-                IWL_DEBUG_SCAN("Aborting scan due to not ready.\n");
-                return -EIO;
-        }
-        if (test_bit(STATUS_SCANNING, &priv->status)) {
-                IWL_DEBUG_SCAN("Scan already in progress.\n");
-                return -EAGAIN;
-        }
-        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
-                IWL_DEBUG_SCAN("Scan request while abort pending.  "
-                               "Queuing.\n");
-                return -EAGAIN;
-        }
-        IWL_DEBUG_INFO("Starting scan...\n");
-        priv->scan_bands = 2;
-        set_bit(STATUS_SCANNING, &priv->status);
-        priv->scan_start = jiffies;
-        priv->scan_pass_start = priv->scan_start;
-        queue_work(priv->workqueue, &priv->request_scan);
-        return 0;
-}
 static void iwl_set_flags_for_band(struct iwl_priv *priv,
                                   enum ieee80211_band band)
 {
@@ -1188,22 +821,11 @@ static void iwl4965_connection_init_rx_config(struct iwl_priv *priv)
 static int iwl4965_set_mode(struct iwl_priv *priv, int mode)
 {
-        if (mode == IEEE80211_IF_TYPE_IBSS) {
-                const struct iwl_channel_info *ch_info;
-                ch_info = iwl_get_channel_info(priv,
-                        priv->band,
-                        le16_to_cpu(priv->staging_rxon.channel));
-                if (!ch_info || !is_channel_ibss(ch_info)) {
-                        IWL_ERROR("channel %d not IBSS channel\n",
-                                  le16_to_cpu(priv->staging_rxon.channel));
-                        return -EINVAL;
-                }
-        }
        priv->iw_mode = mode;
+        /* init channel/phymode to values given at driver init */
+        iwl_set_rxon_channel(priv, IEEE80211_BAND_2GHZ, 6);
        iwl4965_connection_init_rx_config(priv);
        memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
@@ -1214,7 +836,7 @@ static int iwl4965_set_mode(struct iwl_priv *priv, int mode)
                return -EAGAIN;
        cancel_delayed_work(&priv->scan_check);
-        if (iwl4965_scan_cancel_timeout(priv, 100)) {
+        if (iwl_scan_cancel_timeout(priv, 100)) {
                IWL_WARNING("Aborted scan still in progress after 100ms\n");
                IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
                return -EAGAIN;
@@ -1272,64 +894,6 @@ static void iwl4965_set_rate(struct iwl_priv *priv)
                   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
 }
-void iwl4965_radio_kill_sw(struct iwl_priv *priv, int disable_radio)
-{
-        unsigned long flags;
-        if (!!disable_radio == test_bit(STATUS_RF_KILL_SW, &priv->status))
-                return;
-        IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO %s\n",
-                          disable_radio ? "OFF" : "ON");
-        if (disable_radio) {
-                iwl4965_scan_cancel(priv);
-                /* FIXME: This is a workaround for AP */
-                if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
-                        spin_lock_irqsave(&priv->lock, flags);
-                        iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
-                                    CSR_UCODE_SW_BIT_RFKILL);
-                        spin_unlock_irqrestore(&priv->lock, flags);
-                        /* call the host command only if no hw rf-kill set */
-                        if (!test_bit(STATUS_RF_KILL_HW, &priv->status) &&
-                            iwl_is_ready(priv))
-                                iwl4965_send_card_state(priv,
-                                                        CARD_STATE_CMD_DISABLE,
-                                                        0);
-                        set_bit(STATUS_RF_KILL_SW, &priv->status);
-                        /* make sure mac80211 stop sending Tx frame */
-                        if (priv->mac80211_registered)
-                                ieee80211_stop_queues(priv->hw);
-                }
-                return;
-        }
-        spin_lock_irqsave(&priv->lock, flags);
-        iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
-        clear_bit(STATUS_RF_KILL_SW, &priv->status);
-        spin_unlock_irqrestore(&priv->lock, flags);
-        /* wake up ucode */
-        msleep(10);
-        spin_lock_irqsave(&priv->lock, flags);
-        iwl_read32(priv, CSR_UCODE_DRV_GP1);
-        if (!iwl_grab_nic_access(priv))
-                iwl_release_nic_access(priv);
-        spin_unlock_irqrestore(&priv->lock, flags);
-        if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
-                IWL_DEBUG_RF_KILL("Can not turn radio back on - "
-                                  "disabled by HW switch\n");
-                return;
-        }
-        queue_work(priv->workqueue, &priv->restart);
-        return;
-}
 #define IWL_PACKET_RETRY_TIME HZ
 int iwl4965_is_duplicate_packet(struct iwl_priv *priv, struct ieee80211_hdr *header)
@@ -1661,17 +1225,37 @@ static void iwl4965_bg_beacon_update(struct work_struct *work)
        iwl4965_send_beacon_cmd(priv);
 }
+/**
+ * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
+ *
+ * This callback is provided in order to send a statistics request.
+ *
+ * This timer function is continually reset to execute within
+ * REG_RECALIB_PERIOD seconds since the last STATISTICS_NOTIFICATION
+ * was received.  We need to ensure we receive the statistics in order
+ * to update the temperature used for calibrating the TXPOWER.
+ */
+static void iwl4965_bg_statistics_periodic(unsigned long data)
+{
+        struct iwl_priv *priv = (struct iwl_priv *)data;
+        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+                return;
+        iwl_send_statistics_request(priv, CMD_ASYNC);
+}
 static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
                                struct iwl_rx_mem_buffer *rxb)
 {
 #ifdef CONFIG_IWLWIFI_DEBUG
        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
        struct iwl4965_beacon_notif *beacon = &(pkt->u.beacon_status);
-        u8 rate = iwl4965_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+        u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
        IWL_DEBUG_RX("beacon status %x retries %d iss %d "
                "tsf %d %d rate %d\n",
-                le32_to_cpu(beacon->beacon_notify_hdr.status) & TX_STATUS_MSK,
+                le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
                beacon->beacon_notify_hdr.failure_frame,
                le32_to_cpu(beacon->ibss_mgr_status),
                le32_to_cpu(beacon->high_tsf),
@@ -1683,118 +1267,6 @@ static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
                queue_work(priv->workqueue, &priv->beacon_update);
 }
-/* Service response to REPLY_SCAN_CMD (0x80) */
-static void iwl4965_rx_reply_scan(struct iwl_priv *priv,
-                              struct iwl_rx_mem_buffer *rxb)
-{
-#ifdef CONFIG_IWLWIFI_DEBUG
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_scanreq_notification *notif =
-            (struct iwl4965_scanreq_notification *)pkt->u.raw;
-        IWL_DEBUG_RX("Scan request status = 0x%x\n", notif->status);
-#endif
-}
-/* Service SCAN_START_NOTIFICATION (0x82) */
-static void iwl4965_rx_scan_start_notif(struct iwl_priv *priv,
-                                    struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_scanstart_notification *notif =
-            (struct iwl4965_scanstart_notification *)pkt->u.raw;
-        priv->scan_start_tsf = le32_to_cpu(notif->tsf_low);
-        IWL_DEBUG_SCAN("Scan start: "
-                       "%d [802.11%s] "
-                       "(TSF: 0x%08X:%08X) - %d (beacon timer %u)\n",
-                       notif->channel,
-                       notif->band ? "bg" : "a",
-                       notif->tsf_high,
-                       notif->tsf_low, notif->status, notif->beacon_timer);
-}
-/* Service SCAN_RESULTS_NOTIFICATION (0x83) */
-static void iwl4965_rx_scan_results_notif(struct iwl_priv *priv,
-                                      struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_scanresults_notification *notif =
-            (struct iwl4965_scanresults_notification *)pkt->u.raw;
-        IWL_DEBUG_SCAN("Scan ch.res: "
-                       "%d [802.11%s] "
-                       "(TSF: 0x%08X:%08X) - %d "
-                       "elapsed=%lu usec (%dms since last)\n",
-                       notif->channel,
-                       notif->band ? "bg" : "a",
-                       le32_to_cpu(notif->tsf_high),
-                       le32_to_cpu(notif->tsf_low),
-                       le32_to_cpu(notif->statistics[0]),
-                       le32_to_cpu(notif->tsf_low) - priv->scan_start_tsf,
-                       jiffies_to_msecs(elapsed_jiffies
-                                        (priv->last_scan_jiffies, jiffies)));
-        priv->last_scan_jiffies = jiffies;
-        priv->next_scan_jiffies = 0;
-}
-/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
-static void iwl4965_rx_scan_complete_notif(struct iwl_priv *priv,
-                                       struct iwl_rx_mem_buffer *rxb)
-{
-        struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
-        struct iwl4965_scancomplete_notification *scan_notif = (void *)pkt->u.raw;
-        IWL_DEBUG_SCAN("Scan complete: %d channels (TSF 0x%08X:%08X) - %d\n",
-                       scan_notif->scanned_channels,
-                       scan_notif->tsf_low,
-                       scan_notif->tsf_high, scan_notif->status);
-        /* The HW is no longer scanning */
-        clear_bit(STATUS_SCAN_HW, &priv->status);
-        /* The scan completion notification came in, so kill that timer... */
-        cancel_delayed_work(&priv->scan_check);
-        IWL_DEBUG_INFO("Scan pass on %sGHz took %dms\n",
-                       (priv->scan_bands == 2) ? "2.4" : "5.2",
-                       jiffies_to_msecs(elapsed_jiffies
-                                        (priv->scan_pass_start, jiffies)));
-        /* Remove this scanned band from the list
-         * of pending bands to scan */
-        priv->scan_bands--;
-        /* If a request to abort was given, or the scan did not succeed
-         * then we reset the scan state machine and terminate,
-         * re-queuing another scan if one has been requested */
-        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
-                IWL_DEBUG_INFO("Aborted scan completed.\n");
-                clear_bit(STATUS_SCAN_ABORTING, &priv->status);
-        } else {
-                /* If there are more bands on this scan pass reschedule */
-                if (priv->scan_bands > 0)
-                        goto reschedule;
-        }
-        priv->last_scan_jiffies = jiffies;
-        priv->next_scan_jiffies = 0;
-        IWL_DEBUG_INFO("Setting scan to off\n");
-        clear_bit(STATUS_SCANNING, &priv->status);
-        IWL_DEBUG_INFO("Scan took %dms\n",
-                jiffies_to_msecs(elapsed_jiffies(priv->scan_start, jiffies)));
-        queue_work(priv->workqueue, &priv->scan_completed);
-        return;
-reschedule:
-        priv->scan_pass_start = jiffies;
-        queue_work(priv->workqueue, &priv->request_scan);
-}
 /* Handle notification from uCode that card's power state is changing
 * due to software, hardware, or critical temperature RFKILL */
 static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
@@ -1855,7 +1327,7 @@ static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
                clear_bit(STATUS_RF_KILL_SW, &priv->status);
        if (!(flags & RXON_CARD_DISABLED))
-                iwl4965_scan_cancel(priv);
+                iwl_scan_cancel(priv);
        if ((test_bit(STATUS_RF_KILL_HW, &status) !=
             test_bit(STATUS_RF_KILL_HW, &priv->status)) ||
@@ -1905,13 +1377,9 @@ 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;
+        iwl_setup_rx_scan_handlers(priv);
-        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;
@@ -2049,42 +1517,6 @@ void iwl_rx_handle(struct iwl_priv *priv)
        priv->rxq.read = i;
        iwl_rx_queue_restock(priv);
 }
-/* Convert linear signal-to-noise ratio into dB */
-static u8 ratio2dB[100] = {
-/*       0   1   2   3   4   5   6   7   8   9 */
-         0,  0,  6, 10, 12, 14, 16, 17, 18, 19, /* 00 - 09 */
-        20, 21, 22, 22, 23, 23, 24, 25, 26, 26, /* 10 - 19 */
-        26, 26, 26, 27, 27, 28, 28, 28, 29, 29, /* 20 - 29 */
-        29, 30, 30, 30, 31, 31, 31, 31, 32, 32, /* 30 - 39 */
-        32, 32, 32, 33, 33, 33, 33, 33, 34, 34, /* 40 - 49 */
-        34, 34, 34, 34, 35, 35, 35, 35, 35, 35, /* 50 - 59 */
-        36, 36, 36, 36, 36, 36, 36, 37, 37, 37, /* 60 - 69 */
-        37, 37, 37, 37, 37, 38, 38, 38, 38, 38, /* 70 - 79 */
-        38, 38, 38, 38, 38, 39, 39, 39, 39, 39, /* 80 - 89 */
-        39, 39, 39, 39, 39, 40, 40, 40, 40, 40  /* 90 - 99 */
-};
-/* Calculates a relative dB value from a ratio of linear
- *   (i.e. not dB) signal levels.
- * Conversion assumes that levels are voltages (20*log), not powers (10*log). */
-int iwl4965_calc_db_from_ratio(int sig_ratio)
-{
-        /* 1000:1 or higher just report as 60 dB */
-        if (sig_ratio >= 1000)
-                return 60;
-        /* 100:1 or higher, divide by 10 and use table,
-         *   add 20 dB to make up for divide by 10 */
-        if (sig_ratio >= 100)
-                return (20 + (int)ratio2dB[sig_ratio/10]);
-        /* We shouldn't see this */
-        if (sig_ratio < 1)
-                return 0;
-        /* Use table for ratios 1:1 - 99:1 */
-        return (int)ratio2dB[sig_ratio];
-}
 #define PERFECT_RSSI (-20) /* dBm */
 #define WORST_RSSI (-95)   /* dBm */
@@ -2459,138 +1891,6 @@ static irqreturn_t iwl4965_isr(int irq, void *data)
        return IRQ_NONE;
 }
-/* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
- * sending probe req.  This should be set long enough to hear probe responses
- * from more than one AP.  */
-#define IWL_ACTIVE_DWELL_TIME_24    (20)        /* all times in msec */
-#define IWL_ACTIVE_DWELL_TIME_52    (10)
-/* For faster active scanning, scan will move to the next channel if fewer than
- * PLCP_QUIET_THRESH packets are heard on this channel within
- * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
- * time if it's a quiet channel (nothing responded to our probe, and there's
- * no other traffic).
- * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
-#define IWL_PLCP_QUIET_THRESH       __constant_cpu_to_le16(1)   /* packets */
-#define IWL_ACTIVE_QUIET_TIME       __constant_cpu_to_le16(5)   /* msec */
-/* For passive scan, listen PASSIVE_DWELL_TIME (msec) on each channel.
- * Must be set longer than active dwell time.
- * For the most reliable scan, set > AP beacon interval (typically 100msec). */
-#define IWL_PASSIVE_DWELL_TIME_24   (20)        /* all times in msec */
-#define IWL_PASSIVE_DWELL_TIME_52   (10)
-#define IWL_PASSIVE_DWELL_BASE      (100)
-#define IWL_CHANNEL_TUNE_TIME       5
-static inline u16 iwl4965_get_active_dwell_time(struct iwl_priv *priv,
-                                                enum ieee80211_band band)
-{
-        if (band == IEEE80211_BAND_5GHZ)
-                return IWL_ACTIVE_DWELL_TIME_52;
-        else
-                return IWL_ACTIVE_DWELL_TIME_24;
-}
-static u16 iwl4965_get_passive_dwell_time(struct iwl_priv *priv,
-                                          enum ieee80211_band band)
-{
-        u16 active = iwl4965_get_active_dwell_time(priv, band);
-        u16 passive = (band != IEEE80211_BAND_5GHZ) ?
-            IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_24 :
-            IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_52;
-        if (iwl_is_associated(priv)) {
-                /* If we're associated, we clamp the maximum passive
-                 * dwell time to be 98% of the beacon interval (minus
-                 * 2 * channel tune time) */
-                passive = priv->beacon_int;
-                if ((passive > IWL_PASSIVE_DWELL_BASE) || !passive)
-                        passive = IWL_PASSIVE_DWELL_BASE;
-                passive = (passive * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
-        }
-        if (passive <= active)
-                passive = active + 1;
-        return passive;
-}
-static int iwl4965_get_channels_for_scan(struct iwl_priv *priv,
-                                         enum ieee80211_band band,
-                                     u8 is_active, u8 direct_mask,
-                                     struct iwl4965_scan_channel *scan_ch)
-{
-        const struct ieee80211_channel *channels = NULL;
-        const struct ieee80211_supported_band *sband;
-        const struct iwl_channel_info *ch_info;
-        u16 passive_dwell = 0;
-        u16 active_dwell = 0;
-        int added, i;
-        sband = iwl_get_hw_mode(priv, band);
-        if (!sband)
-                return 0;
-        channels = sband->channels;
-        active_dwell = iwl4965_get_active_dwell_time(priv, band);
-        passive_dwell = iwl4965_get_passive_dwell_time(priv, band);
-        for (i = 0, added = 0; i < sband->n_channels; i++) {
-                if (channels[i].flags & IEEE80211_CHAN_DISABLED)
-                        continue;
-                scan_ch->channel = ieee80211_frequency_to_channel(channels[i].center_freq);
-                ch_info = iwl_get_channel_info(priv, band,
-                                         scan_ch->channel);
-                if (!is_channel_valid(ch_info)) {
-                        IWL_DEBUG_SCAN("Channel %d is INVALID for this SKU.\n",
-                                       scan_ch->channel);
-                        continue;
-                }
-                if (!is_active || is_channel_passive(ch_info) ||
-                    (channels[i].flags & IEEE80211_CHAN_PASSIVE_SCAN))
-                        scan_ch->type = 0;      /* passive */
-                else
-                        scan_ch->type = 1;      /* active */
-                if (scan_ch->type & 1)
-                        scan_ch->type |= (direct_mask << 1);
-                scan_ch->active_dwell = cpu_to_le16(active_dwell);
-                scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
-                /* Set txpower levels to defaults */
-                scan_ch->tpc.dsp_atten = 110;
-                /* scan_pwr_info->tpc.dsp_atten; */
-                /*scan_pwr_info->tpc.tx_gain; */
-                if (band == IEEE80211_BAND_5GHZ)
-                        scan_ch->tpc.tx_gain = ((1 << 5) | (3 << 3)) | 3;
-                else {
-                        scan_ch->tpc.tx_gain = ((1 << 5) | (5 << 3));
-                        /* NOTE: if we were doing 6Mb OFDM for scans we'd use
-                         * power level:
-                         * scan_ch->tpc.tx_gain = ((1 << 5) | (2 << 3)) | 3;
-                         */
-                }
-                IWL_DEBUG_SCAN("Scanning %d [%s %d]\n",
-                               scan_ch->channel,
-                               (scan_ch->type & 1) ? "ACTIVE" : "PASSIVE",
-                               (scan_ch->type & 1) ?
-                               active_dwell : passive_dwell);
-                scan_ch++;
-                added++;
-        }
-        IWL_DEBUG_SCAN("total channels to scan %d \n", added);
-        return added;
-}
 /******************************************************************************
 *
 * uCode download functions
@@ -2621,7 +1921,7 @@ static void iwl4965_nic_start(struct iwl_priv *priv)
 */
 static int iwl4965_read_ucode(struct iwl_priv *priv)
 {
-        struct iwl4965_ucode *ucode;
+        struct iwl_ucode *ucode;
        int ret;
        const struct firmware *ucode_raw;
        const char *name = priv->cfg->fw_name;
@@ -2849,9 +2149,6 @@ static void iwl_alive_start(struct iwl_priv *priv)
        /* After the ALIVE response, we can send host commands to 4965 uCode */
        set_bit(STATUS_ALIVE, &priv->status);
-        /* Clear out the uCode error bit if it is set */
-        clear_bit(STATUS_FW_ERROR, &priv->status);
        if (iwl_is_rfkill(priv))
                return;
@@ -2882,7 +2179,7 @@ static void iwl_alive_start(struct iwl_priv *priv)
        iwl4965_commit_rxon(priv);
        /* At this point, the NIC is initialized and operational */
-        iwl4965_rf_kill_ct_config(priv);
+        iwl_rf_kill_ct_config(priv);
        iwl_leds_register(priv);
@@ -2893,15 +2190,19 @@ static void iwl_alive_start(struct iwl_priv *priv)
        if (priv->error_recovering)
                iwl4965_error_recovery(priv);
-        iwlcore_low_level_notify(priv, IWLCORE_START_EVT);
+        iwl_power_update_mode(priv, 1);
        ieee80211_notify_mac(priv->hw, IEEE80211_NOTIFY_RE_ASSOC);
+        if (test_and_clear_bit(STATUS_MODE_PENDING, &priv->status))
+                iwl4965_set_mode(priv, priv->iw_mode);
        return;
 restart:
        queue_work(priv->workqueue, &priv->restart);
 }
-static void iwl4965_cancel_deferred_work(struct iwl_priv *priv);
+static void iwl_cancel_deferred_work(struct iwl_priv *priv);
 static void __iwl4965_down(struct iwl_priv *priv)
 {
@@ -2915,8 +2216,6 @@ static void __iwl4965_down(struct iwl_priv *priv)
        iwl_leds_unregister(priv);
-        iwlcore_low_level_notify(priv, IWLCORE_STOP_EVT);
        iwlcore_clear_stations_table(priv);
        /* Unblock any waiting calls */
@@ -3005,7 +2304,7 @@ static void iwl4965_down(struct iwl_priv *priv)
        __iwl4965_down(priv);
        mutex_unlock(&priv->mutex);
-        iwl4965_cancel_deferred_work(priv);
+        iwl_cancel_deferred_work(priv);
 }
 #define MAX_HW_RESTARTS 5
@@ -3020,13 +2319,6 @@ static int __iwl4965_up(struct iwl_priv *priv)
                return -EIO;
        }
-        if (test_bit(STATUS_RF_KILL_SW, &priv->status)) {
-                IWL_WARNING("Radio disabled by SW RF kill (module "
-                            "parameter)\n");
-                iwl_rfkill_set_hw_state(priv);
-                return -ENODEV;
-        }
        if (!priv->ucode_data_backup.v_addr || !priv->ucode_data.v_addr) {
                IWL_ERROR("ucode not available for device bringup\n");
                return -EIO;
@@ -3080,7 +2372,8 @@ static int __iwl4965_up(struct iwl_priv *priv)
               priv->ucode_data.len);
        /* We return success when we resume from suspend and rf_kill is on. */
-        if (test_bit(STATUS_RF_KILL_HW, &priv->status))
+        if (test_bit(STATUS_RF_KILL_HW, &priv->status) ||
+            test_bit(STATUS_RF_KILL_SW, &priv->status))
                return 0;
        for (i = 0; i < MAX_HW_RESTARTS; i++) {
@@ -3097,6 +2390,9 @@ static int __iwl4965_up(struct iwl_priv *priv)
                        continue;
                }
+                /* Clear out the uCode error bit if it is set */
+                clear_bit(STATUS_FW_ERROR, &priv->status);
                /* start card; "initialize" will load runtime ucode */
                iwl4965_nic_start(priv);
@@ -3107,6 +2403,7 @@ static int __iwl4965_up(struct iwl_priv *priv)
        set_bit(STATUS_EXIT_PENDING, &priv->status);
        __iwl4965_down(priv);
+        clear_bit(STATUS_EXIT_PENDING, &priv->status);
        /* tried to restart and config the device for as long as our
         * patience could withstand */
@@ -3186,255 +2483,45 @@ static void iwl4965_bg_set_monitor(struct work_struct *work)
 {
        struct iwl_priv *priv = container_of(work,
                                struct iwl_priv, set_monitor);
+        int ret;
        IWL_DEBUG(IWL_DL_STATE, "setting monitor mode\n");
        mutex_lock(&priv->mutex);
-        if (!iwl_is_ready(priv))
+        ret = iwl4965_set_mode(priv, IEEE80211_IF_TYPE_MNTR);
-                IWL_DEBUG(IWL_DL_STATE, "leave - not ready\n");
-        else
-                if (iwl4965_set_mode(priv, IEEE80211_IF_TYPE_MNTR) != 0)
-                        IWL_ERROR("iwl4965_set_mode() failed\n");
-        mutex_unlock(&priv->mutex);
-}
-#define IWL_SCAN_CHECK_WATCHDOG (7 * HZ)
+        if (ret) {
+                if (ret == -EAGAIN)
-static void iwl4965_bg_scan_check(struct work_struct *data)
+                        IWL_DEBUG(IWL_DL_STATE, "leave - not ready\n");
-{
+                else
-        struct iwl_priv *priv =
+                        IWL_ERROR("iwl4965_set_mode() failed ret = %d\n", ret);
-            container_of(data, struct iwl_priv, scan_check.work);
-        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-                return;
-        mutex_lock(&priv->mutex);
-        if (test_bit(STATUS_SCANNING, &priv->status) ||
-            test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
-                IWL_DEBUG(IWL_DL_SCAN, "Scan completion watchdog resetting "
-                        "adapter (%dms)\n",
-                        jiffies_to_msecs(IWL_SCAN_CHECK_WATCHDOG));
-                if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
-                        iwl4965_send_scan_abort(priv);
        }
        mutex_unlock(&priv->mutex);
 }
-static void iwl4965_bg_request_scan(struct work_struct *data)
+static void iwl_bg_run_time_calib_work(struct work_struct *work)
 {
-        struct iwl_priv *priv =
+        struct iwl_priv *priv = container_of(work, struct iwl_priv,
-            container_of(data, struct iwl_priv, request_scan);
+                        run_time_calib_work);
-        struct iwl_host_cmd cmd = {
-                .id = REPLY_SCAN_CMD,
-                .len = sizeof(struct iwl4965_scan_cmd),
-                .meta.flags = CMD_SIZE_HUGE,
-        };
-        struct iwl4965_scan_cmd *scan;
-        struct ieee80211_conf *conf = NULL;
-        u16 cmd_len;
-        enum ieee80211_band band;
-        u8 direct_mask;
-        int ret = 0;
-        conf = ieee80211_get_hw_conf(priv->hw);
        mutex_lock(&priv->mutex);
-        if (!iwl_is_ready(priv)) {
+        if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
-                IWL_WARNING("request scan called when driver not ready.\n");
+            test_bit(STATUS_SCANNING, &priv->status)) {
-                goto done;
+                mutex_unlock(&priv->mutex);
-        }
+                return;
-        /* Make sure the scan wasn't cancelled before this queued work
-         * was given the chance to run... */
-        if (!test_bit(STATUS_SCANNING, &priv->status))
-                goto done;
-        /* This should never be called or scheduled if there is currently
-         * a scan active in the hardware. */
-        if (test_bit(STATUS_SCAN_HW, &priv->status)) {
-                IWL_DEBUG_INFO("Multiple concurrent scan requests in parallel. "
-                               "Ignoring second request.\n");
-                ret = -EIO;
-                goto done;
-        }
-        if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
-                IWL_DEBUG_SCAN("Aborting scan due to device shutdown\n");
-                goto done;
-        }
-        if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
-                IWL_DEBUG_HC("Scan request while abort pending.  Queuing.\n");
-                goto done;
-        }
-        if (iwl_is_rfkill(priv)) {
-                IWL_DEBUG_HC("Aborting scan due to RF Kill activation\n");
-                goto done;
-        }
-        if (!test_bit(STATUS_READY, &priv->status)) {
-                IWL_DEBUG_HC("Scan request while uninitialized.  Queuing.\n");
-                goto done;
-        }
-        if (!priv->scan_bands) {
-                IWL_DEBUG_HC("Aborting scan due to no requested bands\n");
-                goto done;
-        }
-        if (!priv->scan) {
-                priv->scan = kmalloc(sizeof(struct iwl4965_scan_cmd) +
-                                     IWL_MAX_SCAN_SIZE, GFP_KERNEL);
-                if (!priv->scan) {
-                        ret = -ENOMEM;
-                        goto done;
-                }
-        }
-        scan = priv->scan;
-        memset(scan, 0, sizeof(struct iwl4965_scan_cmd) + IWL_MAX_SCAN_SIZE);
-        scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
-        scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
-        if (iwl_is_associated(priv)) {
-                u16 interval = 0;
-                u32 extra;
-                u32 suspend_time = 100;
-                u32 scan_suspend_time = 100;
-                unsigned long flags;
-                IWL_DEBUG_INFO("Scanning while associated...\n");
-                spin_lock_irqsave(&priv->lock, flags);
-                interval = priv->beacon_int;
-                spin_unlock_irqrestore(&priv->lock, flags);
-                scan->suspend_time = 0;
-                scan->max_out_time = cpu_to_le32(200 * 1024);
-                if (!interval)
-                        interval = suspend_time;
-                extra = (suspend_time / interval) << 22;
-                scan_suspend_time = (extra |
-                    ((suspend_time % interval) * 1024));
-                scan->suspend_time = cpu_to_le32(scan_suspend_time);
-                IWL_DEBUG_SCAN("suspend_time 0x%X beacon interval %d\n",
-                               scan_suspend_time, interval);
-        }
-        /* We should add the ability for user to lock to PASSIVE ONLY */
-        if (priv->one_direct_scan) {
-                IWL_DEBUG_SCAN
-                    ("Kicking off one direct scan for '%s'\n",
-                     iwl4965_escape_essid(priv->direct_ssid,
-                                      priv->direct_ssid_len));
-                scan->direct_scan[0].id = WLAN_EID_SSID;
-                scan->direct_scan[0].len = priv->direct_ssid_len;
-                memcpy(scan->direct_scan[0].ssid,
-                       priv->direct_ssid, priv->direct_ssid_len);
-                direct_mask = 1;
-        } else if (!iwl_is_associated(priv) && priv->essid_len) {
-                IWL_DEBUG_SCAN
-                  ("Kicking off one direct scan for '%s' when not associated\n",
-                   iwl4965_escape_essid(priv->essid, priv->essid_len));
-                scan->direct_scan[0].id = WLAN_EID_SSID;
-                scan->direct_scan[0].len = priv->essid_len;
-                memcpy(scan->direct_scan[0].ssid, priv->essid, priv->essid_len);
-                direct_mask = 1;
-        } else {
-                IWL_DEBUG_SCAN("Kicking off one indirect scan.\n");
-                direct_mask = 0;
        }
-        scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+        if (priv->start_calib) {
-        scan->tx_cmd.sta_id = priv->hw_params.bcast_sta_id;
+                iwl_chain_noise_calibration(priv, &priv->statistics);
-        scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
-        switch (priv->scan_bands) {
-        case 2:
-                scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
-                scan->tx_cmd.rate_n_flags =
-                                iwl4965_hw_set_rate_n_flags(IWL_RATE_1M_PLCP,
-                                RATE_MCS_ANT_B_MSK|RATE_MCS_CCK_MSK);
-                scan->good_CRC_th = 0;
-                band = IEEE80211_BAND_2GHZ;
-                break;
-        case 1:
-                scan->tx_cmd.rate_n_flags =
-                                iwl4965_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
-                                RATE_MCS_ANT_B_MSK);
-                scan->good_CRC_th = IWL_GOOD_CRC_TH;
-                band = IEEE80211_BAND_5GHZ;
-                break;
-        default:
+                iwl_sensitivity_calibration(priv, &priv->statistics);
-                IWL_WARNING("Invalid scan band count\n");
-                goto done;
        }
-        /* We don't build a direct scan probe request; the uCode will do
-         * that based on the direct_mask added to each channel entry */
-        cmd_len = iwl4965_fill_probe_req(priv, band,
-                                        (struct ieee80211_mgmt *)scan->data,
-                                        IWL_MAX_SCAN_SIZE - sizeof(*scan), 0);
-        scan->tx_cmd.len = cpu_to_le16(cmd_len);
-        /* select Rx chains */
-        /* Force use of chains B and C (0x6) for scan Rx.
-         * Avoid A (0x1) because of its off-channel reception on A-band.
-         * MIMO is not used here, but value is required to make uCode happy. */
-        scan->rx_chain = RXON_RX_CHAIN_DRIVER_FORCE_MSK |
-                        cpu_to_le16((0x7 << RXON_RX_CHAIN_VALID_POS) |
-                        (0x6 << RXON_RX_CHAIN_FORCE_SEL_POS) |
-                        (0x7 << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS));
-        if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR)
-                scan->filter_flags = RXON_FILTER_PROMISC_MSK;
-        if (direct_mask)
-                scan->channel_count =
-                        iwl4965_get_channels_for_scan(
-                                priv, band, 1, /* active */
-                                direct_mask,
-                                (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
-        else
-                scan->channel_count =
-                        iwl4965_get_channels_for_scan(
-                                priv, band, 0, /* passive */
-                                direct_mask,
-                                (void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
-        scan->filter_flags |= (RXON_FILTER_ACCEPT_GRP_MSK |
-                               RXON_FILTER_BCON_AWARE_MSK);
-        cmd.len += le16_to_cpu(scan->tx_cmd.len) +
-            scan->channel_count * sizeof(struct iwl4965_scan_channel);
-        cmd.data = scan;
-        scan->len = cpu_to_le16(cmd.len);
-        set_bit(STATUS_SCAN_HW, &priv->status);
-        ret = iwl_send_cmd_sync(priv, &cmd);
-        if (ret)
-                goto done;
-        queue_delayed_work(priv->workqueue, &priv->scan_check,
-                           IWL_SCAN_CHECK_WATCHDOG);
        mutex_unlock(&priv->mutex);
        return;
- done:
-        /* inform mac80211 scan aborted */
-        queue_work(priv->workqueue, &priv->scan_completed);
-        mutex_unlock(&priv->mutex);
 }
 static void iwl4965_bg_up(struct work_struct *data)
@@ -3498,7 +2585,7 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
        if (!priv->vif || !priv->is_open)
                return;
-        iwl4965_scan_cancel_timeout(priv, 200);
+        iwl_scan_cancel_timeout(priv, 200);
        conf = ieee80211_get_hw_conf(priv->hw);
@@ -3515,10 +2602,9 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
        priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
-#ifdef CONFIG_IWL4965_HT
        if (priv->current_ht_config.is_ht)
                iwl_set_rxon_ht(priv, &priv->current_ht_config);
-#endif /* CONFIG_IWL4965_HT*/
        iwl_set_rxon_chain(priv);
        priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
@@ -3550,10 +2636,9 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
        case IEEE80211_IF_TYPE_IBSS:
-                /* clear out the station table */
+                /* assume default assoc id */
-                iwlcore_clear_stations_table(priv);
+                priv->assoc_id = 1;
-                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);
@@ -3594,29 +2679,14 @@ static void iwl4965_bg_post_associate(struct work_struct *data)
 }
-static void iwl4965_bg_abort_scan(struct work_struct *work)
-{
-        struct iwl_priv *priv = container_of(work, struct iwl_priv, abort_scan);
-        if (!iwl_is_ready(priv))
-                return;
-        mutex_lock(&priv->mutex);
-        set_bit(STATUS_SCAN_ABORTING, &priv->status);
-        iwl4965_send_scan_abort(priv);
-        mutex_unlock(&priv->mutex);
-}
 static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf);
-static void iwl4965_bg_scan_completed(struct work_struct *work)
+static void iwl_bg_scan_completed(struct work_struct *work)
 {
        struct iwl_priv *priv =
            container_of(work, struct iwl_priv, scan_completed);
-        IWL_DEBUG(IWL_DL_SCAN, "SCAN complete scan\n");
+        IWL_DEBUG_SCAN("SCAN complete scan\n");
        if (test_bit(STATUS_EXIT_PENDING, &priv->status))
                return;
@@ -3629,7 +2699,7 @@ static void iwl4965_bg_scan_completed(struct work_struct *work)
        /* Since setting the TXPOWER may have been deferred while
         * performing the scan, fire one off */
        mutex_lock(&priv->mutex);
-        iwl4965_hw_reg_send_txpower(priv);
+        iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
        mutex_unlock(&priv->mutex);
 }
@@ -3738,7 +2808,7 @@ static void iwl4965_mac_stop(struct ieee80211_hw *hw)
                 * RXON_FILTER_ASSOC_MSK BIT
                 */
                mutex_lock(&priv->mutex);
-                iwl4965_scan_cancel_timeout(priv, 100);
+                iwl_scan_cancel_timeout(priv, 100);
                cancel_delayed_work(&priv->post_associate);
                mutex_unlock(&priv->mutex);
        }
@@ -3801,8 +2871,9 @@ static int iwl4965_mac_add_interface(struct ieee80211_hw *hw,
                memcpy(priv->mac_addr, conf->mac_addr, ETH_ALEN);
        }
-        if (iwl_is_ready(priv))
+        if (iwl4965_set_mode(priv, conf->type) == -EAGAIN)
-                iwl4965_set_mode(priv, conf->type);
+                /* we are not ready, will run again when ready */
+                set_bit(STATUS_MODE_PENDING, &priv->status);
        mutex_unlock(&priv->mutex);
@@ -3830,6 +2901,14 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
        priv->add_radiotap = !!(conf->flags & IEEE80211_CONF_RADIOTAP);
+        if (conf->radio_enabled && iwl_radio_kill_sw_enable_radio(priv)) {
+                IWL_DEBUG_MAC80211("leave - RF-KILL - waiting for uCode\n");
+                goto out;
+        }
+        if (!conf->radio_enabled)
+                iwl_radio_kill_sw_disable_radio(priv);
        if (!iwl_is_ready(priv)) {
                IWL_DEBUG_MAC80211("leave - not ready\n");
                ret = -EIO;
@@ -3852,9 +2931,16 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
                goto out;
        }
+        if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
+            !is_channel_ibss(ch_info)) {
+                IWL_ERROR("channel %d in band %d not IBSS channel\n",
+                        conf->channel->hw_value, conf->channel->band);
+                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 */
@@ -3864,7 +2950,6 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
 #endif
        )
                priv->staging_rxon.flags = 0;
-#endif /* CONFIG_IWL4965_HT */
        iwl_set_rxon_channel(priv, conf->channel->band, channel);
@@ -3884,9 +2969,6 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
        }
 #endif
-        if (priv->cfg->ops->lib->radio_kill_sw)
-                priv->cfg->ops->lib->radio_kill_sw(priv, !conf->radio_enabled);
        if (!conf->radio_enabled) {
                IWL_DEBUG_MAC80211("leave - radio disabled\n");
                goto out;
@@ -3898,6 +2980,11 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
                goto out;
        }
+        IWL_DEBUG_MAC80211("TX Power old=%d new=%d\n",
+                           priv->tx_power_user_lmt, conf->power_level);
+        iwl_set_tx_power(priv, conf->power_level, false);
        iwl4965_set_rate(priv);
        if (memcmp(&priv->active_rxon,
@@ -4034,7 +3121,7 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
            !is_multicast_ether_addr(conf->bssid)) {
                /* If there is currently a HW scan going on in the background
                 * then we need to cancel it else the RXON below will fail. */
-                if (iwl4965_scan_cancel_timeout(priv, 100)) {
+                if (iwl_scan_cancel_timeout(priv, 100)) {
                        IWL_WARNING("Aborted scan still in progress "
                                    "after 100ms\n");
                        IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
@@ -4059,7 +3146,7 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
                }
        } else {
-                iwl4965_scan_cancel_timeout(priv, 100);
+                iwl_scan_cancel_timeout(priv, 100);
                priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
                iwl4965_commit_rxon(priv);
        }
@@ -4117,7 +3204,7 @@ static void iwl4965_mac_remove_interface(struct ieee80211_hw *hw,
        mutex_lock(&priv->mutex);
        if (iwl_is_ready_rf(priv)) {
-                iwl4965_scan_cancel_timeout(priv, 100);
+                iwl_scan_cancel_timeout(priv, 100);
                cancel_delayed_work(&priv->post_associate);
                priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
                iwl4965_commit_rxon(priv);
@@ -4231,7 +3318,7 @@ static int iwl4965_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
        }
        if (len) {
                IWL_DEBUG_SCAN("direct scan for %s [%d]\n ",
-                               iwl4965_escape_essid(ssid, len), (int)len);
+                               iwl_escape_essid(ssid, len), (int)len);
                priv->one_direct_scan = 1;
                priv->direct_ssid_len = (u8)
@@ -4240,7 +3327,7 @@ static int iwl4965_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
        } else
                priv->one_direct_scan = 0;
-        rc = iwl4965_scan_initiate(priv);
+        rc = iwl_scan_initiate(priv);
        IWL_DEBUG_MAC80211("leave\n");
@@ -4271,7 +3358,7 @@ static void iwl4965_mac_update_tkip_key(struct ieee80211_hw *hw,
                return;
        }
-        iwl4965_scan_cancel_timeout(priv, 100);
+        iwl_scan_cancel_timeout(priv, 100);
        key_flags |= (STA_KEY_FLG_TKIP | STA_KEY_FLG_MAP_KEY_MSK);
        key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
@@ -4329,7 +3416,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
        }
        mutex_lock(&priv->mutex);
-        iwl4965_scan_cancel_timeout(priv, 100);
+        iwl_scan_cancel_timeout(priv, 100);
        mutex_unlock(&priv->mutex);
        /* If we are getting WEP group key and we didn't receive any key mapping
@@ -4489,11 +3576,9 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
        IWL_DEBUG_MAC80211("enter\n");
        priv->lq_mngr.lq_ready = 0;
-#ifdef CONFIG_IWL4965_HT
        spin_lock_irqsave(&priv->lock, flags);
        memset(&priv->current_ht_config, 0, sizeof(struct iwl_ht_info));
        spin_unlock_irqrestore(&priv->lock, flags);
-#endif /* CONFIG_IWL4965_HT */
        iwl_reset_qos(priv);
@@ -4527,7 +3612,7 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
         * clear RXON_FILTER_ASSOC_MSK bit
         */
        if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
-                iwl4965_scan_cancel_timeout(priv, 100);
+                iwl_scan_cancel_timeout(priv, 100);
                priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
                iwl4965_commit_rxon(priv);
        }
@@ -4583,7 +3668,7 @@ static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *sk
        iwl_reset_qos(priv);
-        queue_work(priv->workqueue, &priv->post_associate.work);
+        iwl4965_post_associate(priv);
        mutex_unlock(&priv->mutex);
@@ -4665,7 +3750,7 @@ static ssize_t show_temperature(struct device *d,
        if (!iwl_is_alive(priv))
                return -EAGAIN;
-        return sprintf(buf, "%d\n", iwl4965_hw_get_temperature(priv));
+        return sprintf(buf, "%d\n", priv->temperature);
 }
 static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
@@ -4683,7 +3768,7 @@ static ssize_t show_tx_power(struct device *d,
                             struct device_attribute *attr, char *buf)
 {
        struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
-        return sprintf(buf, "%d\n", priv->user_txpower_limit);
+        return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
 }
 static ssize_t store_tx_power(struct device *d,
@@ -4699,7 +3784,7 @@ static ssize_t store_tx_power(struct device *d,
                printk(KERN_INFO DRV_NAME
                       ": %s is not in decimal form.\n", buf);
        else
-                iwl4965_hw_reg_set_txpower(priv, val);
+                iwl_set_tx_power(priv, val, false);
        return count;
 }
@@ -4724,7 +3809,7 @@ static ssize_t store_flags(struct device *d,
        mutex_lock(&priv->mutex);
        if (le32_to_cpu(priv->staging_rxon.flags) != flags) {
                /* Cancel any currently running scans... */
-                if (iwl4965_scan_cancel_timeout(priv, 100))
+                if (iwl_scan_cancel_timeout(priv, 100))
                        IWL_WARNING("Could not cancel scan.\n");
                else {
                        IWL_DEBUG_INFO("Committing rxon.flags = 0x%04X\n",
@@ -4759,7 +3844,7 @@ static ssize_t store_filter_flags(struct device *d,
        mutex_lock(&priv->mutex);
        if (le32_to_cpu(priv->staging_rxon.filter_flags) != filter_flags) {
                /* Cancel any currently running scans... */
-                if (iwl4965_scan_cancel_timeout(priv, 100))
+                if (iwl_scan_cancel_timeout(priv, 100))
                        IWL_WARNING("Could not cancel scan.\n");
                else {
                        IWL_DEBUG_INFO("Committing rxon.filter_flags = "
@@ -4957,8 +4042,62 @@ static DEVICE_ATTR(power_level, S_IWUSR | S_IRUSR, show_power_level,
 static ssize_t show_channels(struct device *d,
                             struct device_attribute *attr, char *buf)
 {
-        /* all this shit doesn't belong into sysfs anyway */
-        return 0;
+        struct iwl_priv *priv = dev_get_drvdata(d);
+        struct ieee80211_channel *channels = NULL;
+        const struct ieee80211_supported_band *supp_band = NULL;
+        int len = 0, i;
+        int count = 0;
+        if (!test_bit(STATUS_GEO_CONFIGURED, &priv->status))
+                return -EAGAIN;
+        supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_2GHZ);
+        channels = supp_band->channels;
+        count = supp_band->n_channels;
+        len += sprintf(&buf[len],
+                        "Displaying %d channels in 2.4GHz band "
+                        "(802.11bg):\n", count);
+        for (i = 0; i < count; i++)
+                len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+                                ieee80211_frequency_to_channel(
+                                channels[i].center_freq),
+                                channels[i].max_power,
+                                channels[i].flags & IEEE80211_CHAN_RADAR ?
+                                " (IEEE 802.11h required)" : "",
+                                (!(channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+                                || (channels[i].flags &
+                                IEEE80211_CHAN_RADAR)) ? "" :
+                                ", IBSS",
+                                channels[i].flags &
+                                IEEE80211_CHAN_PASSIVE_SCAN ?
+                                "passive only" : "active/passive");
+        supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_5GHZ);
+        channels = supp_band->channels;
+        count = supp_band->n_channels;
+        len += sprintf(&buf[len], "Displaying %d channels in 5.2GHz band "
+                        "(802.11a):\n", count);
+        for (i = 0; i < count; i++)
+                len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+                                ieee80211_frequency_to_channel(
+                                channels[i].center_freq),
+                                channels[i].max_power,
+                                channels[i].flags & IEEE80211_CHAN_RADAR ?
+                                " (IEEE 802.11h required)" : "",
+                                ((channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+                                || (channels[i].flags &
+                                IEEE80211_CHAN_RADAR)) ? "" :
+                                ", IBSS",
+                                channels[i].flags &
+                                IEEE80211_CHAN_PASSIVE_SCAN ?
+                                "passive only" : "active/passive");
+        return len;
 }
 static DEVICE_ATTR(channels, S_IRUSR, show_channels, NULL);
@@ -5018,7 +4157,7 @@ static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
 *
 *****************************************************************************/
-static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
 {
        priv->workqueue = create_workqueue(DRV_NAME);
@@ -5027,32 +4166,40 @@ static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
        INIT_WORK(&priv->up, iwl4965_bg_up);
        INIT_WORK(&priv->restart, iwl4965_bg_restart);
        INIT_WORK(&priv->rx_replenish, iwl4965_bg_rx_replenish);
-        INIT_WORK(&priv->scan_completed, iwl4965_bg_scan_completed);
-        INIT_WORK(&priv->request_scan, iwl4965_bg_request_scan);
-        INIT_WORK(&priv->abort_scan, iwl4965_bg_abort_scan);
        INIT_WORK(&priv->rf_kill, iwl4965_bg_rf_kill);
        INIT_WORK(&priv->beacon_update, iwl4965_bg_beacon_update);
        INIT_WORK(&priv->set_monitor, iwl4965_bg_set_monitor);
+        INIT_WORK(&priv->run_time_calib_work, iwl_bg_run_time_calib_work);
        INIT_DELAYED_WORK(&priv->post_associate, iwl4965_bg_post_associate);
        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);
+        /* FIXME : remove when resolved PENDING */
+        INIT_WORK(&priv->scan_completed, iwl_bg_scan_completed);
+        iwl_setup_scan_deferred_work(priv);
+        if (priv->cfg->ops->lib->setup_deferred_work)
+                priv->cfg->ops->lib->setup_deferred_work(priv);
+        init_timer(&priv->statistics_periodic);
+        priv->statistics_periodic.data = (unsigned long)priv;
+        priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
        tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
                     iwl4965_irq_tasklet, (unsigned long)priv);
 }
-static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
+static void iwl_cancel_deferred_work(struct iwl_priv *priv)
 {
-        iwl4965_hw_cancel_deferred_work(priv);
+        if (priv->cfg->ops->lib->cancel_deferred_work)
+                priv->cfg->ops->lib->cancel_deferred_work(priv);
        cancel_delayed_work_sync(&priv->init_alive_start);
        cancel_delayed_work(&priv->scan_check);
        cancel_delayed_work(&priv->alive_start);
        cancel_delayed_work(&priv->post_associate);
        cancel_work_sync(&priv->beacon_update);
+        del_timer_sync(&priv->statistics_periodic);
 }
 static struct attribute *iwl4965_sysfs_entries[] = {
@@ -5100,9 +4247,7 @@ static struct ieee80211_ops iwl4965_hw_ops = {
        .reset_tsf = iwl4965_mac_reset_tsf,
        .beacon_update = iwl4965_mac_beacon_update,
        .bss_info_changed = iwl4965_bss_info_changed,
-#ifdef CONFIG_IWL4965_HT
        .ampdu_action = iwl4965_mac_ampdu_action,
-#endif  /* CONFIG_IWL4965_HT */
        .hw_scan = iwl4965_mac_hw_scan
 };
@@ -5266,7 +4411,7 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
        }
-        iwl4965_setup_deferred_work(priv);
+        iwl_setup_deferred_work(priv);
        iwl4965_setup_rx_handlers(priv);
        /********************
@@ -5287,8 +4432,11 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
        if (err)
                IWL_ERROR("failed to create debugfs files\n");
-        /* notify iwlcore to init */
+        err = iwl_rfkill_init(priv);
-        iwlcore_low_level_notify(priv, IWLCORE_INIT_EVT);
+        if (err)
+                IWL_ERROR("Unable to initialize RFKILL system. "
+                                  "Ignoring error: %d\n", err);
+        iwl_power_initialize(priv);
        return 0;
 out_remove_sysfs:
@@ -5351,8 +4499,7 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
                }
        }
-        iwlcore_low_level_notify(priv, IWLCORE_REMOVE_EVT);
+        iwl_rfkill_unregister(priv);
        iwl4965_dealloc_ucode_pci(priv);
        if (priv->rxq.bd)
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index 873ab10a0786..cc70d53fadd3 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -159,7 +159,9 @@ static int if_cs_poll_while_fw_download(struct if_cs_card *card, uint addr, u8 r
-/* First the bitmasks for the host/card interrupt/status registers: */
+/*
+ * 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
@@ -167,35 +169,110 @@ static int if_cs_poll_while_fw_download(struct if_cs_card *card, uint addr, u8 r
 #define IF_CS_BIT_EVENT                 0x0010
 #define IF_CS_BIT_MASK                  0x001f
-/* And now the individual registers and assorted masks */
+/*
+ * It's not really clear to me what the host status register is for. It
+ * needs to be set almost in union with "host int cause". The following
+ * bits from above are used:
+ *
+ *   IF_CS_BIT_TX         driver downloaded a data packet
+ *   IF_CS_BIT_RX         driver got a data packet
+ *   IF_CS_BIT_COMMAND    driver downloaded a command
+ *   IF_CS_BIT_RESP       not used (has some meaning with powerdown)
+ *   IF_CS_BIT_EVENT      driver read a host event
+ */
 #define IF_CS_HOST_STATUS               0x00000000
+/*
+ * With the host int cause register can the host (that is, Linux) cause
+ * an interrupt in the firmware, to tell the firmware about those events:
+ *
+ *   IF_CS_BIT_TX         a data packet has been downloaded
+ *   IF_CS_BIT_RX         a received data packet has retrieved
+ *   IF_CS_BIT_COMMAND    a firmware block or a command has been downloaded
+ *   IF_CS_BIT_RESP       not used (has some meaning with powerdown)
+ *   IF_CS_BIT_EVENT      a host event (link lost etc) has been retrieved
+ */
 #define IF_CS_HOST_INT_CAUSE            0x00000002
+/*
+ * The host int mask register is used to enable/disable interrupt.  However,
+ * I have the suspicion that disabled interrupts are lost.
+ */
 #define IF_CS_HOST_INT_MASK             0x00000004
-#define IF_CS_HOST_WRITE                0x00000016
+/*
-#define IF_CS_HOST_WRITE_LEN            0x00000014
+ * Used to send or receive data packets:
+ */
-#define IF_CS_HOST_CMD                  0x0000001A
+#define IF_CS_WRITE                     0x00000016
-#define IF_CS_HOST_CMD_LEN              0x00000018
+#define IF_CS_WRITE_LEN                 0x00000014
 #define IF_CS_READ                      0x00000010
 #define IF_CS_READ_LEN                  0x00000024
-#define IF_CS_CARD_CMD                  0x00000012
+/*
-#define IF_CS_CARD_CMD_LEN              0x00000030
+ * Used to send commands (and to send firmware block) and to
+ * receive command responses:
+ */
+#define IF_CS_CMD                       0x0000001A
+#define IF_CS_CMD_LEN                   0x00000018
+#define IF_CS_RESP                      0x00000012
+#define IF_CS_RESP_LEN                  0x00000030
+/*
+ * The card status registers shows what the card/firmware actually
+ * accepts:
+ *
+ *   IF_CS_BIT_TX        you may send a data packet
+ *   IF_CS_BIT_RX        you may retrieve a data packet
+ *   IF_CS_BIT_COMMAND   you may send a command
+ *   IF_CS_BIT_RESP      you may retrieve a command response
+ *   IF_CS_BIT_EVENT     the card has a event for use (link lost, snr low etc)
+ *
+ * When reading this register several times, you will get back the same
+ * results --- with one exception: the IF_CS_BIT_EVENT clear itself
+ * automatically.
+ *
+ * Not that we don't rely on BIT_RX,_BIT_RESP or BIT_EVENT because
+ * we handle this via the card int cause register.
+ */
 #define IF_CS_CARD_STATUS               0x00000020
 #define IF_CS_CARD_STATUS_MASK          0x7f00
+/*
+ * The card int cause register is used by the card/firmware to notify us
+ * about the following events:
+ *
+ *   IF_CS_BIT_TX        a data packet has successfully been sentx
+ *   IF_CS_BIT_RX        a data packet has been received and can be retrieved
+ *   IF_CS_BIT_COMMAND   not used
+ *   IF_CS_BIT_RESP      the firmware has a command response for us
+ *   IF_CS_BIT_EVENT     the card has a event for use (link lost, snr low etc)
+ */
 #define IF_CS_CARD_INT_CAUSE            0x00000022
-#define IF_CS_CARD_SQ_READ_LOW          0x00000028
+/*
-#define IF_CS_CARD_SQ_HELPER_OK         0x10
+ * This is used to for handshaking with the card's bootloader/helper image
+ * to synchronize downloading of firmware blocks.
+ */
+#define IF_CS_SQ_READ_LOW               0x00000028
+#define IF_CS_SQ_HELPER_OK              0x10
+/*
+ * The scratch register tells us ...
+ *
+ * IF_CS_SCRATCH_BOOT_OK     the bootloader runs
+ * IF_CS_SCRATCH_HELPER_OK   the helper firmware already runs
+ */
 #define IF_CS_SCRATCH                   0x0000003F
+#define IF_CS_SCRATCH_BOOT_OK           0x00
+#define IF_CS_SCRATCH_HELPER_OK         0x5a
+/*
+ * Used to detect ancient chips:
+ */
+#define IF_CS_PRODUCT_ID                0x0000001C
+#define IF_CS_CF8385_B1_REV             0x12
 /********************************************************************/
@@ -228,8 +305,8 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
        /* Is hardware ready? */
        while (1) {
-                u16 val = if_cs_read16(card, IF_CS_CARD_STATUS);
+                u16 status = if_cs_read16(card, IF_CS_CARD_STATUS);
-                if (val & IF_CS_BIT_COMMAND)
+                if (status & IF_CS_BIT_COMMAND)
                        break;
                if (++loops > 100) {
                        lbs_pr_err("card not ready for commands\n");
@@ -238,12 +315,12 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
                mdelay(1);
        }
-        if_cs_write16(card, IF_CS_HOST_CMD_LEN, nb);
+        if_cs_write16(card, IF_CS_CMD_LEN, nb);
-        if_cs_write16_rep(card, IF_CS_HOST_CMD, buf, nb / 2);
+        if_cs_write16_rep(card, IF_CS_CMD, buf, nb / 2);
        /* Are we supposed to transfer an odd amount of bytes? */
        if (nb & 1)
-                if_cs_write8(card, IF_CS_HOST_CMD, buf[nb-1]);
+                if_cs_write8(card, IF_CS_CMD, buf[nb-1]);
        /* "Assert the download over interrupt command in the Host
         * status register" */
@@ -274,12 +351,12 @@ static void if_cs_send_data(struct lbs_private *priv, u8 *buf, u16 nb)
        status = if_cs_read16(card, IF_CS_CARD_STATUS);
        BUG_ON((status & IF_CS_BIT_TX) == 0);
-        if_cs_write16(card, IF_CS_HOST_WRITE_LEN, nb);
+        if_cs_write16(card, IF_CS_WRITE_LEN, nb);
        /* write even number of bytes, then odd byte if necessary */
-        if_cs_write16_rep(card, IF_CS_HOST_WRITE, buf, nb / 2);
+        if_cs_write16_rep(card, IF_CS_WRITE, buf, nb / 2);
        if (nb & 1)
-                if_cs_write8(card, IF_CS_HOST_WRITE, buf[nb-1]);
+                if_cs_write8(card, IF_CS_WRITE, buf[nb-1]);
        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);
@@ -307,16 +384,16 @@ static int if_cs_receive_cmdres(struct lbs_private *priv, u8 *data, u32 *len)
                goto out;
        }
-        *len = if_cs_read16(priv->card, IF_CS_CARD_CMD_LEN);
+        *len = if_cs_read16(priv->card, IF_CS_RESP_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_CARD_CMD, data, *len/sizeof(u16));
+        if_cs_read16_rep(priv->card, IF_CS_RESP, data, *len/sizeof(u16));
        if (*len & 1)
-                data[*len-1] = if_cs_read8(priv->card, IF_CS_CARD_CMD);
+                data[*len-1] = if_cs_read8(priv->card, IF_CS_RESP);
        /* This is a workaround for a firmware that reports too much
         * bytes */
@@ -379,6 +456,8 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
        /* Ask card interrupt cause register if there is something for us */
        cause = if_cs_read16(card, IF_CS_CARD_INT_CAUSE);
+        lbs_deb_cs("cause 0x%04x\n", cause);
        if (cause == 0) {
                /* Not for us */
                return IRQ_NONE;
@@ -390,10 +469,6 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
                return IRQ_HANDLED;
        }
-        /* 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_BIT_RX) {
                struct sk_buff *skb;
                lbs_deb_cs("rx packet\n");
@@ -426,14 +501,15 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
        }
        if (cause & IF_CS_BIT_EVENT) {
-                u16 event = if_cs_read16(priv->card, IF_CS_CARD_STATUS)
+                u16 status = 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, (status & IF_CS_CARD_STATUS_MASK) >> 8);
-                lbs_queue_event(priv, event >> 8 & 0xff);
        }
+        /* Clear interrupt cause */
+        if_cs_write16(card, IF_CS_CARD_INT_CAUSE, cause & IF_CS_BIT_MASK);
        lbs_deb_leave(LBS_DEB_CS);
        return IRQ_HANDLED;
 }
@@ -464,11 +540,11 @@ static int if_cs_prog_helper(struct if_cs_card *card)
        /* "If the value is 0x5a, the firmware is already
         * downloaded successfully"
         */
-        if (scratch == 0x5a)
+        if (scratch == IF_CS_SCRATCH_HELPER_OK)
                goto done;
        /* "If the value is != 00, it is invalid value of register */
-        if (scratch != 0x00) {
+        if (scratch != IF_CS_SCRATCH_BOOT_OK) {
                ret = -ENODEV;
                goto done;
        }
@@ -496,11 +572,11 @@ 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_HOST_CMD_LEN, count);
+                if_cs_write16(card, IF_CS_CMD_LEN, count);
                /* "write this to I/O Command port register as 16 bit writes */
                if (count)
-                        if_cs_write16_rep(card, IF_CS_HOST_CMD,
+                        if_cs_write16_rep(card, IF_CS_CMD,
                                &fw->data[sent],
                                count >> 1);
@@ -557,15 +633,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_CARD_SQ_READ_LOW,
+        ret = if_cs_poll_while_fw_download(card, IF_CS_SQ_READ_LOW,
-                IF_CS_CARD_SQ_HELPER_OK);
+                IF_CS_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_CARD_SQ_READ_LOW);
+                len = if_cs_read16(card, IF_CS_SQ_READ_LOW);
                if (len & 1) {
                        retry++;
                        lbs_pr_info("odd, need to retry this firmware block\n");
@@ -583,9 +659,9 @@ static int if_cs_prog_real(struct if_cs_card *card)
                }
-                if_cs_write16(card, IF_CS_HOST_CMD_LEN, len);
+                if_cs_write16(card, IF_CS_CMD_LEN, len);
-                if_cs_write16_rep(card, IF_CS_HOST_CMD,
+                if_cs_write16_rep(card, IF_CS_CMD,
                        &fw->data[sent],
                        (len+1) >> 1);
                if_cs_write8(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
@@ -789,6 +865,12 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
               p_dev->irq.AssignedIRQ, p_dev->io.BasePort1,
               p_dev->io.BasePort1 + p_dev->io.NumPorts1 - 1);
+        /* Check if we have a current silicon */
+        if (if_cs_read8(card, IF_CS_PRODUCT_ID) < IF_CS_CF8385_B1_REV) {
+                lbs_pr_err("old chips like 8385 rev B1 aren't supported\n");
+                ret = -ENODEV;
+                goto out2;
+        }
        /* Load the firmware early, before calling into libertas.ko */
        ret = if_cs_prog_helper(card);
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
new file mode 100644
index 000000000000..8da352ae6825
--- /dev/null
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -0,0 +1,514 @@
+/*
+ * mac80211_hwsim - software simulator of 802.11 radio(s) for mac80211
+ * Copyright (c) 2008, Jouni Malinen <j@w1.fi>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+/*
+ * TODO:
+ * - IBSS mode simulation (Beacon transmission with competition for "air time")
+ * - IEEE 802.11a and 802.11n modes
+ * - RX filtering based on filter configuration (data->rx_filter)
+ */
+#include <net/mac80211.h>
+#include <net/ieee80211_radiotap.h>
+#include <linux/if_arp.h>
+#include <linux/rtnetlink.h>
+#include <linux/etherdevice.h>
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Software simulator of 802.11 radio(s) for mac80211");
+MODULE_LICENSE("GPL");
+static int radios = 2;
+module_param(radios, int, 0444);
+MODULE_PARM_DESC(radios, "Number of simulated radios");
+static struct class *hwsim_class;
+static struct ieee80211_hw **hwsim_radios;
+static int hwsim_radio_count;
+static struct net_device *hwsim_mon; /* global monitor netdev */
+static const struct ieee80211_channel hwsim_channels[] = {
+        { .center_freq = 2412 },
+        { .center_freq = 2417 },
+        { .center_freq = 2422 },
+        { .center_freq = 2427 },
+        { .center_freq = 2432 },
+        { .center_freq = 2437 },
+        { .center_freq = 2442 },
+        { .center_freq = 2447 },
+        { .center_freq = 2452 },
+        { .center_freq = 2457 },
+        { .center_freq = 2462 },
+        { .center_freq = 2467 },
+        { .center_freq = 2472 },
+        { .center_freq = 2484 },
+};
+static const struct ieee80211_rate hwsim_rates[] = {
+        { .bitrate = 10 },
+        { .bitrate = 20, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+        { .bitrate = 55, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+        { .bitrate = 110, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+        { .bitrate = 60 },
+        { .bitrate = 90 },
+        { .bitrate = 120 },
+        { .bitrate = 180 },
+        { .bitrate = 240 },
+        { .bitrate = 360 },
+        { .bitrate = 480 },
+        { .bitrate = 540 }
+};
+struct mac80211_hwsim_data {
+        struct device *dev;
+        struct ieee80211_supported_band band;
+        struct ieee80211_channel channels[ARRAY_SIZE(hwsim_channels)];
+        struct ieee80211_rate rates[ARRAY_SIZE(hwsim_rates)];
+        struct ieee80211_channel *channel;
+        int radio_enabled;
+        unsigned long beacon_int; /* in jiffies unit */
+        unsigned int rx_filter;
+        int started;
+        struct timer_list beacon_timer;
+};
+struct hwsim_radiotap_hdr {
+        struct ieee80211_radiotap_header hdr;
+        u8 rt_flags;
+        u8 rt_rate;
+        __le16 rt_channel;
+        __le16 rt_chbitmask;
+} __attribute__ ((packed));
+static int hwsim_mon_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+        /* TODO: allow packet injection */
+        dev_kfree_skb(skb);
+        return 0;
+}
+static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
+                                      struct sk_buff *tx_skb)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        struct sk_buff *skb;
+        struct hwsim_radiotap_hdr *hdr;
+        u16 flags;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_skb);
+        struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
+        if (!netif_running(hwsim_mon))
+                return;
+        skb = skb_copy_expand(tx_skb, sizeof(*hdr), 0, GFP_ATOMIC);
+        if (skb == NULL)
+                return;
+        hdr = (struct hwsim_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
+        hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+        hdr->hdr.it_pad = 0;
+        hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
+        hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
+                                          (1 << IEEE80211_RADIOTAP_RATE) |
+                                          (1 << IEEE80211_RADIOTAP_CHANNEL));
+        hdr->rt_flags = 0;
+        hdr->rt_rate = txrate->bitrate / 5;
+        hdr->rt_channel = data->channel->center_freq;
+        flags = IEEE80211_CHAN_2GHZ;
+        if (txrate->flags & IEEE80211_RATE_ERP_G)
+                flags |= IEEE80211_CHAN_OFDM;
+        else
+                flags |= IEEE80211_CHAN_CCK;
+        hdr->rt_chbitmask = cpu_to_le16(flags);
+        skb->dev = hwsim_mon;
+        skb_set_mac_header(skb, 0);
+        skb->ip_summed = CHECKSUM_UNNECESSARY;
+        skb->pkt_type = PACKET_OTHERHOST;
+        skb->protocol = htons(ETH_P_802_2);
+        memset(skb->cb, 0, sizeof(skb->cb));
+        netif_rx(skb);
+}
+static int mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
+                                   struct sk_buff *skb)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        int i, ack = 0;
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+        struct ieee80211_rx_status rx_status;
+        memset(&rx_status, 0, sizeof(rx_status));
+        /* TODO: set mactime */
+        rx_status.freq = data->channel->center_freq;
+        rx_status.band = data->channel->band;
+        rx_status.rate_idx = info->tx_rate_idx;
+        /* TODO: simulate signal strength (and optional packet drop) */
+        /* Copy skb to all enabled radios that are on the current frequency */
+        for (i = 0; i < hwsim_radio_count; i++) {
+                struct mac80211_hwsim_data *data2;
+                struct sk_buff *nskb;
+                if (hwsim_radios[i] == NULL || hwsim_radios[i] == hw)
+                        continue;
+                data2 = hwsim_radios[i]->priv;
+                if (!data2->started || !data2->radio_enabled ||
+                    data->channel->center_freq != data2->channel->center_freq)
+                        continue;
+                nskb = skb_copy(skb, GFP_ATOMIC);
+                if (nskb == NULL)
+                        continue;
+                if (memcmp(hdr->addr1, hwsim_radios[i]->wiphy->perm_addr,
+                           ETH_ALEN) == 0)
+                        ack = 1;
+                ieee80211_rx_irqsafe(hwsim_radios[i], nskb, &rx_status);
+        }
+        return ack;
+}
+static int mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        int ack;
+        struct ieee80211_tx_info *txi;
+        mac80211_hwsim_monitor_rx(hw, skb);
+        if (skb->len < 10) {
+                /* Should not happen; just a sanity check for addr1 use */
+                dev_kfree_skb(skb);
+                return NETDEV_TX_OK;
+        }
+        if (!data->radio_enabled) {
+                printk(KERN_DEBUG "%s: dropped TX frame since radio "
+                       "disabled\n", wiphy_name(hw->wiphy));
+                dev_kfree_skb(skb);
+                return NETDEV_TX_OK;
+        }
+        ack = mac80211_hwsim_tx_frame(hw, skb);
+        txi = IEEE80211_SKB_CB(skb);
+        memset(&txi->status, 0, sizeof(txi->status));
+        if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK)) {
+                if (ack)
+                        txi->flags |= IEEE80211_TX_STAT_ACK;
+                else
+                        txi->status.excessive_retries = 1;
+        }
+        ieee80211_tx_status_irqsafe(hw, skb);
+        return NETDEV_TX_OK;
+}
+static int mac80211_hwsim_start(struct ieee80211_hw *hw)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+        data->started = 1;
+        return 0;
+}
+static void mac80211_hwsim_stop(struct ieee80211_hw *hw)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        data->started = 0;
+        printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+}
+static int mac80211_hwsim_add_interface(struct ieee80211_hw *hw,
+                                        struct ieee80211_if_init_conf *conf)
+{
+        DECLARE_MAC_BUF(mac);
+        printk(KERN_DEBUG "%s:%s (type=%d mac_addr=%s)\n",
+               wiphy_name(hw->wiphy), __func__, conf->type,
+               print_mac(mac, conf->mac_addr));
+        return 0;
+}
+static void mac80211_hwsim_remove_interface(
+        struct ieee80211_hw *hw, struct ieee80211_if_init_conf *conf)
+{
+        DECLARE_MAC_BUF(mac);
+        printk(KERN_DEBUG "%s:%s (type=%d mac_addr=%s)\n",
+               wiphy_name(hw->wiphy), __func__, conf->type,
+               print_mac(mac, conf->mac_addr));
+}
+static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
+                                     struct ieee80211_vif *vif)
+{
+        struct ieee80211_hw *hw = arg;
+        struct sk_buff *skb;
+        struct ieee80211_tx_info *info;
+        if (vif->type != IEEE80211_IF_TYPE_AP)
+                return;
+        skb = ieee80211_beacon_get(hw, vif);
+        if (skb == NULL)
+                return;
+        info = IEEE80211_SKB_CB(skb);
+        mac80211_hwsim_monitor_rx(hw, skb);
+        mac80211_hwsim_tx_frame(hw, skb);
+        dev_kfree_skb(skb);
+}
+static void mac80211_hwsim_beacon(unsigned long arg)
+{
+        struct ieee80211_hw *hw = (struct ieee80211_hw *) arg;
+        struct mac80211_hwsim_data *data = hw->priv;
+        if (!data->started || !data->radio_enabled)
+                return;
+        ieee80211_iterate_active_interfaces_atomic(
+                hw, mac80211_hwsim_beacon_tx, hw);
+        data->beacon_timer.expires = jiffies + data->beacon_int;
+        add_timer(&data->beacon_timer);
+}
+static int mac80211_hwsim_config(struct ieee80211_hw *hw,
+                                 struct ieee80211_conf *conf)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        printk(KERN_DEBUG "%s:%s (freq=%d radio_enabled=%d beacon_int=%d)\n",
+               wiphy_name(hw->wiphy), __func__,
+               conf->channel->center_freq, conf->radio_enabled,
+               conf->beacon_int);
+        data->channel = conf->channel;
+        data->radio_enabled = conf->radio_enabled;
+        data->beacon_int = 1024 * conf->beacon_int / 1000 * HZ / 1000;
+        if (data->beacon_int < 1)
+                data->beacon_int = 1;
+        if (!data->started || !data->radio_enabled)
+                del_timer(&data->beacon_timer);
+        else
+                mod_timer(&data->beacon_timer, jiffies + data->beacon_int);
+        return 0;
+}
+static void mac80211_hwsim_configure_filter(struct ieee80211_hw *hw,
+                                            unsigned int changed_flags,
+                                            unsigned int *total_flags,
+                                            int mc_count,
+                                            struct dev_addr_list *mc_list)
+{
+        struct mac80211_hwsim_data *data = hw->priv;
+        printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+        data->rx_filter = 0;
+        if (*total_flags & FIF_PROMISC_IN_BSS)
+                data->rx_filter |= FIF_PROMISC_IN_BSS;
+        if (*total_flags & FIF_ALLMULTI)
+                data->rx_filter |= FIF_ALLMULTI;
+        *total_flags = data->rx_filter;
+}
+static const struct ieee80211_ops mac80211_hwsim_ops =
+{
+        .tx = mac80211_hwsim_tx,
+        .start = mac80211_hwsim_start,
+        .stop = mac80211_hwsim_stop,
+        .add_interface = mac80211_hwsim_add_interface,
+        .remove_interface = mac80211_hwsim_remove_interface,
+        .config = mac80211_hwsim_config,
+        .configure_filter = mac80211_hwsim_configure_filter,
+};
+static void mac80211_hwsim_free(void)
+{
+        int i;
+        for (i = 0; i < hwsim_radio_count; i++) {
+                if (hwsim_radios[i]) {
+                        struct mac80211_hwsim_data *data;
+                        data = hwsim_radios[i]->priv;
+                        ieee80211_unregister_hw(hwsim_radios[i]);
+                        if (!IS_ERR(data->dev))
+                                device_unregister(data->dev);
+                        ieee80211_free_hw(hwsim_radios[i]);
+                }
+        }
+        kfree(hwsim_radios);
+        class_destroy(hwsim_class);
+}
+static struct device_driver mac80211_hwsim_driver = {
+        .name = "mac80211_hwsim"
+};
+static void hwsim_mon_setup(struct net_device *dev)
+{
+        dev->hard_start_xmit = hwsim_mon_xmit;
+        dev->destructor = free_netdev;
+        ether_setup(dev);
+        dev->tx_queue_len = 0;
+        dev->type = ARPHRD_IEEE80211_RADIOTAP;
+        memset(dev->dev_addr, 0, ETH_ALEN);
+        dev->dev_addr[0] = 0x12;
+}
+static int __init init_mac80211_hwsim(void)
+{
+        int i, err = 0;
+        u8 addr[ETH_ALEN];
+        struct mac80211_hwsim_data *data;
+        struct ieee80211_hw *hw;
+        DECLARE_MAC_BUF(mac);
+        if (radios < 1 || radios > 65535)
+                return -EINVAL;
+        hwsim_radio_count = radios;
+        hwsim_radios = kcalloc(hwsim_radio_count,
+                               sizeof(struct ieee80211_hw *), GFP_KERNEL);
+        if (hwsim_radios == NULL)
+                return -ENOMEM;
+        hwsim_class = class_create(THIS_MODULE, "mac80211_hwsim");
+        if (IS_ERR(hwsim_class)) {
+                kfree(hwsim_radios);
+                return PTR_ERR(hwsim_class);
+        }
+        memset(addr, 0, ETH_ALEN);
+        addr[0] = 0x02;
+        for (i = 0; i < hwsim_radio_count; i++) {
+                printk(KERN_DEBUG "mac80211_hwsim: Initializing radio %d\n",
+                       i);
+                hw = ieee80211_alloc_hw(sizeof(*data), &mac80211_hwsim_ops);
+                if (hw == NULL) {
+                        printk(KERN_DEBUG "mac80211_hwsim: ieee80211_alloc_hw "
+                               "failed\n");
+                        err = -ENOMEM;
+                        goto failed;
+                }
+                hwsim_radios[i] = hw;
+                data = hw->priv;
+                data->dev = device_create(hwsim_class, NULL, 0, "hwsim%d", i);
+                if (IS_ERR(data->dev)) {
+                        printk(KERN_DEBUG "mac80211_hwsim: device_create "
+                               "failed (%ld)\n", PTR_ERR(data->dev));
+                        err = -ENOMEM;
+                        goto failed;
+                }
+                data->dev->driver = &mac80211_hwsim_driver;
+                dev_set_drvdata(data->dev, hw);
+                SET_IEEE80211_DEV(hw, data->dev);
+                addr[3] = i >> 8;
+                addr[4] = i;
+                SET_IEEE80211_PERM_ADDR(hw, addr);
+                hw->channel_change_time = 1;
+                hw->queues = 1;
+                memcpy(data->channels, hwsim_channels, sizeof(hwsim_channels));
+                memcpy(data->rates, hwsim_rates, sizeof(hwsim_rates));
+                data->band.channels = data->channels;
+                data->band.n_channels = ARRAY_SIZE(hwsim_channels);
+                data->band.bitrates = data->rates;
+                data->band.n_bitrates = ARRAY_SIZE(hwsim_rates);
+                hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &data->band;
+                err = ieee80211_register_hw(hw);
+                if (err < 0) {
+                        printk(KERN_DEBUG "mac80211_hwsim: "
+                               "ieee80211_register_hw failed (%d)\n", err);
+                        goto failed;
+                }
+                printk(KERN_DEBUG "%s: hwaddr %s registered\n",
+                       wiphy_name(hw->wiphy),
+                       print_mac(mac, hw->wiphy->perm_addr));
+                setup_timer(&data->beacon_timer, mac80211_hwsim_beacon,
+                            (unsigned long) hw);
+        }
+        hwsim_mon = alloc_netdev(0, "hwsim%d", hwsim_mon_setup);
+        if (hwsim_mon == NULL)
+                goto failed;
+        rtnl_lock();
+        err = dev_alloc_name(hwsim_mon, hwsim_mon->name);
+        if (err < 0) {
+                goto failed_mon;
+        }
+        err = register_netdevice(hwsim_mon);
+        if (err < 0)
+                goto failed_mon;
+        rtnl_unlock();
+        return 0;
+failed_mon:
+        rtnl_unlock();
+        free_netdev(hwsim_mon);
+failed:
+        mac80211_hwsim_free();
+        return err;
+}
+static void __exit exit_mac80211_hwsim(void)
+{
+        printk(KERN_DEBUG "mac80211_hwsim: unregister %d radios\n",
+               hwsim_radio_count);
+        unregister_netdev(hwsim_mon);
+        mac80211_hwsim_free();
+}
+module_init(init_mac80211_hwsim);
+module_exit(exit_mac80211_hwsim);
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 3954897d0678..a36d2c85e26e 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -310,8 +310,11 @@ enum wpa_key_mgmt { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
 #define CAP_MODE_MASK           7
 #define CAP_SUPPORT_TXPOWER     8
-#define WORK_CONNECTION_EVENT   (1<<0)
+#define WORK_LINK_UP            (1<<0)
-#define WORK_SET_MULTICAST_LIST (1<<1)
+#define WORK_LINK_DOWN          (1<<1)
+#define WORK_SET_MULTICAST_LIST (1<<2)
+#define COMMAND_BUFFER_SIZE     (CONTROL_BUFFER_SIZE + sizeof(struct rndis_set))
 /* RNDIS device private data */
 struct rndis_wext_private {
@@ -361,6 +364,8 @@ struct rndis_wext_private {
        u8  *wpa_ie;
        int  wpa_cipher_pair;
        int  wpa_cipher_group;
+        u8 command_buffer[COMMAND_BUFFER_SIZE];
 };
@@ -427,18 +432,23 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
        buflen = *len + sizeof(*u.get);
        if (buflen < CONTROL_BUFFER_SIZE)
                buflen = CONTROL_BUFFER_SIZE;
-        u.buf = kmalloc(buflen, GFP_KERNEL);
-        if (!u.buf)
+        if (buflen > COMMAND_BUFFER_SIZE) {
-                return -ENOMEM;
+                u.buf = kmalloc(buflen, GFP_KERNEL);
+                if (!u.buf)
+                        return -ENOMEM;
+        } else {
+                u.buf = priv->command_buffer;
+        }
+        mutex_lock(&priv->command_lock);
        memset(u.get, 0, sizeof *u.get);
        u.get->msg_type = RNDIS_MSG_QUERY;
        u.get->msg_len = ccpu2(sizeof *u.get);
        u.get->oid = oid;
-        mutex_lock(&priv->command_lock);
        ret = rndis_command(dev, u.header);
-        mutex_unlock(&priv->command_lock);
        if (ret == 0) {
                ret = le32_to_cpu(u.get_c->len);
                *len = (*len > ret) ? ret : *len;
@@ -446,7 +456,10 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
                ret = rndis_error_status(u.get_c->status);
        }
-        kfree(u.buf);
+        mutex_unlock(&priv->command_lock);
+        if (u.buf != priv->command_buffer)
+                kfree(u.buf);
        return ret;
 }
@@ -465,9 +478,16 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
        buflen = len + sizeof(*u.set);
        if (buflen < CONTROL_BUFFER_SIZE)
                buflen = CONTROL_BUFFER_SIZE;
-        u.buf = kmalloc(buflen, GFP_KERNEL);
-        if (!u.buf)
+        if (buflen > COMMAND_BUFFER_SIZE) {
-                return -ENOMEM;
+                u.buf = kmalloc(buflen, GFP_KERNEL);
+                if (!u.buf)
+                        return -ENOMEM;
+        } else {
+                u.buf = priv->command_buffer;
+        }
+        mutex_lock(&priv->command_lock);
        memset(u.set, 0, sizeof *u.set);
        u.set->msg_type = RNDIS_MSG_SET;
@@ -478,14 +498,14 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
        u.set->handle = ccpu2(0);
        memcpy(u.buf + sizeof(*u.set), data, len);
-        mutex_lock(&priv->command_lock);
        ret = rndis_command(dev, u.header);
-        mutex_unlock(&priv->command_lock);
        if (ret == 0)
                ret = rndis_error_status(u.set_c->status);
-        kfree(u.buf);
+        mutex_unlock(&priv->command_lock);
+        if (u.buf != priv->command_buffer)
+                kfree(u.buf);
        return ret;
 }
@@ -620,8 +640,7 @@ static void dsconfig_to_freq(unsigned int dsconfig, struct iw_freq *freq)
 static int freq_to_dsconfig(struct iw_freq *freq, unsigned int *dsconfig)
 {
        if (freq->m < 1000 && freq->e == 0) {
-                if (freq->m >= 1 &&
+                if (freq->m >= 1 && freq->m <= ARRAY_SIZE(freq_chan))
-                        freq->m <= (sizeof(freq_chan) / sizeof(freq_chan[0])))
                        *dsconfig = freq_chan[freq->m - 1] * 1000;
                else
                        return -1;
@@ -1159,10 +1178,9 @@ static int rndis_iw_get_range(struct net_device *dev,
                range->throughput = 11 * 1000 * 1000 / 2;
        }
-        range->num_channels = (sizeof(freq_chan)/sizeof(freq_chan[0]));
+        range->num_channels = ARRAY_SIZE(freq_chan);
-        for (i = 0; i < (sizeof(freq_chan)/sizeof(freq_chan[0])) &&
+        for (i = 0; i < ARRAY_SIZE(freq_chan) && i < IW_MAX_FREQUENCIES; i++) {
-                        i < IW_MAX_FREQUENCIES; i++) {
                range->freq[i].i = i + 1;
                range->freq[i].m = freq_chan[i] * 100000;
                range->freq[i].e = 1;
@@ -2213,7 +2231,9 @@ static void rndis_wext_worker(struct work_struct *work)
        int assoc_size = sizeof(*info) + IW_CUSTOM_MAX + 32;
        int ret, offset;
-        if (test_and_clear_bit(WORK_CONNECTION_EVENT, &priv->work_pending)) {
+        if (test_and_clear_bit(WORK_LINK_UP, &priv->work_pending)) {
+                netif_carrier_on(usbdev->net);
                info = kzalloc(assoc_size, GFP_KERNEL);
                if (!info)
                        goto get_bssid;
@@ -2251,6 +2271,15 @@ get_bssid:
                }
        }
+        if (test_and_clear_bit(WORK_LINK_DOWN, &priv->work_pending)) {
+                netif_carrier_off(usbdev->net);
+                evt.data.flags = 0;
+                evt.data.length = 0;
+                memset(evt.ap_addr.sa_data, 0, ETH_ALEN);
+                wireless_send_event(usbdev->net, SIOCGIWAP, &evt, NULL);
+        }
        if (test_and_clear_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending))
                set_multicast_list(usbdev);
 }
@@ -2260,29 +2289,24 @@ static void rndis_wext_set_multicast_list(struct net_device *dev)
        struct usbnet *usbdev = dev->priv;
        struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
+        if (test_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending))
+                return;
        set_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending);
        queue_work(priv->workqueue, &priv->work);
 }
-static void rndis_wext_link_change(struct usbnet *dev, int state)
+static void rndis_wext_link_change(struct usbnet *usbdev, int state)
 {
-        struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+        struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
-        union iwreq_data evt;
-        if (state) {
+        /* queue work to avoid recursive calls into rndis_command */
-                /* queue work to avoid recursive calls into rndis_command */
+        set_bit(state ? WORK_LINK_UP : WORK_LINK_DOWN, &priv->work_pending);
-                set_bit(WORK_CONNECTION_EVENT, &priv->work_pending);
+        queue_work(priv->workqueue, &priv->work);
-                queue_work(priv->workqueue, &priv->work);
-        } else {
-                evt.data.flags = 0;
-                evt.data.length = 0;
-                memset(evt.ap_addr.sa_data, 0, ETH_ALEN);
-                wireless_send_event(dev->net, SIOCGIWAP, &evt, NULL);
-        }
 }
-static int rndis_wext_get_caps(struct usbnet *dev)
+static int rndis_wext_get_caps(struct usbnet *usbdev)
 {
        struct {
                __le32  num_items;
@@ -2290,18 +2314,18 @@ static int rndis_wext_get_caps(struct usbnet *dev)
        } networks_supported;
        int len, retval, i, n;
        __le32 tx_power;
-        struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+        struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
        /* determine if supports setting txpower */
        len = sizeof(tx_power);
-        retval = rndis_query_oid(dev, OID_802_11_TX_POWER_LEVEL, &tx_power,
+        retval = rndis_query_oid(usbdev, OID_802_11_TX_POWER_LEVEL, &tx_power,
-                                                                &len);
+                                                                        &len);
        if (retval == 0 && le32_to_cpu(tx_power) != 0xFF)
                priv->caps |= CAP_SUPPORT_TXPOWER;
        /* determine supported modes */
        len = sizeof(networks_supported);
-        retval = rndis_query_oid(dev, OID_802_11_NETWORK_TYPES_SUPPORTED,
+        retval = rndis_query_oid(usbdev, OID_802_11_NETWORK_TYPES_SUPPORTED,
                                                &networks_supported, &len);
        if (retval >= 0) {
                n = le32_to_cpu(networks_supported.num_items);
@@ -2440,9 +2464,9 @@ end:
 }
-static int bcm4320_early_init(struct usbnet *dev)
+static int bcm4320_early_init(struct usbnet *usbdev)
 {
-        struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+        struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
        char buf[8];
        /* Early initialization settings, setting these won't have effect
@@ -2490,51 +2514,48 @@ static int bcm4320_early_init(struct usbnet *dev)
        else
                priv->param_workaround_interval = modparam_workaround_interval;
-        rndis_set_config_parameter_str(dev, "Country", priv->param_country);
+        rndis_set_config_parameter_str(usbdev, "Country", priv->param_country);
-        rndis_set_config_parameter_str(dev, "FrameBursting",
+        rndis_set_config_parameter_str(usbdev, "FrameBursting",
                                        priv->param_frameburst ? "1" : "0");
-        rndis_set_config_parameter_str(dev, "Afterburner",
+        rndis_set_config_parameter_str(usbdev, "Afterburner",
                                        priv->param_afterburner ? "1" : "0");
        sprintf(buf, "%d", priv->param_power_save);
-        rndis_set_config_parameter_str(dev, "PowerSaveMode", buf);
+        rndis_set_config_parameter_str(usbdev, "PowerSaveMode", buf);
        sprintf(buf, "%d", priv->param_power_output);
-        rndis_set_config_parameter_str(dev, "PwrOut", buf);
+        rndis_set_config_parameter_str(usbdev, "PwrOut", buf);
        sprintf(buf, "%d", priv->param_roamtrigger);
-        rndis_set_config_parameter_str(dev, "RoamTrigger", buf);
+        rndis_set_config_parameter_str(usbdev, "RoamTrigger", buf);
        sprintf(buf, "%d", priv->param_roamdelta);
-        rndis_set_config_parameter_str(dev, "RoamDelta", buf);
+        rndis_set_config_parameter_str(usbdev, "RoamDelta", buf);
        return 0;
 }
-static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
+static int rndis_wext_bind(struct usbnet *usbdev, struct usb_interface *intf)
 {
-        struct net_device *net = dev->net;
        struct rndis_wext_private *priv;
        int retval, len;
        __le32 tmp;
        /* allocate rndis private data */
-        priv = kmalloc(sizeof(struct rndis_wext_private), GFP_KERNEL);
+        priv = kzalloc(sizeof(struct rndis_wext_private), GFP_KERNEL);
        if (!priv)
                return -ENOMEM;
        /* These have to be initialized before calling generic_rndis_bind().
         * Otherwise we'll be in big trouble in rndis_wext_early_init().
         */
-        dev->driver_priv = priv;
+        usbdev->driver_priv = priv;
-        memset(priv, 0, sizeof(*priv));
-        memset(priv->name, 0, sizeof(priv->name));
        strcpy(priv->name, "IEEE802.11");
-        net->wireless_handlers = &rndis_iw_handlers;
+        usbdev->net->wireless_handlers = &rndis_iw_handlers;
-        priv->usbdev = dev;
+        priv->usbdev = usbdev;
        mutex_init(&priv->command_lock);
        spin_lock_init(&priv->stats_lock);
        /* try bind rndis_host */
-        retval = generic_rndis_bind(dev, intf, FLAG_RNDIS_PHYM_WIRELESS);
+        retval = generic_rndis_bind(usbdev, intf, FLAG_RNDIS_PHYM_WIRELESS);
        if (retval < 0)
                goto fail;
@@ -2545,20 +2566,21 @@ static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
         * rndis_host wants to avoid all OID as much as possible
         * so do promisc/multicast handling in rndis_wext.
         */
-        dev->net->set_multicast_list = rndis_wext_set_multicast_list;
+        usbdev->net->set_multicast_list = rndis_wext_set_multicast_list;
        tmp = RNDIS_PACKET_TYPE_DIRECTED | RNDIS_PACKET_TYPE_BROADCAST;
-        retval = rndis_set_oid(dev, OID_GEN_CURRENT_PACKET_FILTER, &tmp,
+        retval = rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &tmp,
                                                                sizeof(tmp));
        len = sizeof(tmp);
-        retval = rndis_query_oid(dev, OID_802_3_MAXIMUM_LIST_SIZE, &tmp, &len);
+        retval = rndis_query_oid(usbdev, OID_802_3_MAXIMUM_LIST_SIZE, &tmp,
+                                                                &len);
        priv->multicast_size = le32_to_cpu(tmp);
        if (retval < 0 || priv->multicast_size < 0)
                priv->multicast_size = 0;
        if (priv->multicast_size > 0)
-                dev->net->flags |= IFF_MULTICAST;
+                usbdev->net->flags |= IFF_MULTICAST;
        else
-                dev->net->flags &= ~IFF_MULTICAST;
+                usbdev->net->flags &= ~IFF_MULTICAST;
        priv->iwstats.qual.qual = 0;
        priv->iwstats.qual.level = 0;
@@ -2568,12 +2590,13 @@ static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
                                        | IW_QUAL_QUAL_INVALID
                                        | IW_QUAL_LEVEL_INVALID;
-        rndis_wext_get_caps(dev);
+        rndis_wext_get_caps(usbdev);
-        set_default_iw_params(dev);
+        set_default_iw_params(usbdev);
        /* turn radio on */
        priv->radio_on = 1;
-        disassociate(dev, 1);
+        disassociate(usbdev, 1);
+        netif_carrier_off(usbdev->net);
        /* because rndis_command() sleeps we need to use workqueue */
        priv->workqueue = create_singlethread_workqueue("rndis_wlan");
@@ -2590,12 +2613,12 @@ fail:
 }
-static void rndis_wext_unbind(struct usbnet *dev, struct usb_interface *intf)
+static void rndis_wext_unbind(struct usbnet *usbdev, struct usb_interface *intf)
 {
-        struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+        struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
        /* turn radio off */
-        disassociate(dev, 0);
+        disassociate(usbdev, 0);
        cancel_delayed_work_sync(&priv->stats_work);
        cancel_work_sync(&priv->work);
@@ -2606,13 +2629,13 @@ static void rndis_wext_unbind(struct usbnet *dev, struct usb_interface *intf)
                kfree(priv->wpa_ie);
        kfree(priv);
-        rndis_unbind(dev, intf);
+        rndis_unbind(usbdev, intf);
 }
-static int rndis_wext_reset(struct usbnet *dev)
+static int rndis_wext_reset(struct usbnet *usbdev)
 {
-        return deauthenticate(dev);
+        return deauthenticate(usbdev);
 }
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 900140d3b304..bb3d83560d02 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -277,6 +277,17 @@ static int rt2400pci_blink_set(struct led_classdev *led_cdev,
        return 0;
 }
+static void rt2400pci_init_led(struct rt2x00_dev *rt2x00dev,
+                               struct rt2x00_led *led,
+                               enum led_type type)
+{
+        led->rt2x00dev = rt2x00dev;
+        led->type = type;
+        led->led_dev.brightness_set = rt2400pci_brightness_set;
+        led->led_dev.blink_set = rt2400pci_blink_set;
+        led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2400PCI_LEDS */
 /*
@@ -781,25 +792,32 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
        return 0;
 }
-static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2400pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 {
        unsigned int i;
-        u16 eeprom;
-        u8 reg_id;
        u8 value;
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt2400pci_bbp_read(rt2x00dev, 0, &value);
                if ((value != 0xff) && (value != 0x00))
-                        goto continue_csr_init;
+                        return 0;
-                NOTICE(rt2x00dev, "Waiting for BBP register.\n");
                udelay(REGISTER_BUSY_DELAY);
        }
        ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
        return -EACCES;
+}
+static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+        unsigned int i;
+        u16 eeprom;
+        u8 reg_id;
+        u8 value;
+        if (unlikely(rt2400pci_wait_bbp_ready(rt2x00dev)))
+                return -EACCES;
-continue_csr_init:
        rt2400pci_bbp_write(rt2x00dev, 1, 0x00);
        rt2400pci_bbp_write(rt2x00dev, 3, 0x27);
        rt2400pci_bbp_write(rt2x00dev, 4, 0x08);
@@ -838,7 +856,8 @@ static void rt2400pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
        rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
        rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
-                           state == STATE_RADIO_RX_OFF);
+                           (state == STATE_RADIO_RX_OFF) ||
+                           (state == STATE_RADIO_RX_OFF_LINK));
        rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 }
@@ -875,17 +894,10 @@ static int rt2400pci_enable_radio(struct rt2x00_dev *rt2x00dev)
        /*
         * Initialize all registers.
         */
-        if (rt2400pci_init_queues(rt2x00dev) ||
+        if (unlikely(rt2400pci_init_queues(rt2x00dev) ||
-            rt2400pci_init_registers(rt2x00dev) ||
+                     rt2400pci_init_registers(rt2x00dev) ||
-            rt2400pci_init_bbp(rt2x00dev)) {
+                     rt2400pci_init_bbp(rt2x00dev)))
-                ERROR(rt2x00dev, "Register initialization failed.\n");
                return -EIO;
-        }
-        /*
-         * Enable interrupts.
-         */
-        rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
        return 0;
 }
@@ -907,11 +919,6 @@ static void rt2400pci_disable_radio(struct rt2x00_dev *rt2x00dev)
        rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
        rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
        rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
-        /*
-         * Disable interrupts.
-         */
-        rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
 }
 static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
@@ -946,10 +953,6 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
                msleep(10);
        }
-        NOTICE(rt2x00dev, "Device failed to enter state %d, "
-               "current device state: bbp %d and rf %d.\n",
-               state, bbp_state, rf_state);
        return -EBUSY;
 }
@@ -967,11 +970,13 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        case STATE_RADIO_RX_ON:
        case STATE_RADIO_RX_ON_LINK:
-                rt2400pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-                break;
        case STATE_RADIO_RX_OFF:
        case STATE_RADIO_RX_OFF_LINK:
-                rt2400pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+                rt2400pci_toggle_rx(rt2x00dev, state);
+                break;
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                rt2400pci_toggle_irq(rt2x00dev, state);
                break;
        case STATE_DEEP_SLEEP:
        case STATE_SLEEP:
@@ -984,6 +989,10 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        }
+        if (unlikely(retval))
+                ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+                      state, retval);
        return retval;
 }
@@ -1007,8 +1016,8 @@ static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
        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_BUFFER_LENGTH, skb->len);
-        rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, skbdesc->data_len);
+        rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, skb->len);
        rt2x00_desc_write(txd, 2, word);
        rt2x00_desc_read(txd, 3, &word);
@@ -1300,23 +1309,10 @@ static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2400PCI_LEDS
        value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
-        rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
+        rt2400pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
-        rt2x00dev->led_radio.type = LED_TYPE_RADIO;
+        if (value == LED_MODE_TXRX_ACTIVITY)
-        rt2x00dev->led_radio.led_dev.brightness_set =
+                rt2400pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
-            rt2400pci_brightness_set;
+                                   LED_TYPE_ACTIVITY);
-        rt2x00dev->led_radio.led_dev.blink_set =
-            rt2400pci_blink_set;
-        rt2x00dev->led_radio.flags = LED_INITIALIZED;
-        if (value == LED_MODE_TXRX_ACTIVITY) {
-                rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-                rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-                rt2x00dev->led_qual.led_dev.brightness_set =
-                    rt2400pci_brightness_set;
-                rt2x00dev->led_qual.led_dev.blink_set =
-                    rt2400pci_blink_set;
-                rt2x00dev->led_qual.flags = LED_INITIALIZED;
-        }
 #endif /* CONFIG_RT2400PCI_LEDS */
        /*
@@ -1511,9 +1507,6 @@ static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
         */
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        skbdesc->data = skb->data;
-        skbdesc->data_len = skb->len;
        skbdesc->desc = entry_priv->desc;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.h b/drivers/net/wireless/rt2x00/rt2400pci.h
index e9aa326be9f6..bc5564258228 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.h
+++ b/drivers/net/wireless/rt2x00/rt2400pci.h
@@ -37,8 +37,6 @@
 * Signal information.
 * Defaul offset is required for RSSI <-> dBm conversion.
 */
-#define MAX_SIGNAL                      100
-#define MAX_RX_SSI                      -1
 #define DEFAULT_RSSI_OFFSET             100
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 673350953b89..3c956b91c4e3 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -277,6 +277,17 @@ static int rt2500pci_blink_set(struct led_classdev *led_cdev,
        return 0;
 }
+static void rt2500pci_init_led(struct rt2x00_dev *rt2x00dev,
+                               struct rt2x00_led *led,
+                               enum led_type type)
+{
+        led->rt2x00dev = rt2x00dev;
+        led->type = type;
+        led->led_dev.brightness_set = rt2500pci_brightness_set;
+        led->led_dev.blink_set = rt2500pci_blink_set;
+        led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2500PCI_LEDS */
 /*
@@ -924,25 +935,32 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
        return 0;
 }
-static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2500pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 {
        unsigned int i;
-        u16 eeprom;
-        u8 reg_id;
        u8 value;
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt2500pci_bbp_read(rt2x00dev, 0, &value);
                if ((value != 0xff) && (value != 0x00))
-                        goto continue_csr_init;
+                        return 0;
-                NOTICE(rt2x00dev, "Waiting for BBP register.\n");
                udelay(REGISTER_BUSY_DELAY);
        }
        ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
        return -EACCES;
+}
+static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+        unsigned int i;
+        u16 eeprom;
+        u8 reg_id;
+        u8 value;
+        if (unlikely(rt2500pci_wait_bbp_ready(rt2x00dev)))
+                return -EACCES;
-continue_csr_init:
        rt2500pci_bbp_write(rt2x00dev, 3, 0x02);
        rt2500pci_bbp_write(rt2x00dev, 4, 0x19);
        rt2500pci_bbp_write(rt2x00dev, 14, 0x1c);
@@ -997,7 +1015,8 @@ static void rt2500pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
        rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
        rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
-                           state == STATE_RADIO_RX_OFF);
+                           (state == STATE_RADIO_RX_OFF) ||
+                           (state == STATE_RADIO_RX_OFF_LINK));
        rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 }
@@ -1034,17 +1053,10 @@ static int rt2500pci_enable_radio(struct rt2x00_dev *rt2x00dev)
        /*
         * Initialize all registers.
         */
-        if (rt2500pci_init_queues(rt2x00dev) ||
+        if (unlikely(rt2500pci_init_queues(rt2x00dev) ||
-            rt2500pci_init_registers(rt2x00dev) ||
+                     rt2500pci_init_registers(rt2x00dev) ||
-            rt2500pci_init_bbp(rt2x00dev)) {
+                     rt2500pci_init_bbp(rt2x00dev)))
-                ERROR(rt2x00dev, "Register initialization failed.\n");
                return -EIO;
-        }
-        /*
-         * Enable interrupts.
-         */
-        rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
        return 0;
 }
@@ -1066,11 +1078,6 @@ static void rt2500pci_disable_radio(struct rt2x00_dev *rt2x00dev)
        rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
        rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
        rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
-        /*
-         * Disable interrupts.
-         */
-        rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
 }
 static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
@@ -1105,10 +1112,6 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
                msleep(10);
        }
-        NOTICE(rt2x00dev, "Device failed to enter state %d, "
-               "current device state: bbp %d and rf %d.\n",
-               state, bbp_state, rf_state);
        return -EBUSY;
 }
@@ -1126,11 +1129,13 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        case STATE_RADIO_RX_ON:
        case STATE_RADIO_RX_ON_LINK:
-                rt2500pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-                break;
        case STATE_RADIO_RX_OFF:
        case STATE_RADIO_RX_OFF_LINK:
-                rt2500pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+                rt2500pci_toggle_rx(rt2x00dev, state);
+                break;
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                rt2500pci_toggle_irq(rt2x00dev, state);
                break;
        case STATE_DEEP_SLEEP:
        case STATE_SLEEP:
@@ -1143,6 +1148,10 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        }
+        if (unlikely(retval))
+                ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+                      state, retval);
        return retval;
 }
@@ -1478,23 +1487,10 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2500PCI_LEDS
        value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
-        rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
+        rt2500pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
-        rt2x00dev->led_radio.type = LED_TYPE_RADIO;
+        if (value == LED_MODE_TXRX_ACTIVITY)
-        rt2x00dev->led_radio.led_dev.brightness_set =
+                rt2500pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
-            rt2500pci_brightness_set;
+                                   LED_TYPE_ACTIVITY);
-        rt2x00dev->led_radio.led_dev.blink_set =
-            rt2500pci_blink_set;
-        rt2x00dev->led_radio.flags = LED_INITIALIZED;
-        if (value == LED_MODE_TXRX_ACTIVITY) {
-                rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-                rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-                rt2x00dev->led_qual.led_dev.brightness_set =
-                    rt2500pci_brightness_set;
-                rt2x00dev->led_qual.led_dev.blink_set =
-                    rt2500pci_blink_set;
-                rt2x00dev->led_qual.flags = LED_INITIALIZED;
-        }
 #endif /* CONFIG_RT2500PCI_LEDS */
        /*
@@ -1827,9 +1823,6 @@ static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
         */
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        skbdesc->data = skb->data;
-        skbdesc->data_len = skb->len;
        skbdesc->desc = entry_priv->desc;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.h b/drivers/net/wireless/rt2x00/rt2500pci.h
index ea93b8f423a9..cb648c30a5b3 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.h
+++ b/drivers/net/wireless/rt2x00/rt2500pci.h
@@ -48,8 +48,6 @@
 * Signal information.
 * Defaul offset is required for RSSI <-> dBm conversion.
 */
-#define MAX_SIGNAL                      100
-#define MAX_RX_SSI                      -1
 #define DEFAULT_RSSI_OFFSET             121
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index cca1504550dc..9851cefaabf3 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -316,6 +316,17 @@ static int rt2500usb_blink_set(struct led_classdev *led_cdev,
        return 0;
 }
+static void rt2500usb_init_led(struct rt2x00_dev *rt2x00dev,
+                               struct rt2x00_led *led,
+                               enum led_type type)
+{
+        led->rt2x00dev = rt2x00dev;
+        led->type = type;
+        led->led_dev.brightness_set = rt2500usb_brightness_set;
+        led->led_dev.blink_set = rt2500usb_blink_set;
+        led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2500USB_LEDS */
 /*
@@ -847,25 +858,32 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
        return 0;
 }
-static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2500usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 {
        unsigned int i;
-        u16 eeprom;
        u8 value;
-        u8 reg_id;
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt2500usb_bbp_read(rt2x00dev, 0, &value);
                if ((value != 0xff) && (value != 0x00))
-                        goto continue_csr_init;
+                        return 0;
-                NOTICE(rt2x00dev, "Waiting for BBP register.\n");
                udelay(REGISTER_BUSY_DELAY);
        }
        ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
        return -EACCES;
+}
+static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+        unsigned int i;
+        u16 eeprom;
+        u8 value;
+        u8 reg_id;
+        if (unlikely(rt2500usb_wait_bbp_ready(rt2x00dev)))
+                return -EACCES;
-continue_csr_init:
        rt2500usb_bbp_write(rt2x00dev, 3, 0x02);
        rt2500usb_bbp_write(rt2x00dev, 4, 0x19);
        rt2500usb_bbp_write(rt2x00dev, 14, 0x1c);
@@ -921,7 +939,8 @@ static void rt2500usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
        rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
        rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX,
-                           state == STATE_RADIO_RX_OFF);
+                           (state == STATE_RADIO_RX_OFF) ||
+                           (state == STATE_RADIO_RX_OFF_LINK));
        rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
 }
@@ -930,11 +949,9 @@ static int rt2500usb_enable_radio(struct rt2x00_dev *rt2x00dev)
        /*
         * Initialize all registers.
         */
-        if (rt2500usb_init_registers(rt2x00dev) ||
+        if (unlikely(rt2500usb_init_registers(rt2x00dev) ||
-            rt2500usb_init_bbp(rt2x00dev)) {
+                     rt2500usb_init_bbp(rt2x00dev)))
-                ERROR(rt2x00dev, "Register initialization failed.\n");
                return -EIO;
-        }
        return 0;
 }
@@ -987,10 +1004,6 @@ static int rt2500usb_set_state(struct rt2x00_dev *rt2x00dev,
                msleep(30);
        }
-        NOTICE(rt2x00dev, "Device failed to enter state %d, "
-               "current device state: bbp %d and rf %d.\n",
-               state, bbp_state, rf_state);
        return -EBUSY;
 }
@@ -1008,11 +1021,13 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        case STATE_RADIO_RX_ON:
        case STATE_RADIO_RX_ON_LINK:
-                rt2500usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-                break;
        case STATE_RADIO_RX_OFF:
        case STATE_RADIO_RX_OFF_LINK:
-                rt2500usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+                rt2500usb_toggle_rx(rt2x00dev, state);
+                break;
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                /* No support, but no error either */
                break;
        case STATE_DEEP_SLEEP:
        case STATE_SLEEP:
@@ -1025,6 +1040,10 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        }
+        if (unlikely(retval))
+                ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+                      state, retval);
        return retval;
 }
@@ -1069,7 +1088,8 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
        rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
                           test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags));
        rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
-        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+        rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT,
+                           skb->len - skbdesc->desc_len);
        rt2x00_set_field32(&word, TXD_W0_CIPHER, CIPHER_NONE);
        rt2x00_desc_write(txd, 0, word);
 }
@@ -1097,8 +1117,10 @@ static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 {
        u16 reg;
-        if (queue != QID_BEACON)
+        if (queue != QID_BEACON) {
+                rt2x00usb_kick_tx_queue(rt2x00dev, queue);
                return;
+        }
        rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
        if (!rt2x00_get_field16(reg, TXRX_CSR19_BEACON_GEN)) {
@@ -1134,14 +1156,10 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
        u32 word1;
        /*
-         * Copy descriptor to the skb->cb array, this has 2 benefits:
+         * Copy descriptor to the skbdesc->desc buffer, making it safe from moving of
-         * 1) Each descriptor word is 4 byte aligned.
+         * frame data in rt2x00usb.
-         * 2) Descriptor is safe  from moving of frame data in rt2x00usb.
         */
-        skbdesc->desc_len =
+        memcpy(skbdesc->desc, rxd, 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;
        /*
@@ -1175,8 +1193,6 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
         * Adjust the skb memory window to the frame boundaries.
         */
        skb_trim(entry->skb, rxdesc->size);
-        skbdesc->data = entry->skb->data;
-        skbdesc->data_len = rxdesc->size;
 }
 /*
@@ -1377,23 +1393,10 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2500USB_LEDS
        value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
-        rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
+        rt2500usb_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
-        rt2x00dev->led_radio.type = LED_TYPE_RADIO;
+        if (value == LED_MODE_TXRX_ACTIVITY)
-        rt2x00dev->led_radio.led_dev.brightness_set =
+                rt2500usb_init_led(rt2x00dev, &rt2x00dev->led_qual,
-            rt2500usb_brightness_set;
+                                   LED_TYPE_ACTIVITY);
-        rt2x00dev->led_radio.led_dev.blink_set =
-            rt2500usb_blink_set;
-        rt2x00dev->led_radio.flags = LED_INITIALIZED;
-        if (value == LED_MODE_TXRX_ACTIVITY) {
-                rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-                rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-                rt2x00dev->led_qual.led_dev.brightness_set =
-                    rt2500usb_brightness_set;
-                rt2x00dev->led_qual.led_dev.blink_set =
-                    rt2500usb_blink_set;
-                rt2x00dev->led_qual.flags = LED_INITIALIZED;
-        }
 #endif /* CONFIG_RT2500USB_LEDS */
        /*
@@ -1703,9 +1706,6 @@ static int rt2500usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
         */
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        skbdesc->data = skb->data + intf->beacon->queue->desc_size;
-        skbdesc->data_len = skb->len - intf->beacon->queue->desc_size;
        skbdesc->desc = skb->data;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.h b/drivers/net/wireless/rt2x00/rt2500usb.h
index 7d50098f0cc5..3e21fdf2b00f 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.h
+++ b/drivers/net/wireless/rt2x00/rt2500usb.h
@@ -48,8 +48,6 @@
 * Signal information.
 * Defaul offset is required for RSSI <-> dBm conversion.
 */
-#define MAX_SIGNAL                      100
-#define MAX_RX_SSI                      -1
 #define DEFAULT_RSSI_OFFSET             120
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 15ec797c5ec1..0da8f972a1b2 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.6"
+#define DRV_VERSION     "2.1.7"
 #define DRV_PROJECT     "http://rt2x00.serialmonkey.com"
 /*
@@ -546,8 +546,7 @@ struct rt2x00lib_ops {
        void (*write_tx_desc) (struct rt2x00_dev *rt2x00dev,
                               struct sk_buff *skb,
                               struct txentry_desc *txdesc);
-        int (*write_tx_data) (struct rt2x00_dev *rt2x00dev,
+        int (*write_tx_data) (struct queue_entry *entry);
-                              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,
@@ -827,7 +826,7 @@ struct rt2x00_dev {
         * The Beacon array also contains the Atim queue
         * if that is supported by the device.
         */
-        int data_queues;
+        unsigned int data_queues;
        struct data_queue *rx;
        struct data_queue *tx;
        struct data_queue *bcn;
@@ -931,6 +930,12 @@ static inline u16 get_duration_res(const unsigned int size, const u8 rate)
 }
 /**
+ * rt2x00queue_alloc_rxskb - allocate a skb for RX purposes.
+ * @queue: The queue for which the skb will be applicable.
+ */
+struct sk_buff *rt2x00queue_alloc_rxskb(struct data_queue *queue);
+/**
 * 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.
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index bd92cb8e68e0..300cf061035f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -133,7 +133,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
                return;
        }
-        skbcopy = alloc_skb(sizeof(*dump_hdr) + desc->desc_len + desc->data_len,
+        skbcopy = alloc_skb(sizeof(*dump_hdr) + desc->desc_len + skb->len,
                            GFP_ATOMIC);
        if (!skbcopy) {
                DEBUG(rt2x00dev, "Failed to copy skb for dump.\n");
@@ -144,7 +144,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
        dump_hdr->version = cpu_to_le32(DUMP_HEADER_VERSION);
        dump_hdr->header_length = cpu_to_le32(sizeof(*dump_hdr));
        dump_hdr->desc_length = cpu_to_le32(desc->desc_len);
-        dump_hdr->data_length = cpu_to_le32(desc->data_len);
+        dump_hdr->data_length = cpu_to_le32(skb->len);
        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);
@@ -155,7 +155,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
        dump_hdr->timestamp_usec = cpu_to_le32(timestamp.tv_usec);
        memcpy(skb_put(skbcopy, desc->desc_len), desc->desc, desc->desc_len);
-        memcpy(skb_put(skbcopy, desc->data_len), desc->data, desc->data_len);
+        memcpy(skb_put(skbcopy, skb->len), skb->data, skb->len);
        skb_queue_tail(&intf->frame_dump_skbqueue, skbcopy);
        wake_up_interruptible(&intf->frame_dump_waitqueue);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index dc5ab90a52c3..9ea677320daa 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -112,6 +112,8 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
        if (status)
                return status;
+        rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_IRQ_ON);
        rt2x00leds_led_radio(rt2x00dev, true);
        rt2x00led_led_activity(rt2x00dev, true);
@@ -157,6 +159,7 @@ void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev)
         * Disable radio.
         */
        rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_OFF);
+        rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_IRQ_OFF);
        rt2x00led_led_activity(rt2x00dev, false);
        rt2x00leds_led_radio(rt2x00dev, false);
 }
@@ -551,14 +554,32 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
 {
        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
        struct ieee80211_rx_status *rx_status = &rt2x00dev->rx_status;
+        unsigned int header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
        struct ieee80211_supported_band *sband;
        struct ieee80211_hdr *hdr;
        const struct rt2x00_rate *rate;
+        unsigned int align;
        unsigned int i;
        int idx = -1;
        u16 fc;
        /*
+         * The data behind the ieee80211 header must be
+         * aligned on a 4 byte boundary.
+         */
+        align = ((unsigned long)(entry->skb->data + header_size)) & 3;
+        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 */
+        skb_trim(entry->skb, rxdesc->size);
+        /*
         * Update RX statistics.
         */
        sband = &rt2x00dev->bands[rt2x00dev->curr_band];
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index c4ce534e3cdb..558f45bf27e3 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -101,6 +101,7 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 /*
 * Queue handlers.
 */
+int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb);
 void rt2x00queue_init_rx(struct rt2x00_dev *rt2x00dev);
 void rt2x00queue_init_tx(struct rt2x00_dev *rt2x00dev);
 int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev);
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index b02dbc8a666e..c90992f613fe 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -34,7 +34,6 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
                                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;
@@ -65,6 +64,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
        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_RTS_CTS;
        rts_info->flags &= ~IEEE80211_TX_CTL_USE_CTS_PROTECT;
        rts_info->flags &= ~IEEE80211_TX_CTL_REQ_TX_STATUS;
@@ -82,14 +82,7 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
                                  frag_skb->data, size, tx_info,
                                  (struct ieee80211_rts *)(skb->data));
-        /*
+        if (rt2x00queue_write_tx_frame(queue, skb)) {
-         * Initialize skb descriptor
-         */
-        skbdesc = get_skb_frame_desc(skb);
-        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb)) {
                WARNING(rt2x00dev, "Failed to send RTS/CTS frame.\n");
                return NETDEV_TX_BUSY;
        }
@@ -135,18 +128,16 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
        }
        /*
-         * If CTS/RTS is required. and this frame is not CTS or RTS,
+         * If CTS/RTS is required. create and queue that frame first.
-         * create and queue that frame first. But make sure we have
+         * Make sure we have at least enough entries available to send
-         * at least enough entries available to send this CTS/RTS
+         * this CTS/RTS frame as well as the data frame.
-         * frame as well as the data frame.
         * Note that when the driver has set the set_rts_threshold()
         * callback function it doesn't need software generation of
-         * neither RTS or CTS-to-self frames and handles everything
+         * either RTS or CTS-to-self frame and handles everything
         * inside the hardware.
         */
        frame_control = le16_to_cpu(ieee80211hdr->frame_control);
-        if (!is_rts_frame(frame_control) && !is_cts_frame(frame_control) &&
+        if ((tx_info->flags & (IEEE80211_TX_CTL_USE_RTS_CTS |
-            (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) {
@@ -160,17 +151,14 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
                }
        }
-        if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb)) {
+        if (rt2x00queue_write_tx_frame(queue, skb)) {
                ieee80211_stop_queue(rt2x00dev->hw, qid);
                return NETDEV_TX_BUSY;
        }
-        if (rt2x00queue_full(queue))
+        if (rt2x00queue_threshold(queue))
                ieee80211_stop_queue(rt2x00dev->hw, qid);
-        if (rt2x00dev->ops->lib->kick_tx_queue)
-                rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, qid);
        return NETDEV_TX_OK;
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_tx);
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 70a3d135f64e..82e80b69d0be 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -34,52 +34,38 @@
 /*
 * TX data handlers.
 */
-int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
+int rt2x00pci_write_tx_data(struct queue_entry *entry)
-                            struct data_queue *queue, struct sk_buff *skb)
 {
-        struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
        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(entry_priv->desc, 0, &word);
-        if (rt2x00_get_field32(word, TXD_ENTRY_OWNER_NIC) ||
+        /*
-            rt2x00_get_field32(word, TXD_ENTRY_VALID)) {
+         * This should not happen, we already checked the entry
-                ERROR(rt2x00dev,
+         * was ours. When the hardware disagrees there has been
-                      "Arrived at non-free entry in the non-full queue %d.\n"
+         * a queue corruption!
+         */
+        if (unlikely(rt2x00_get_field32(word, TXD_ENTRY_OWNER_NIC) ||
+                     rt2x00_get_field32(word, TXD_ENTRY_VALID))) {
+                ERROR(entry->queue->rt2x00dev,
+                      "Corrupt queue %d, accessing entry which is not ours.\n"
                      "Please file bug report to %s.\n",
                      entry->queue->qid, DRV_PROJECT);
                return -EINVAL;
        }
        /*
-         * 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);
+        skbdesc = get_skb_frame_desc(entry->skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->data = skb->data;
-        skbdesc->data_len = skb->len;
        skbdesc->desc = entry_priv->desc;
-        skbdesc->desc_len = queue->desc_size;
+        skbdesc->desc_len = entry->queue->desc_size;
        skbdesc->entry = entry;
-        memcpy(entry_priv->data, skb->data, skb->len);
+        memcpy(entry_priv->data, entry->skb->data, entry->skb->len);
-        rt2x00queue_write_tx_descriptor(entry, &txdesc);
-        rt2x00queue_index_inc(queue, Q_INDEX);
        return 0;
 }
@@ -93,11 +79,8 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
        struct data_queue *queue = rt2x00dev->rx;
        struct queue_entry *entry;
        struct queue_entry_priv_pci *entry_priv;
-        struct ieee80211_hdr *hdr;
        struct skb_frame_desc *skbdesc;
        struct rxdone_entry_desc rxdesc;
-        int header_size;
-        int align;
        u32 word;
        while (1) {
@@ -111,35 +94,21 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
                memset(&rxdesc, 0, sizeof(rxdesc));
                rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
-                hdr = (struct ieee80211_hdr *)entry_priv->data;
-                header_size =
-                    ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
-                /*
-                 * The data behind the ieee80211 header must be
-                 * aligned on a 4 byte boundary.
-                 */
-                align = header_size % 4;
                /*
-                 * Allocate the sk_buffer, initialize it and copy
+                 * Allocate the sk_buffer and copy all data into it.
-                 * all data into it.
                 */
-                entry->skb = dev_alloc_skb(rxdesc.size + align);
+                entry->skb = rt2x00queue_alloc_rxskb(queue);
                if (!entry->skb)
                        return;
-                skb_reserve(entry->skb, align);
+                memcpy(entry->skb->data, entry_priv->data, rxdesc.size);
-                memcpy(skb_put(entry->skb, rxdesc.size),
+                skb_trim(entry->skb, rxdesc.size);
-                       entry_priv->data, rxdesc.size);
                /*
                 * Fill in skb descriptor
                 */
                skbdesc = get_skb_frame_desc(entry->skb);
                memset(skbdesc, 0, sizeof(*skbdesc));
-                skbdesc->data = entry->skb->data;
-                skbdesc->data_len = entry->skb->len;
                skbdesc->desc = entry_priv->desc;
                skbdesc->desc_len = queue->desc_size;
                skbdesc->entry = entry;
@@ -178,14 +147,15 @@ void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
        rt2x00_set_field32(&word, TXD_ENTRY_VALID, 0);
        rt2x00_desc_write(entry_priv->desc, 0, word);
+        __clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
        rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
        /*
-         * If the data queue was full before the txdone handler
+         * If the data queue was below the threshold before the txdone
-         * we must make sure the packet queue in the mac80211 stack
+         * handler we must make sure the packet queue in the mac80211 stack
         * is reenabled when the txdone handler has finished.
         */
-        if (!rt2x00queue_full(entry->queue))
+        if (!rt2x00queue_threshold(entry->queue))
                ieee80211_wake_queue(rt2x00dev->hw, qid);
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
index 37c851e442c1..87c4a0cd78db 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -87,11 +87,14 @@ rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
        memcpy_toio(rt2x00dev->csr.base + offset, value, length);
 }
-/*
+/**
- * TX data handlers.
+ * rt2x00pci_write_tx_data - Initialize data for TX operation
+ * @entry: The entry where the frame is located
+ *
+ * This function will initialize the DMA and skb descriptor
+ * to prepare the entry for the actual TX operation.
 */
-int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
+int rt2x00pci_write_tx_data(struct queue_entry *entry);
-                            struct data_queue *queue, struct sk_buff *skb);
 /**
 * struct queue_entry_priv_pci: Per entry PCI specific information
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index e69ef4b19239..7b52039b01a6 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -29,6 +29,45 @@
 #include "rt2x00.h"
 #include "rt2x00lib.h"
+struct sk_buff *rt2x00queue_alloc_rxskb(struct data_queue *queue)
+{
+        struct sk_buff *skb;
+        unsigned int frame_size;
+        unsigned int reserved_size;
+        /*
+         * The frame size includes descriptor size, because the
+         * hardware directly receive the frame into the skbuffer.
+         */
+        frame_size = queue->data_size + queue->desc_size;
+        /*
+         * 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, reserved_size);
+        skb_put(skb, frame_size);
+        return skb;
+}
+EXPORT_SYMBOL_GPL(rt2x00queue_alloc_rxskb);
 void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
                                      struct txentry_desc *txdesc)
 {
@@ -91,7 +130,7 @@ void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
        /*
         * Check if more fragments are pending
         */
-        if (ieee80211_get_morefrag(hdr)) {
+        if (ieee80211_has_morefrags(hdr->frame_control)) {
                __set_bit(ENTRY_TXD_BURST, &txdesc->flags);
                __set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
        }
@@ -163,8 +202,8 @@ 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 data_queue *queue = entry->queue;
-        struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+        struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
        rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, entry->skb, txdesc);
@@ -175,19 +214,61 @@ void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
        rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TX, entry->skb);
        /*
-         * We are done writing the frame to the queue entry,
+         * Check if we need to kick the queue, there are however a few rules
-         * also kick the queue in case the correct flags are set,
+         *      1) Don't kick beacon queue
-         * note that this will automatically filter beacons and
+         *      2) Don't kick unless this is the last in frame in a burst.
-         * RTS/CTS frames since those frames don't have this flag
+         *         When the burst flag is set, this frame is always followed
-         * set.
+         *         by another frame which in some way are related to eachother.
+         *         This is true for fragments, RTS or CTS-to-self frames.
+         *      3) Rule 2 can be broken when the available entries
+         *         in the queue are less then a certain threshold.
         */
-        if (rt2x00dev->ops->lib->kick_tx_queue &&
+        if (entry->queue->qid == QID_BEACON)
-            !(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
+                return;
-                rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev,
-                                                   entry->queue->qid);
+        if (rt2x00queue_threshold(queue) ||
+            !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
+                rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, queue->qid);
 }
 EXPORT_SYMBOL_GPL(rt2x00queue_write_tx_descriptor);
+int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
+{
+        struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
+        struct txentry_desc txdesc;
+        if (unlikely(rt2x00queue_full(queue)))
+                return -EINVAL;
+        if (__test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
+                ERROR(queue->rt2x00dev,
+                      "Arrived at non-free entry in the non-full queue %d.\n"
+                      "Please file bug report to %s.\n",
+                      queue->qid, DRV_PROJECT);
+                return -EINVAL;
+        }
+        /*
+         * 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);
+        if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
+                __clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+                return -EIO;
+        }
+        __set_bit(ENTRY_DATA_PENDING, &entry->flags);
+        rt2x00queue_index_inc(queue, Q_INDEX);
+        rt2x00queue_write_tx_descriptor(entry, &txdesc);
+        return 0;
+}
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
                                         const enum data_queue_qid queue)
 {
@@ -312,6 +393,7 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
        rt2x00queue_reset(queue);
        queue->limit = qdesc->entry_num;
+        queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
        queue->data_size = qdesc->data_size;
        queue->desc_size = qdesc->desc_size;
@@ -439,7 +521,8 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
         * TX: qid = QID_AC_BE + index
         * TX: cw_min: 2^5 = 32.
         * TX: cw_max: 2^10 = 1024.
-         * BCN & Atim: qid = QID_MGMT
+         * BCN: qid = QID_BEACON
+         * ATIM: qid = QID_ATIM
         */
        rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
@@ -447,9 +530,9 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
        tx_queue_for_each(rt2x00dev, queue)
                rt2x00queue_init(rt2x00dev, queue, qid++);
-        rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_MGMT);
+        rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
        if (req_atim)
-                rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_MGMT);
+                rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
        return 0;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 4d00ced14cc7..fcf52520b016 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -82,12 +82,10 @@ enum data_queue_qid {
 /**
 * enum skb_frame_desc_flags: Flags for &struct skb_frame_desc
 *
- * @FRAME_DESC_DRIVER_GENERATED: Frame was generated inside driver
- *      and should not be reported back to mac80211 during txdone.
 */
-enum skb_frame_desc_flags {
+//enum skb_frame_desc_flags {
-        FRAME_DESC_DRIVER_GENERATED = 1 << 0,
+//      TEMPORARILY EMPTY
-};
+//};
 /**
 * struct skb_frame_desc: Descriptor information for the skb buffer
@@ -107,11 +105,8 @@ enum skb_frame_desc_flags {
 struct skb_frame_desc {
        unsigned int flags;
-        unsigned short data_len;
-        unsigned short desc_len;
-        void *data;
        void *desc;
+        unsigned int desc_len;
        struct queue_entry *entry;
 };
@@ -260,11 +255,14 @@ struct txentry_desc {
 * @ENTRY_OWNER_DEVICE_CRYPTO: This entry is owned by the device for data
 *      encryption or decryption. The entry should only be touched after
 *      the device has signaled it is done with it.
+ * @ENTRY_DATA_PENDING: This entry contains a valid frame and is waiting
+ *      for the signal to start sending.
 */
 enum queue_entry_flags {
        ENTRY_BCN_ASSIGNED,
        ENTRY_OWNER_DEVICE_DATA,
        ENTRY_OWNER_DEVICE_CRYPTO,
+        ENTRY_DATA_PENDING,
 };
 /**
@@ -322,6 +320,7 @@ enum queue_index {
 *      index corruption due to concurrency.
 * @count: Number of frames handled in the queue.
 * @limit: Maximum number of entries in the queue.
+ * @threshold: Minimum number of free entries before queue is kicked by force.
 * @length: Number of frames in queue.
 * @index: Index pointers to entry positions in the queue,
 *      use &enum queue_index to get a specific index field.
@@ -340,6 +339,7 @@ struct data_queue {
        spinlock_t lock;
        unsigned int count;
        unsigned short limit;
+        unsigned short threshold;
        unsigned short length;
        unsigned short index[Q_INDEX_MAX];
@@ -464,6 +464,15 @@ static inline int rt2x00queue_available(struct data_queue *queue)
 }
 /**
+ * rt2x00queue_threshold - Check if the queue is below threshold
+ * @queue: Queue to check.
+ */
+static inline int rt2x00queue_threshold(struct data_queue *queue)
+{
+        return rt2x00queue_available(queue) < queue->threshold;
+}
+/**
 * rt2x00_desc_read - Read a word from the hardware descriptor.
 * @desc: Base descriptor address
 * @word: Word index from where the descriptor should be read.
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
index 3f255df58b78..7e88ce5651b9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -130,83 +130,107 @@ struct rt2x00_field32 {
 /*
 * Power of two check, this will check
- * if the mask that has been given contains
+ * if the mask that has been given contains and contiguous set of bits.
- * and contiguous set of bits.
+ * Note that we cannot use the is_power_of_2() function since this
+ * check must be done at compile-time.
 */
 #define is_power_of_two(x)      ( !((x) & ((x)-1)) )
 #define low_bit_mask(x)         ( ((x)-1) & ~(x) )
 #define is_valid_mask(x)        is_power_of_two(1 + (x) + low_bit_mask(x))
+/*
+ * Macro's to find first set bit in a variable.
+ * These macro's behaves the same as the __ffs() function with
+ * the most important difference that this is done during
+ * compile-time rather then run-time.
+ */
+#define compile_ffs2(__x) \
+        __builtin_choose_expr(((__x) & 0x1), 0, 1)
+#define compile_ffs4(__x) \
+        __builtin_choose_expr(((__x) & 0x3), \
+                              (compile_ffs2((__x))), \
+                              (compile_ffs2((__x) >> 2) + 2))
+#define compile_ffs8(__x) \
+        __builtin_choose_expr(((__x) & 0xf), \
+                              (compile_ffs4((__x))), \
+                              (compile_ffs4((__x) >> 4) + 4))
+#define compile_ffs16(__x) \
+        __builtin_choose_expr(((__x) & 0xff), \
+                              (compile_ffs8((__x))), \
+                              (compile_ffs8((__x) >> 8) + 8))
+#define compile_ffs32(__x) \
+        __builtin_choose_expr(((__x) & 0xffff), \
+                              (compile_ffs16((__x))), \
+                              (compile_ffs16((__x) >> 16) + 16))
+/*
+ * This macro will check the requirements for the FIELD{8,16,32} macros
+ * The mask should be a constant non-zero contiguous set of bits which
+ * does not exceed the given typelimit.
+ */
+#define FIELD_CHECK(__mask, __type)                     \
+        BUILD_BUG_ON(!__builtin_constant_p(__mask) ||   \
+                     !(__mask) ||                       \
+                     !is_valid_mask(__mask) ||          \
+                     (__mask) != (__type)(__mask))      \
 #define FIELD8(__mask)                          \
 ({                                              \
-        BUILD_BUG_ON(!(__mask) ||               \
+        FIELD_CHECK(__mask, u8);                \
-                     !is_valid_mask(__mask) ||  \
-                     (__mask) != (u8)(__mask)); \
        (struct rt2x00_field8) {                \
-                __ffs(__mask), (__mask)         \
+                compile_ffs8(__mask), (__mask)  \
        };                                      \
 })
 #define FIELD16(__mask)                         \
 ({                                              \
-        BUILD_BUG_ON(!(__mask) ||               \
+        FIELD_CHECK(__mask, u16);               \
-                     !is_valid_mask(__mask) ||  \
-                     (__mask) != (u16)(__mask));\
        (struct rt2x00_field16) {               \
-                __ffs(__mask), (__mask)         \
+                compile_ffs16(__mask), (__mask) \
        };                                      \
 })
 #define FIELD32(__mask)                         \
 ({                                              \
-        BUILD_BUG_ON(!(__mask) ||               \
+        FIELD_CHECK(__mask, u32);               \
-                     !is_valid_mask(__mask) ||  \
-                     (__mask) != (u32)(__mask));\
        (struct rt2x00_field32) {               \
-                __ffs(__mask), (__mask)         \
+                compile_ffs32(__mask), (__mask) \
        };                                      \
 })
-static inline void rt2x00_set_field32(u32 *reg,
+#define SET_FIELD(__reg, __type, __field, __value)\
-                                      const struct rt2x00_field32 field,
+({                                              \
-                                      const u32 value)
+        typecheck(__type, __field);             \
-{
+        *(__reg) &= ~((__field).bit_mask);      \
-        *reg &= ~(field.bit_mask);
+        *(__reg) |= ((__value) <<               \
-        *reg |= (value << field.bit_offset) & field.bit_mask;
+            ((__field).bit_offset)) &           \
-}
+            ((__field).bit_mask);               \
+})
-static inline u32 rt2x00_get_field32(const u32 reg,
-                                     const struct rt2x00_field32 field)
+#define GET_FIELD(__reg, __type, __field)       \
-{
+({                                              \
-        return (reg & field.bit_mask) >> field.bit_offset;
+        typecheck(__type, __field);             \
-}
+        ((__reg) & ((__field).bit_mask)) >>     \
+            ((__field).bit_offset);             \
-static inline void rt2x00_set_field16(u16 *reg,
+})
-                                      const struct rt2x00_field16 field,
-                                      const u16 value)
+#define rt2x00_set_field32(__reg, __field, __value) \
-{
+        SET_FIELD(__reg, struct rt2x00_field32, __field, __value)
-        *reg &= ~(field.bit_mask);
+#define rt2x00_get_field32(__reg, __field) \
-        *reg |= (value << field.bit_offset) & field.bit_mask;
+        GET_FIELD(__reg, struct rt2x00_field32, __field)
-}
+#define rt2x00_set_field16(__reg, __field, __value) \
-static inline u16 rt2x00_get_field16(const u16 reg,
+        SET_FIELD(__reg, struct rt2x00_field16, __field, __value)
-                                     const struct rt2x00_field16 field)
+#define rt2x00_get_field16(__reg, __field) \
-{
+        GET_FIELD(__reg, struct rt2x00_field16, __field)
-        return (reg & field.bit_mask) >> field.bit_offset;
-}
+#define rt2x00_set_field8(__reg, __field, __value) \
+        SET_FIELD(__reg, struct rt2x00_field8, __field, __value)
-static inline void rt2x00_set_field8(u8 *reg,
+#define rt2x00_get_field8(__reg, __field) \
-                                     const struct rt2x00_field8 field,
+        GET_FIELD(__reg, struct rt2x00_field8, __field)
-                                     const u8 value)
-{
-        *reg &= ~(field.bit_mask);
-        *reg |= (value << field.bit_offset) & field.bit_mask;
-}
-static inline u8 rt2x00_get_field8(const u8 reg,
-                                   const struct rt2x00_field8 field)
-{
-        return (reg & field.bit_mask) >> field.bit_offset;
-}
 #endif /* RT2X00REG_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 52d12fdc0ccf..68d87f09e054 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -130,16 +130,12 @@ 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 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) ||
            !__test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
                return;
-        rt2x00_desc_read(txd, 0, &word);
        /*
         * Remove the descriptor data from the buffer.
         */
@@ -169,124 +165,101 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
        rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
        /*
-         * If the data queue was full before the txdone handler
+         * If the data queue was below the threshold before the txdone
-         * we must make sure the packet queue in the mac80211 stack
+         * handler we must make sure the packet queue in the mac80211 stack
         * is reenabled when the txdone handler has finished.
         */
-        if (!rt2x00queue_full(entry->queue))
+        if (!rt2x00queue_threshold(entry->queue))
                ieee80211_wake_queue(rt2x00dev->hw, qid);
 }
-int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
+int rt2x00usb_write_tx_data(struct queue_entry *entry)
-                            struct data_queue *queue, struct sk_buff *skb)
 {
+        struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
        struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
-        struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
        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))
-                return -EINVAL;
-        if (test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
-                ERROR(rt2x00dev,
-                      "Arrived at non-free entry in the non-full queue %d.\n"
-                      "Please file bug report to %s.\n",
-                      entry->queue->qid, DRV_PROJECT);
-                return -EINVAL;
-        }
-        /*
-         * 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);
+        skb_push(entry->skb, entry->queue->desc_size);
-        memset(skb->data, 0, queue->desc_size);
+        memset(entry->skb->data, 0, entry->queue->desc_size);
        /*
         * Fill in skb descriptor
         */
-        skbdesc = get_skb_frame_desc(skb);
+        skbdesc = get_skb_frame_desc(entry->skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->data = skb->data + queue->desc_size;
+        skbdesc->desc = entry->skb->data;
-        skbdesc->data_len = skb->len - queue->desc_size;
+        skbdesc->desc_len = entry->queue->desc_size;
-        skbdesc->desc = skb->data;
-        skbdesc->desc_len = queue->desc_size;
        skbdesc->entry = entry;
-        rt2x00queue_write_tx_descriptor(entry, &txdesc);
        /*
         * USB devices cannot blindly pass the skb->len as the
         * length of the data to usb_fill_bulk_urb. Pass the skb
         * to the driver to determine what the length should be.
         */
-        length = rt2x00dev->ops->lib->get_tx_data_len(rt2x00dev, skb);
+        length = rt2x00dev->ops->lib->get_tx_data_len(rt2x00dev, entry->skb);
-        /*
+        usb_fill_bulk_urb(entry_priv->urb, usb_dev,
-         * Initialize URB and send the frame to the device.
+                          usb_sndbulkpipe(usb_dev, 1),
-         */
+                          entry->skb->data, length,
-        __set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+                          rt2x00usb_interrupt_txdone, entry);
-        usb_fill_bulk_urb(entry_priv->urb, usb_dev, usb_sndbulkpipe(usb_dev, 1),
-                          skb->data, length, rt2x00usb_interrupt_txdone, entry);
-        usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
-        rt2x00queue_index_inc(queue, Q_INDEX);
        return 0;
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_write_tx_data);
-/*
+static inline void rt2x00usb_kick_tx_entry(struct queue_entry *entry)
- * RX data handlers.
- */
-static struct sk_buff* rt2x00usb_alloc_rxskb(struct data_queue *queue)
 {
-        struct sk_buff *skb;
+        struct queue_entry_priv_usb *entry_priv = entry->priv_data;
-        unsigned int frame_size;
-        unsigned int reserved_size;
-        /*
+        if (__test_and_clear_bit(ENTRY_DATA_PENDING, &entry->flags))
-         * The frame size includes descriptor size, because the
+                usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
-         * hardware directly receive the frame into the skbuffer.
+}
-         */
-        frame_size = queue->data_size + queue->desc_size;
+void rt2x00usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+                             const enum data_queue_qid qid)
+{
+        struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, qid);
+        unsigned long irqflags;
+        unsigned int index;
+        unsigned int index_done;
+        unsigned int i;
        /*
-         * For the allocation we should keep a few things in mind:
+         * Only protect the range we are going to loop over,
-         * 1) 4byte alignment of 802.11 payload
+         * if during our loop a extra entry is set to pending
-         *
+         * it should not be kicked during this run, since it
-         * For (1) we need at most 4 bytes to guarentee the correct
+         * is part of another TX operation.
-         * 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;
+        spin_lock_irqsave(&queue->lock, irqflags);
+        index = queue->index[Q_INDEX];
+        index_done = queue->index[Q_INDEX_DONE];
+        spin_unlock_irqrestore(&queue->lock, irqflags);
        /*
-         * Allocate skbuffer.
+         * Start from the TX done pointer, this guarentees that we will
+         * send out all frames in the correct order.
         */
-        skb = dev_alloc_skb(frame_size + reserved_size);
+        if (index_done < index) {
-        if (!skb)
+                for (i = index_done; i < index; i++)
-                return NULL;
+                        rt2x00usb_kick_tx_entry(&queue->entries[i]);
+        } else {
-        skb_reserve(skb, reserved_size);
+                for (i = index_done; i < queue->limit; i++)
-        skb_put(skb, frame_size);
+                        rt2x00usb_kick_tx_entry(&queue->entries[i]);
-        return skb;
+                for (i = 0; i < index; i++)
+                        rt2x00usb_kick_tx_entry(&queue->entries[i]);
+        }
 }
+EXPORT_SYMBOL_GPL(rt2x00usb_kick_tx_queue);
+/*
+ * RX data handlers.
+ */
 static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 {
        struct queue_entry *entry = (struct queue_entry *)urb->context;
@@ -294,8 +267,7 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
        struct sk_buff *skb;
        struct skb_frame_desc *skbdesc;
        struct rxdone_entry_desc rxdesc;
-        unsigned int header_size;
+        u8 rxd[32];
-        unsigned int align;
        if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
            !test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
@@ -315,39 +287,18 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
        skbdesc = get_skb_frame_desc(entry->skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
        skbdesc->entry = entry;
+        skbdesc->desc = rxd;
+        skbdesc->desc_len = entry->queue->desc_size;
        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. 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.
-         */
-        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
         * so we can recycle the existing sk buffer for the new frame.
         */
-        skb = rt2x00usb_alloc_rxskb(entry->queue);
+        skb = rt2x00queue_alloc_rxskb(entry->queue);
        if (!skb)
                goto skip_entry;
@@ -519,7 +470,7 @@ int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev)
         */
        entry_size = rt2x00dev->rx->data_size + rt2x00dev->rx->desc_size;
        for (i = 0; i < rt2x00dev->rx->limit; i++) {
-                skb = rt2x00usb_alloc_rxskb(rt2x00dev->rx);
+                skb = rt2x00queue_alloc_rxskb(rt2x00dev->rx);
                if (!skb)
                        goto exit;
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
index 26f53f868af6..b1187c812e7f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -212,11 +212,14 @@ static inline int rt2x00usb_eeprom_read(struct rt2x00_dev *rt2x00dev,
 */
 void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev);
-/*
+/**
- * TX data handlers.
+ * rt2x00usb_write_tx_data - Initialize URB for TX operation
+ * @entry: The entry where the frame is located
+ *
+ * This function will initialize the URB and skb descriptor
+ * to prepare the entry for the actual TX operation.
 */
-int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
+int rt2x00usb_write_tx_data(struct queue_entry *entry);
-                            struct data_queue *queue, struct sk_buff *skb);
 /**
 * struct queue_entry_priv_usb: Per entry USB specific information
@@ -245,6 +248,17 @@ struct queue_entry_priv_usb_bcn {
        struct urb *guardian_urb;
 };
+/**
+ * rt2x00usb_kick_tx_queue - Kick data queue
+ * @rt2x00dev: Pointer to &struct rt2x00_dev
+ * @qid: Data queue to kick
+ *
+ * This will walk through all entries of the queue and push all pending
+ * frames to the hardware as a single burst.
+ */
+void rt2x00usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+                             const enum data_queue_qid qid);
 /*
 * Device initialization handlers.
 */
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index e13ed5ced26e..5b7267ece1b9 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -330,6 +330,17 @@ static int rt61pci_blink_set(struct led_classdev *led_cdev,
        return 0;
 }
+static void rt61pci_init_led(struct rt2x00_dev *rt2x00dev,
+                             struct rt2x00_led *led,
+                             enum led_type type)
+{
+        led->rt2x00dev = rt2x00dev;
+        led->type = type;
+        led->led_dev.brightness_set = rt61pci_brightness_set;
+        led->led_dev.blink_set = rt61pci_blink_set;
+        led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT61PCI_LEDS */
 /*
@@ -1270,25 +1281,32 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
        return 0;
 }
-static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt61pci_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 {
        unsigned int i;
-        u16 eeprom;
-        u8 reg_id;
        u8 value;
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt61pci_bbp_read(rt2x00dev, 0, &value);
                if ((value != 0xff) && (value != 0x00))
-                        goto continue_csr_init;
+                        return 0;
-                NOTICE(rt2x00dev, "Waiting for BBP register.\n");
                udelay(REGISTER_BUSY_DELAY);
        }
        ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
        return -EACCES;
+}
+static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+        unsigned int i;
+        u16 eeprom;
+        u8 reg_id;
+        u8 value;
+        if (unlikely(rt61pci_wait_bbp_ready(rt2x00dev)))
+                return -EACCES;
-continue_csr_init:
        rt61pci_bbp_write(rt2x00dev, 3, 0x00);
        rt61pci_bbp_write(rt2x00dev, 15, 0x30);
        rt61pci_bbp_write(rt2x00dev, 21, 0xc8);
@@ -1337,7 +1355,8 @@ static void rt61pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
        rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
        rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
-                           state == STATE_RADIO_RX_OFF);
+                           (state == STATE_RADIO_RX_OFF) ||
+                           (state == STATE_RADIO_RX_OFF_LINK));
        rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
 }
@@ -1389,17 +1408,10 @@ static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
        /*
         * Initialize all registers.
         */
-        if (rt61pci_init_queues(rt2x00dev) ||
+        if (unlikely(rt61pci_init_queues(rt2x00dev) ||
-            rt61pci_init_registers(rt2x00dev) ||
+                     rt61pci_init_registers(rt2x00dev) ||
-            rt61pci_init_bbp(rt2x00dev)) {
+                     rt61pci_init_bbp(rt2x00dev)))
-                ERROR(rt2x00dev, "Register initialization failed.\n");
                return -EIO;
-        }
-        /*
-         * Enable interrupts.
-         */
-        rt61pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
        /*
         * Enable RX.
@@ -1431,11 +1443,6 @@ static void rt61pci_disable_radio(struct rt2x00_dev *rt2x00dev)
        rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC2, 1);
        rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC3, 1);
        rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
-        /*
-         * Disable interrupts.
-         */
-        rt61pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
 }
 static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
@@ -1443,7 +1450,6 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
        u32 reg;
        unsigned int i;
        char put_to_sleep;
-        char current_state;
        put_to_sleep = (state != STATE_AWAKE);
@@ -1459,16 +1465,12 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
         */
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt2x00pci_register_read(rt2x00dev, MAC_CSR12, &reg);
-                current_state =
+                state = rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
-                    rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+                if (state == !put_to_sleep)
-                if (current_state == !put_to_sleep)
                        return 0;
                msleep(10);
        }
-        NOTICE(rt2x00dev, "Device failed to enter state %d, "
-               "current device state %d.\n", !put_to_sleep, current_state);
        return -EBUSY;
 }
@@ -1486,11 +1488,13 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        case STATE_RADIO_RX_ON:
        case STATE_RADIO_RX_ON_LINK:
-                rt61pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-                break;
        case STATE_RADIO_RX_OFF:
        case STATE_RADIO_RX_OFF_LINK:
-                rt61pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+                rt61pci_toggle_rx(rt2x00dev, state);
+                break;
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                rt61pci_toggle_irq(rt2x00dev, state);
                break;
        case STATE_DEEP_SLEEP:
        case STATE_SLEEP:
@@ -1503,6 +1507,10 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        }
+        if (unlikely(retval))
+                ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+                      state, retval);
        return retval;
 }
@@ -1554,7 +1562,7 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
        if (skbdesc->desc_len > TXINFO_SIZE) {
                rt2x00_desc_read(txd, 11, &word);
-                rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skbdesc->data_len);
+                rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skb->len);
                rt2x00_desc_write(txd, 11, word);
        }
@@ -1573,7 +1581,7 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
        rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
                           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_DATABYTE_COUNT, skb->len);
        rt2x00_set_field32(&word, TXD_W0_BURST,
                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
        rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
@@ -2067,31 +2075,11 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
        rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
        value = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
-        rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
+        rt61pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
-        rt2x00dev->led_radio.type = LED_TYPE_RADIO;
+        rt61pci_init_led(rt2x00dev, &rt2x00dev->led_assoc, LED_TYPE_ASSOC);
-        rt2x00dev->led_radio.led_dev.brightness_set =
+        if (value == LED_MODE_SIGNAL_STRENGTH)
-            rt61pci_brightness_set;
+                rt61pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
-        rt2x00dev->led_radio.led_dev.blink_set =
+                                 LED_TYPE_QUALITY);
-            rt61pci_blink_set;
-        rt2x00dev->led_radio.flags = LED_INITIALIZED;
-        rt2x00dev->led_assoc.rt2x00dev = rt2x00dev;
-        rt2x00dev->led_assoc.type = LED_TYPE_ASSOC;
-        rt2x00dev->led_assoc.led_dev.brightness_set =
-            rt61pci_brightness_set;
-        rt2x00dev->led_assoc.led_dev.blink_set =
-            rt61pci_blink_set;
-        rt2x00dev->led_assoc.flags = LED_INITIALIZED;
-        if (value == LED_MODE_SIGNAL_STRENGTH) {
-                rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-                rt2x00dev->led_qual.type = LED_TYPE_QUALITY;
-                rt2x00dev->led_qual.led_dev.brightness_set =
-                    rt61pci_brightness_set;
-                rt2x00dev->led_qual.led_dev.blink_set =
-                    rt61pci_blink_set;
-                rt2x00dev->led_qual.flags = LED_INITIALIZED;
-        }
        rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
        rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
@@ -2387,9 +2375,6 @@ static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
         */
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        skbdesc->data = skb->data;
-        skbdesc->data_len = skb->len;
        skbdesc->desc = entry_priv->desc;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
@@ -2414,7 +2399,7 @@ static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
                                      skbdesc->desc, skbdesc->desc_len);
        rt2x00pci_register_multiwrite(rt2x00dev,
                                      beacon_base + skbdesc->desc_len,
-                                      skbdesc->data, skbdesc->data_len);
+                                      skb->data, skb->len);
        rt61pci_kick_tx_queue(rt2x00dev, QID_BEACON);
        return 0;
diff --git a/drivers/net/wireless/rt2x00/rt61pci.h b/drivers/net/wireless/rt2x00/rt61pci.h
index c5a04b9329d2..1004d5b899e6 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.h
+++ b/drivers/net/wireless/rt2x00/rt61pci.h
@@ -39,8 +39,6 @@
 * Signal information.
 * Defaul offset is required for RSSI <-> dBm conversion.
 */
-#define MAX_SIGNAL                      100
-#define MAX_RX_SSI                      -1
 #define DEFAULT_RSSI_OFFSET             120
 /*
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 26c2e0a1a308..505a9f5e09e9 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -335,6 +335,17 @@ static int rt73usb_blink_set(struct led_classdev *led_cdev,
        return 0;
 }
+static void rt73usb_init_led(struct rt2x00_dev *rt2x00dev,
+                             struct rt2x00_led *led,
+                             enum led_type type)
+{
+        led->rt2x00dev = rt2x00dev;
+        led->type = type;
+        led->led_dev.brightness_set = rt73usb_brightness_set;
+        led->led_dev.blink_set = rt73usb_blink_set;
+        led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT73USB_LEDS */
 /*
@@ -1084,25 +1095,32 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
        return 0;
 }
-static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt73usb_wait_bbp_ready(struct rt2x00_dev *rt2x00dev)
 {
        unsigned int i;
-        u16 eeprom;
-        u8 reg_id;
        u8 value;
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt73usb_bbp_read(rt2x00dev, 0, &value);
                if ((value != 0xff) && (value != 0x00))
-                        goto continue_csr_init;
+                        return 0;
-                NOTICE(rt2x00dev, "Waiting for BBP register.\n");
                udelay(REGISTER_BUSY_DELAY);
        }
        ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
        return -EACCES;
+}
+static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+        unsigned int i;
+        u16 eeprom;
+        u8 reg_id;
+        u8 value;
+        if (unlikely(rt73usb_wait_bbp_ready(rt2x00dev)))
+                return -EACCES;
-continue_csr_init:
        rt73usb_bbp_write(rt2x00dev, 3, 0x80);
        rt73usb_bbp_write(rt2x00dev, 15, 0x30);
        rt73usb_bbp_write(rt2x00dev, 21, 0xc8);
@@ -1152,7 +1170,8 @@ static void rt73usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
        rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
        rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
-                           state == STATE_RADIO_RX_OFF);
+                           (state == STATE_RADIO_RX_OFF) ||
+                           (state == STATE_RADIO_RX_OFF_LINK));
        rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 }
@@ -1161,11 +1180,9 @@ static int rt73usb_enable_radio(struct rt2x00_dev *rt2x00dev)
        /*
         * Initialize all registers.
         */
-        if (rt73usb_init_registers(rt2x00dev) ||
+        if (unlikely(rt73usb_init_registers(rt2x00dev) ||
-            rt73usb_init_bbp(rt2x00dev)) {
+                     rt73usb_init_bbp(rt2x00dev)))
-                ERROR(rt2x00dev, "Register initialization failed.\n");
                return -EIO;
-        }
        return 0;
 }
@@ -1187,7 +1204,6 @@ static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
        u32 reg;
        unsigned int i;
        char put_to_sleep;
-        char current_state;
        put_to_sleep = (state != STATE_AWAKE);
@@ -1203,16 +1219,12 @@ static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
         */
        for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
                rt73usb_register_read(rt2x00dev, MAC_CSR12, &reg);
-                current_state =
+                state = rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
-                    rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+                if (state == !put_to_sleep)
-                if (current_state == !put_to_sleep)
                        return 0;
                msleep(10);
        }
-        NOTICE(rt2x00dev, "Device failed to enter state %d, "
-               "current device state %d.\n", !put_to_sleep, current_state);
        return -EBUSY;
 }
@@ -1230,11 +1242,13 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        case STATE_RADIO_RX_ON:
        case STATE_RADIO_RX_ON_LINK:
-                rt73usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-                break;
        case STATE_RADIO_RX_OFF:
        case STATE_RADIO_RX_OFF_LINK:
-                rt73usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+                rt73usb_toggle_rx(rt2x00dev, state);
+                break;
+        case STATE_RADIO_IRQ_ON:
+        case STATE_RADIO_IRQ_OFF:
+                /* No support, but no error either */
                break;
        case STATE_DEEP_SLEEP:
        case STATE_SLEEP:
@@ -1247,6 +1261,10 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
                break;
        }
+        if (unlikely(retval))
+                ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+                      state, retval);
        return retval;
 }
@@ -1302,7 +1320,8 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
        rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
                           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_DATABYTE_COUNT,
+                           skb->len - skbdesc->desc_len);
        rt2x00_set_field32(&word, TXD_W0_BURST2,
                           test_bit(ENTRY_TXD_BURST, &txdesc->flags));
        rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
@@ -1332,8 +1351,10 @@ static void rt73usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 {
        u32 reg;
-        if (queue != QID_BEACON)
+        if (queue != QID_BEACON) {
+                rt2x00usb_kick_tx_queue(rt2x00dev, queue);
                return;
+        }
        /*
         * For Wi-Fi faily generated beacons between participating stations.
@@ -1407,14 +1428,10 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
        u32 word1;
        /*
-         * Copy descriptor to the skb->cb array, this has 2 benefits:
+         * Copy descriptor to the skbdesc->desc buffer, making it safe from moving of
-         * 1) Each descriptor word is 4 byte aligned.
+         * frame data in rt2x00usb.
-         * 2) Descriptor is safe  from moving of frame data in rt2x00usb.
         */
-        skbdesc->desc_len =
+        memcpy(skbdesc->desc, rxd, 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;
        /*
@@ -1446,8 +1463,6 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
         */
        skb_pull(entry->skb, entry->queue->desc_size);
        skb_trim(entry->skb, rxdesc->size);
-        skbdesc->data = entry->skb->data;
-        skbdesc->data_len = rxdesc->size;
 }
 /*
@@ -1620,31 +1635,11 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT73USB_LEDS
        rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
-        rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
+        rt73usb_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
-        rt2x00dev->led_radio.type = LED_TYPE_RADIO;
+        rt73usb_init_led(rt2x00dev, &rt2x00dev->led_assoc, LED_TYPE_ASSOC);
-        rt2x00dev->led_radio.led_dev.brightness_set =
+        if (value == LED_MODE_SIGNAL_STRENGTH)
-            rt73usb_brightness_set;
+                rt73usb_init_led(rt2x00dev, &rt2x00dev->led_qual,
-        rt2x00dev->led_radio.led_dev.blink_set =
+                                 LED_TYPE_QUALITY);
-            rt73usb_blink_set;
-        rt2x00dev->led_radio.flags = LED_INITIALIZED;
-        rt2x00dev->led_assoc.rt2x00dev = rt2x00dev;
-        rt2x00dev->led_assoc.type = LED_TYPE_ASSOC;
-        rt2x00dev->led_assoc.led_dev.brightness_set =
-            rt73usb_brightness_set;
-        rt2x00dev->led_assoc.led_dev.blink_set =
-            rt73usb_blink_set;
-        rt2x00dev->led_assoc.flags = LED_INITIALIZED;
-        if (value == LED_MODE_SIGNAL_STRENGTH) {
-                rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-                rt2x00dev->led_qual.type = LED_TYPE_QUALITY;
-                rt2x00dev->led_qual.led_dev.brightness_set =
-                    rt73usb_brightness_set;
-                rt2x00dev->led_qual.led_dev.blink_set =
-                    rt73usb_blink_set;
-                rt2x00dev->led_qual.flags = LED_INITIALIZED;
-        }
        rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
        rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
@@ -1980,9 +1975,6 @@ static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
         */
        skbdesc = get_skb_frame_desc(skb);
        memset(skbdesc, 0, sizeof(*skbdesc));
-        skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-        skbdesc->data = skb->data + intf->beacon->queue->desc_size;
-        skbdesc->data_len = skb->len - intf->beacon->queue->desc_size;
        skbdesc->desc = skb->data;
        skbdesc->desc_len = intf->beacon->queue->desc_size;
        skbdesc->entry = intf->beacon;
diff --git a/drivers/net/wireless/rt2x00/rt73usb.h b/drivers/net/wireless/rt2x00/rt73usb.h
index 25cdcc9bf7c4..148493501011 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.h
+++ b/drivers/net/wireless/rt2x00/rt73usb.h
@@ -39,8 +39,6 @@
 * Signal information.
 * Defaul offset is required for RSSI <-> dBm conversion.
 */
-#define MAX_SIGNAL                      100
-#define MAX_RX_SSI                      -1
 #define DEFAULT_RSSI_OFFSET             120
 /*
diff --git a/drivers/net/wireless/rtl8187_dev.c b/drivers/net/wireless/rtl8187_dev.c
index 0078c7e9918c..bec96d762c6c 100644
--- a/drivers/net/wireless/rtl8187_dev.c
+++ b/drivers/net/wireless/rtl8187_dev.c
@@ -181,7 +181,7 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
        flags |= RTL8187_TX_FLAG_NO_ENCRYPT;
        flags |= ieee80211_get_tx_rate(dev, info)->hw_value << 24;
-        if (ieee80211_get_morefrag((struct ieee80211_hdr *)skb->data))
+        if (ieee80211_has_morefrags(((struct ieee80211_hdr *)skb->data)->frame_control))
                flags |= RTL8187_TX_FLAG_MORE_FRAG;
        if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
                flags |= RTL8187_TX_FLAG_RTS;
diff --git a/include/linux/ieee80211.h b/include/linux/ieee80211.h
index 9300f37cd7e8..2998e3b5f166 100644
--- a/include/linux/ieee80211.h
+++ b/include/linux/ieee80211.h
@@ -98,6 +98,7 @@
 #define IEEE80211_MAX_SSID_LEN          32
 #define IEEE80211_MAX_MESH_ID_LEN       32
+#define IEEE80211_QOS_CTL_LEN           2
 struct ieee80211_hdr {
        __le16 frame_control;
@@ -109,6 +110,355 @@ struct ieee80211_hdr {
        u8 addr4[6];
 } __attribute__ ((packed));
+/**
+ * ieee80211_has_tods - check if IEEE80211_FCTL_TODS is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_tods(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_TODS)) != 0;
+}
+/**
+ * ieee80211_has_fromds - check if IEEE80211_FCTL_FROMDS is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_fromds(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FROMDS)) != 0;
+}
+/**
+ * ieee80211_has_a4 - check if IEEE80211_FCTL_TODS and IEEE80211_FCTL_FROMDS are set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_a4(__le16 fc)
+{
+        __le16 tmp = cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS);
+        return (fc & tmp) == tmp;
+}
+/**
+ * ieee80211_has_morefrags - check if IEEE80211_FCTL_MOREFRAGS is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_morefrags(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) != 0;
+}
+/**
+ * ieee80211_has_retry - check if IEEE80211_FCTL_RETRY is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_retry(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_RETRY)) != 0;
+}
+/**
+ * ieee80211_has_pm - check if IEEE80211_FCTL_PM is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_pm(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_PM)) != 0;
+}
+/**
+ * ieee80211_has_moredata - check if IEEE80211_FCTL_MOREDATA is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_moredata(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_MOREDATA)) != 0;
+}
+/**
+ * ieee80211_has_protected - check if IEEE80211_FCTL_PROTECTED is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_protected(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_PROTECTED)) != 0;
+}
+/**
+ * ieee80211_has_order - check if IEEE80211_FCTL_ORDER is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_has_order(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_ORDER)) != 0;
+}
+/**
+ * ieee80211_is_mgmt - check if type is IEEE80211_FTYPE_MGMT
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_mgmt(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT);
+}
+/**
+ * ieee80211_is_ctl - check if type is IEEE80211_FTYPE_CTL
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_ctl(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL);
+}
+/**
+ * ieee80211_is_data - check if type is IEEE80211_FTYPE_DATA
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_data(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_DATA);
+}
+/**
+ * ieee80211_is_data_qos - check if type is IEEE80211_FTYPE_DATA and IEEE80211_STYPE_QOS_DATA is set
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_data_qos(__le16 fc)
+{
+        /*
+         * mask with QOS_DATA rather than IEEE80211_FCTL_STYPE as we just need
+         * to check the one bit
+         */
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_STYPE_QOS_DATA)) ==
+               cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_DATA);
+}
+/**
+ * ieee80211_is_data_present - check if type is IEEE80211_FTYPE_DATA and has data
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_data_present(__le16 fc)
+{
+        /*
+         * mask with 0x40 and test that that bit is clear to only return true
+         * for the data-containing substypes.
+         */
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | 0x40)) ==
+               cpu_to_le16(IEEE80211_FTYPE_DATA);
+}
+/**
+ * ieee80211_is_assoc_req - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_ASSOC_REQ
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_assoc_req(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ASSOC_REQ);
+}
+/**
+ * ieee80211_is_assoc_resp - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_ASSOC_RESP
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_assoc_resp(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ASSOC_RESP);
+}
+/**
+ * ieee80211_is_reassoc_req - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_REASSOC_REQ
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_reassoc_req(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_REASSOC_REQ);
+}
+/**
+ * ieee80211_is_reassoc_resp - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_REASSOC_RESP
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_reassoc_resp(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_REASSOC_RESP);
+}
+/**
+ * ieee80211_is_probe_req - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_PROBE_REQ
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_probe_req(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_REQ);
+}
+/**
+ * ieee80211_is_probe_resp - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_PROBE_RESP
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_probe_resp(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
+}
+/**
+ * ieee80211_is_beacon - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_BEACON
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_beacon(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON);
+}
+/**
+ * ieee80211_is_atim - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_ATIM
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_atim(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ATIM);
+}
+/**
+ * ieee80211_is_disassoc - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_DISASSOC
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_disassoc(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DISASSOC);
+}
+/**
+ * ieee80211_is_auth - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_AUTH
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_auth(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_AUTH);
+}
+/**
+ * ieee80211_is_deauth - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_DEAUTH
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_deauth(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
+}
+/**
+ * ieee80211_is_action - check if IEEE80211_FTYPE_MGMT && IEEE80211_STYPE_ACTION
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_action(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_ACTION);
+}
+/**
+ * ieee80211_is_back_req - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_BACK_REQ
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_back_req(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_BACK_REQ);
+}
+/**
+ * ieee80211_is_back - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_BACK
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_back(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_BACK);
+}
+/**
+ * ieee80211_is_pspoll - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_PSPOLL
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_pspoll(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_PSPOLL);
+}
+/**
+ * ieee80211_is_rts - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_RTS
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_rts(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
+}
+/**
+ * ieee80211_is_cts - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_CTS
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_cts(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CTS);
+}
+/**
+ * ieee80211_is_ack - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_ACK
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_ack(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_ACK);
+}
+/**
+ * ieee80211_is_cfend - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_CFEND
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_cfend(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CFEND);
+}
+/**
+ * ieee80211_is_cfendack - check if IEEE80211_FTYPE_CTL && IEEE80211_STYPE_CFENDACK
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_cfendack(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_CFENDACK);
+}
+/**
+ * ieee80211_is_nullfunc - check if FTYPE=IEEE80211_FTYPE_DATA and STYPE=IEEE80211_STYPE_NULLFUNC
+ * @fc: frame control bytes in little-endian byteorder
+ */
+static inline int ieee80211_is_nullfunc(__le16 fc)
+{
+        return (fc & cpu_to_le16(IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) ==
+               cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_NULLFUNC);
+}
 struct ieee80211s_hdr {
        u8 flags;
@@ -320,6 +670,7 @@ struct ieee80211_ht_addt_info {
 #define IEEE80211_HT_CAP_MCS_TX_UEQM            0x10
 /* 802.11n HT IE masks */
 #define IEEE80211_HT_IE_CHA_SEC_OFFSET          0x03
+#define IEEE80211_HT_IE_CHA_SEC_NONE            0x00
 #define IEEE80211_HT_IE_CHA_SEC_ABOVE           0x01
 #define IEEE80211_HT_IE_CHA_SEC_BELOW           0x03
 #define IEEE80211_HT_IE_CHA_WIDTH               0x04
@@ -552,65 +903,57 @@ enum ieee80211_back_parties {
 #define WLAN_MAX_KEY_LEN                32
 /**
+ * ieee80211_get_qos_ctl - get pointer to qos control bytes
+ * @hdr: the frame
+ *
+ * The qos ctrl bytes come after the frame_control, duration, seq_num
+ * and 3 or 4 addresses of length ETH_ALEN.
+ * 3 addr: 2 + 2 + 2 + 3*6 = 24
+ * 4 addr: 2 + 2 + 2 + 4*6 = 30
+ */
+static inline u8 *ieee80211_get_qos_ctl(struct ieee80211_hdr *hdr)
+{
+        if (ieee80211_has_a4(hdr->frame_control))
+                return (u8 *)hdr + 30;
+        else
+                return (u8 *)hdr + 24;
+}
+/**
 * ieee80211_get_SA - get pointer to SA
+ * @hdr: the frame
 *
 * Given an 802.11 frame, this function returns the offset
 * to the source address (SA). It does not verify that the
 * header is long enough to contain the address, and the
 * header must be long enough to contain the frame control
 * field.
- *
- * @hdr: the frame
 */
 static inline u8 *ieee80211_get_SA(struct ieee80211_hdr *hdr)
 {
-        __le16 fc = hdr->frame_control;
+        if (ieee80211_has_a4(hdr->frame_control))
-        fc &= cpu_to_le16(IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS);
-        switch (fc) {
-        case __constant_cpu_to_le16(IEEE80211_FCTL_FROMDS):
-                return hdr->addr3;
-        case __constant_cpu_to_le16(IEEE80211_FCTL_TODS|IEEE80211_FCTL_FROMDS):
                return hdr->addr4;
-        default:
+        if (ieee80211_has_fromds(hdr->frame_control))
-                return hdr->addr2;
+                return hdr->addr3;
-        }
+        return hdr->addr2;
 }
 /**
 * ieee80211_get_DA - get pointer to DA
+ * @hdr: the frame
 *
 * Given an 802.11 frame, this function returns the offset
 * to the destination address (DA). It does not verify that
 * the header is long enough to contain the address, and the
 * header must be long enough to contain the frame control
 * field.
- *
- * @hdr: the frame
 */
 static inline u8 *ieee80211_get_DA(struct ieee80211_hdr *hdr)
 {
-        __le16 fc = hdr->frame_control;
+        if (ieee80211_has_tods(hdr->frame_control))
-        fc &= cpu_to_le16(IEEE80211_FCTL_TODS);
-        if (fc)
                return hdr->addr3;
        else
                return hdr->addr1;
 }
-/**
- * ieee80211_get_morefrag - determine whether the MOREFRAGS bit is set
- *
- * This function determines whether the "more fragments" bit is set
- * in the frame.
- *
- * @hdr: the frame
- */
-static inline int ieee80211_get_morefrag(struct ieee80211_hdr *hdr)
-{
-        __le16 fc = hdr->frame_control;
-        return !!(fc & cpu_to_le16(IEEE80211_FCTL_MOREFRAGS));
-}
 #endif /* IEEE80211_H */
diff --git a/include/linux/nl80211.h b/include/linux/nl80211.h
index ea6517e58b04..aa8411e2a160 100644
--- a/include/linux/nl80211.h
+++ b/include/linux/nl80211.h
@@ -122,13 +122,13 @@ enum nl80211_commands {
        NL80211_CMD_NEW_STATION,
        NL80211_CMD_DEL_STATION,
-        /* add commands here */
        NL80211_CMD_GET_MPATH,
        NL80211_CMD_SET_MPATH,
        NL80211_CMD_NEW_MPATH,
        NL80211_CMD_DEL_MPATH,
+        /* add commands here */
        /* used to define NL80211_CMD_MAX below */
        __NL80211_CMD_AFTER_LAST,
        NL80211_CMD_MAX = __NL80211_CMD_AFTER_LAST - 1
@@ -230,13 +230,13 @@ enum nl80211_attrs {
        NL80211_ATTR_MNTR_FLAGS,
-        /* add attributes here, update the policy in nl80211.c */
        NL80211_ATTR_MESH_ID,
        NL80211_ATTR_STA_PLINK_ACTION,
        NL80211_ATTR_MPATH_NEXT_HOP,
        NL80211_ATTR_MPATH_INFO,
+        /* add attributes here, update the policy in nl80211.c */
        __NL80211_ATTR_AFTER_LAST,
        NL80211_ATTR_MAX = __NL80211_ATTR_AFTER_LAST - 1
 };
diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 1196de85f8db..7ab4ff6159a2 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -1535,7 +1535,7 @@ ieee80211_get_buffered_bc(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
 *
 * @skb: the frame
 */
-int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb);
+unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb);
 /**
 * ieee80211_get_hdrlen - get header length from frame control
@@ -1548,6 +1548,12 @@ int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb);
 int ieee80211_get_hdrlen(u16 fc);
 /**
+ * ieee80211_hdrlen - get header length in bytes from frame control
+ * @fc: frame control field in little-endian format
+ */
+unsigned int ieee80211_hdrlen(__le16 fc);
+/**
 * ieee80211_get_tkip_key - get a TKIP rc4 for skb
 *
 * This function computes a TKIP rc4 key for an skb. It computes
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index b19bd16703b2..14fccf16b80f 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -876,7 +876,7 @@ void ieee80211_rx_bss_list_deinit(struct net_device *dev);
 int ieee80211_sta_set_extra_ie(struct net_device *dev, char *ie, size_t len);
 struct sta_info *ieee80211_ibss_add_sta(struct net_device *dev,
                                        struct sk_buff *skb, u8 *bssid,
-                                        u8 *addr);
+                                        u8 *addr, u64 supp_rates);
 int ieee80211_sta_deauthenticate(struct net_device *dev, u16 reason);
 int ieee80211_sta_disassociate(struct net_device *dev, u16 reason);
 void ieee80211_bss_info_change_notify(struct ieee80211_sub_if_data *sdata,
diff --git a/net/mac80211/main.c b/net/mac80211/main.c
index b182f018a187..5c5396edad32 100644
--- a/net/mac80211/main.c
+++ b/net/mac80211/main.c
@@ -1707,7 +1707,8 @@ int ieee80211_register_hw(struct ieee80211_hw *hw)
        debugfs_hw_add(local);
-        local->hw.conf.beacon_int = 1000;
+        if (local->hw.conf.beacon_int < 10)
+                local->hw.conf.beacon_int = 100;
        local->wstats_flags |= local->hw.flags & (IEEE80211_HW_SIGNAL_UNSPEC |
                                                  IEEE80211_HW_SIGNAL_DB |
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index 7f05820dc629..55659a730dc1 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -2863,7 +2863,8 @@ static void ieee80211_rx_bss_info(struct net_device *dev,
                                       dev->name, print_mac(mac, mgmt->bssid));
                        ieee80211_sta_join_ibss(dev, &sdata->u.sta, bss);
                        ieee80211_ibss_add_sta(dev, NULL,
-                                               mgmt->bssid, mgmt->sa);
+                                               mgmt->bssid, mgmt->sa,
+                                               BIT(rx_status->rate_idx));
                }
        }
@@ -3583,7 +3584,7 @@ static int ieee80211_sta_create_ibss(struct net_device *dev,
        sband = local->hw.wiphy->bands[bss->band];
        if (local->hw.conf.beacon_int == 0)
-                local->hw.conf.beacon_int = 10000;
+                local->hw.conf.beacon_int = 100;
        bss->beacon_int = local->hw.conf.beacon_int;
        bss->last_update = jiffies;
        bss->capability = WLAN_CAPABILITY_IBSS;
@@ -4307,12 +4308,13 @@ int ieee80211_sta_set_extra_ie(struct net_device *dev, char *ie, size_t len)
 struct sta_info *ieee80211_ibss_add_sta(struct net_device *dev,
                                        struct sk_buff *skb, u8 *bssid,
-                                        u8 *addr)
+                                        u8 *addr, u64 supp_rates)
 {
        struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
        struct sta_info *sta;
        struct ieee80211_sub_if_data *sdata = IEEE80211_DEV_TO_SUB_IF(dev);
        DECLARE_MAC_BUF(mac);
+        int band = local->hw.conf.channel->band;
        /* TODO: Could consider removing the least recently used entry and
         * allow new one to be added. */
@@ -4324,6 +4326,9 @@ struct sta_info *ieee80211_ibss_add_sta(struct net_device *dev,
                return NULL;
        }
+        if (!ieee80211_bssid_match(bssid, sdata->u.sta.bssid))
+                return NULL;
        printk(KERN_DEBUG "%s: Adding new IBSS station %s (dev=%s)\n",
               wiphy_name(local->hw.wiphy), print_mac(mac, addr), dev->name);
@@ -4333,8 +4338,10 @@ struct sta_info *ieee80211_ibss_add_sta(struct net_device *dev,
        set_sta_flags(sta, WLAN_STA_AUTHORIZED);
-        sta->supp_rates[local->hw.conf.channel->band] =
+        if (supp_rates)
-                sdata->u.sta.supp_rates_bits[local->hw.conf.channel->band];
+                sta->supp_rates[band] = supp_rates;
+        else
+                sta->supp_rates[band] = sdata->u.sta.supp_rates_bits[band];
        rate_control_rate_init(sta, local);
diff --git a/net/mac80211/rx.c b/net/mac80211/rx.c
index a3643fd86af9..c32a0bcd53b7 100644
--- a/net/mac80211/rx.c
+++ b/net/mac80211/rx.c
@@ -67,12 +67,9 @@ static inline int should_drop_frame(struct ieee80211_rx_status *status,
                return 1;
        if (unlikely(skb->len < 16 + present_fcs_len + radiotap_len))
                return 1;
-        if (((hdr->frame_control & cpu_to_le16(IEEE80211_FCTL_FTYPE)) ==
+        if (ieee80211_is_ctl(hdr->frame_control) &&
-                        cpu_to_le16(IEEE80211_FTYPE_CTL)) &&
+            !ieee80211_is_pspoll(hdr->frame_control) &&
-            ((hdr->frame_control & cpu_to_le16(IEEE80211_FCTL_STYPE)) !=
+            !ieee80211_is_back_req(hdr->frame_control))
-                        cpu_to_le16(IEEE80211_STYPE_PSPOLL)) &&
-            ((hdr->frame_control & cpu_to_le16(IEEE80211_FCTL_STYPE)) !=
-                        cpu_to_le16(IEEE80211_STYPE_BACK_REQ)))
                return 1;
        return 0;
 }
@@ -1826,8 +1823,13 @@ static int prepare_for_handlers(struct ieee80211_sub_if_data *sdata,
                if (!bssid)
                        return 0;
                if ((rx->fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT &&
-                    (rx->fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON)
+                    (rx->fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON) {
+                        if (!rx->sta)
+                                rx->sta = ieee80211_ibss_add_sta(sdata->dev,
+                                                rx->skb, bssid, hdr->addr2,
+                                                BIT(rx->status->rate_idx));
                        return 1;
+                }
                else if (!ieee80211_bssid_match(bssid, sdata->u.sta.bssid)) {
                        if (!(rx->flags & IEEE80211_RX_IN_SCAN))
                                return 0;
@@ -1840,7 +1842,8 @@ static int prepare_for_handlers(struct ieee80211_sub_if_data *sdata,
                        rx->flags &= ~IEEE80211_RX_RA_MATCH;
                } else if (!rx->sta)
                        rx->sta = ieee80211_ibss_add_sta(sdata->dev, rx->skb,
-                                                         bssid, hdr->addr2);
+                                                bssid, hdr->addr2,
+                                                BIT(rx->status->rate_idx));
                break;
        case IEEE80211_IF_TYPE_MESH_POINT:
                if (!multicast &&
@@ -2118,7 +2121,7 @@ static u8 ieee80211_rx_reorder_ampdu(struct ieee80211_local *local,
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
        struct sta_info *sta;
        struct tid_ampdu_rx *tid_agg_rx;
-        u16 fc, sc;
+        u16 sc;
        u16 mpdu_seq_num;
        u8 ret = 0, *qc;
        int tid;
@@ -2127,14 +2130,12 @@ static u8 ieee80211_rx_reorder_ampdu(struct ieee80211_local *local,
        if (!sta)
                return ret;
-        fc = le16_to_cpu(hdr->frame_control);
        /* filter the QoS data rx stream according to
         * STA/TID and check if this STA/TID is on aggregation */
-        if (!WLAN_FC_IS_QOS_DATA(fc))
+        if (!ieee80211_is_data_qos(hdr->frame_control))
                goto end_reorder;
-        qc = skb->data + ieee80211_get_hdrlen(fc) - QOS_CONTROL_LEN;
+        qc = ieee80211_get_qos_ctl(hdr);
        tid = qc[0] & QOS_CONTROL_TID_MASK;
        if (sta->ampdu_mlme.tid_state_rx[tid] != HT_AGG_STATE_OPERATIONAL)
@@ -2143,7 +2144,7 @@ static u8 ieee80211_rx_reorder_ampdu(struct ieee80211_local *local,
        tid_agg_rx = sta->ampdu_mlme.tid_rx[tid];
        /* null data frames are excluded */
-        if (unlikely(fc & IEEE80211_STYPE_NULLFUNC))
+        if (unlikely(ieee80211_is_nullfunc(hdr->frame_control)))
                goto end_reorder;
        /* new un-ordered ampdu frame - process it */
diff --git a/net/mac80211/tkip.c b/net/mac80211/tkip.c
index a00cf1ea7719..e710243d82e2 100644
--- a/net/mac80211/tkip.c
+++ b/net/mac80211/tkip.c
@@ -64,6 +64,14 @@ static u16 tkipS(u16 val)
        return tkip_sbox[val & 0xff] ^ swab16(tkip_sbox[val >> 8]);
 }
+static u8 *write_tkip_iv(u8 *pos, u16 iv16)
+{
+        *pos++ = iv16 >> 8;
+        *pos++ = ((iv16 >> 8) | 0x20) & 0x7f;
+        *pos++ = iv16 & 0xFF;
+        return pos;
+}
 /*
 * P1K := Phase1(TA, TK, TSC)
 * TA = transmitter address (48 bits)
@@ -71,11 +79,10 @@ static u16 tkipS(u16 val)
 * TSC = TKIP sequence counter (48 bits, only 32 msb bits used)
 * P1K: 80 bits
 */
-static void tkip_mixing_phase1(struct ieee80211_key *key, const u8 *ta,
+static void tkip_mixing_phase1(const u8 *tk, struct tkip_ctx *ctx,
-                               struct tkip_ctx *ctx, u32 tsc_IV32)
+                               const u8 *ta, u32 tsc_IV32)
 {
        int i, j;
-        const u8 *tk = &key->conf.key[ALG_TKIP_TEMP_ENCR_KEY];
        u16 *p1k = ctx->p1k;
        p1k[0] = tsc_IV32 & 0xFFFF;
@@ -95,12 +102,11 @@ static void tkip_mixing_phase1(struct ieee80211_key *key, const u8 *ta,
        ctx->initialized = 1;
 }
-static void tkip_mixing_phase2(struct ieee80211_key *key, struct tkip_ctx *ctx,
+static void tkip_mixing_phase2(const u8 *tk, struct tkip_ctx *ctx,
                               u16 tsc_IV16, u8 *rc4key)
 {
        u16 ppk[6];
        const u16 *p1k = ctx->p1k;
-        const u8 *tk = &key->conf.key[ALG_TKIP_TEMP_ENCR_KEY];
        int i;
        ppk[0] = p1k[0];
@@ -123,12 +129,9 @@ static void tkip_mixing_phase2(struct ieee80211_key *key, struct tkip_ctx *ctx,
        ppk[4] += ror16(ppk[3], 1);
        ppk[5] += ror16(ppk[4], 1);
-        rc4key[0] = tsc_IV16 >> 8;
+        rc4key = write_tkip_iv(rc4key, tsc_IV16);
-        rc4key[1] = ((tsc_IV16 >> 8) | 0x20) & 0x7f;
+        *rc4key++ = ((ppk[5] ^ get_unaligned_le16(tk)) >> 1) & 0xFF;
-        rc4key[2] = tsc_IV16 & 0xFF;
-        rc4key[3] = ((ppk[5] ^ get_unaligned_le16(tk)) >> 1) & 0xFF;
-        rc4key += 4;
        for (i = 0; i < 6; i++)
                put_unaligned_le16(ppk[i], rc4key + 2 * i);
 }
@@ -136,51 +139,41 @@ static void tkip_mixing_phase2(struct ieee80211_key *key, struct tkip_ctx *ctx,
 /* Add TKIP IV and Ext. IV at @pos. @iv0, @iv1, and @iv2 are the first octets
 * of the IV. Returns pointer to the octet following IVs (i.e., beginning of
 * the packet payload). */
-u8 *ieee80211_tkip_add_iv(u8 *pos, struct ieee80211_key *key,
+u8 *ieee80211_tkip_add_iv(u8 *pos, struct ieee80211_key *key, u16 iv16)
-                           u8 iv0, u8 iv1, u8 iv2)
 {
-        *pos++ = iv0;
+        pos = write_tkip_iv(pos, iv16);
-        *pos++ = iv1;
-        *pos++ = iv2;
        *pos++ = (key->conf.keyidx << 6) | (1 << 5) /* Ext IV */;
        put_unaligned_le32(key->u.tkip.tx.iv32, pos);
        return pos + 4;
 }
-static void ieee80211_tkip_gen_rc4key(struct ieee80211_key *key, u8 *ta,
-                               u8 *rc4key)
-{
-        /* Calculate per-packet key */
-        if (key->u.tkip.tx.iv16 == 0 || !key->u.tkip.tx.initialized)
-                tkip_mixing_phase1(key, ta, &key->u.tkip.tx, key->u.tkip.tx.iv32);
-        tkip_mixing_phase2(key, &key->u.tkip.tx, key->u.tkip.tx.iv16, rc4key);
-}
 void ieee80211_get_tkip_key(struct ieee80211_key_conf *keyconf,
                        struct sk_buff *skb, enum ieee80211_tkip_key_type type,
                        u8 *outkey)
 {
        struct ieee80211_key *key = (struct ieee80211_key *)
                        container_of(keyconf, struct ieee80211_key, conf);
-        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-        u8 *data = (u8 *) hdr;
+        u8 *data;
-        u16 fc = le16_to_cpu(hdr->frame_control);
+        const u8 *tk;
-        int hdr_len = ieee80211_get_hdrlen(fc);
+        struct tkip_ctx *ctx;
-        u8 *ta = hdr->addr2;
        u16 iv16;
        u32 iv32;
-        iv16 = data[hdr_len + 2] | (data[hdr_len] << 8);
+        data = (u8 *)hdr + ieee80211_hdrlen(hdr->frame_control);
-        iv32 = get_unaligned_le32(data + hdr_len + 4);
+        iv16 = data[2] | (data[0] << 8);
+        iv32 = get_unaligned_le32(&data[4]);
+        tk = &key->conf.key[ALG_TKIP_TEMP_ENCR_KEY];
+        ctx = &key->u.tkip.tx;
 #ifdef CONFIG_TKIP_DEBUG
        printk(KERN_DEBUG "TKIP encrypt: iv16 = 0x%04x, iv32 = 0x%08x\n",
                        iv16, iv32);
-        if (iv32 != key->u.tkip.tx.iv32) {
+        if (iv32 != ctx->iv32) {
                printk(KERN_DEBUG "skb: iv32 = 0x%08x key: iv32 = 0x%08x\n",
-                        iv32, key->u.tkip.tx.iv32);
+                        iv32, ctx->iv32);
                printk(KERN_DEBUG "Wrap around of iv16 in the middle of a "
                        "fragmented packet\n");
        }
@@ -189,15 +182,15 @@ void ieee80211_get_tkip_key(struct ieee80211_key_conf *keyconf,
        /* Update the p1k only when the iv16 in the packet wraps around, this
         * might occur after the wrap around of iv16 in the key in case of
         * fragmented packets. */
-        if (iv16 == 0 || !key->u.tkip.tx.initialized)
+        if (iv16 == 0 || !ctx->initialized)
-                tkip_mixing_phase1(key, ta, &key->u.tkip.tx, iv32);
+                tkip_mixing_phase1(tk, ctx, hdr->addr2, iv32);
        if (type == IEEE80211_TKIP_P1_KEY) {
-                memcpy(outkey, key->u.tkip.tx.p1k, sizeof(u16) * 5);
+                memcpy(outkey, ctx->p1k, sizeof(u16) * 5);
                return;
        }
-        tkip_mixing_phase2(key, &key->u.tkip.tx, iv16, outkey);
+        tkip_mixing_phase2(tk, ctx, iv16, outkey);
 }
 EXPORT_SYMBOL(ieee80211_get_tkip_key);
@@ -211,9 +204,16 @@ void ieee80211_tkip_encrypt_data(struct crypto_blkcipher *tfm,
                                 u8 *pos, size_t payload_len, u8 *ta)
 {
        u8 rc4key[16];
+        struct tkip_ctx *ctx = &key->u.tkip.tx;
+        const u8 *tk = &key->conf.key[ALG_TKIP_TEMP_ENCR_KEY];
-        ieee80211_tkip_gen_rc4key(key, ta, rc4key);
+        /* Calculate per-packet key */
-        pos = ieee80211_tkip_add_iv(pos, key, rc4key[0], rc4key[1], rc4key[2]);
+        if (ctx->iv16 == 0 || !ctx->initialized)
+                tkip_mixing_phase1(tk, ctx, ta, ctx->iv32);
+        tkip_mixing_phase2(tk, ctx, ctx->iv16, rc4key);
+        pos = ieee80211_tkip_add_iv(pos, key, key->u.tkip.tx.iv16);
        ieee80211_wep_encrypt_data(tfm, rc4key, 16, pos, payload_len);
 }
@@ -231,6 +231,7 @@ int ieee80211_tkip_decrypt_data(struct crypto_blkcipher *tfm,
        u32 iv16;
        u8 rc4key[16], keyid, *pos = payload;
        int res;
+        const u8 *tk = &key->conf.key[ALG_TKIP_TEMP_ENCR_KEY];
        if (payload_len < 12)
                return -1;
@@ -281,7 +282,7 @@ int ieee80211_tkip_decrypt_data(struct crypto_blkcipher *tfm,
        if (!key->u.tkip.rx[queue].initialized ||
            key->u.tkip.rx[queue].iv32 != iv32) {
                /* IV16 wrapped around - perform TKIP phase 1 */
-                tkip_mixing_phase1(key, ta, &key->u.tkip.rx[queue], iv32);
+                tkip_mixing_phase1(tk, &key->u.tkip.rx[queue], ta, iv32);
 #ifdef CONFIG_TKIP_DEBUG
                {
                        int i;
@@ -314,7 +315,7 @@ int ieee80211_tkip_decrypt_data(struct crypto_blkcipher *tfm,
                }
        }
-        tkip_mixing_phase2(key, &key->u.tkip.rx[queue], iv16, rc4key);
+        tkip_mixing_phase2(tk, &key->u.tkip.rx[queue], iv16, rc4key);
 #ifdef CONFIG_TKIP_DEBUG
        {
                int i;
diff --git a/net/mac80211/tkip.h b/net/mac80211/tkip.h
index b890427fc959..d4714383f5fc 100644
--- a/net/mac80211/tkip.h
+++ b/net/mac80211/tkip.h
@@ -13,8 +13,8 @@
 #include <linux/crypto.h>
 #include "key.h"
-u8 *ieee80211_tkip_add_iv(u8 *pos, struct ieee80211_key *key,
+u8 *ieee80211_tkip_add_iv(u8 *pos, struct ieee80211_key *key, u16 iv16);
-                          u8 iv0, u8 iv1, u8 iv2);
 void ieee80211_tkip_encrypt_data(struct crypto_blkcipher *tfm,
                                 struct ieee80211_key *key,
                                 u8 *pos, size_t payload_len, u8 *ta);
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index 1ad9e664f287..195cb6dd02a0 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -660,9 +660,8 @@ ieee80211_tx_h_fragment(struct ieee80211_tx_data *tx)
        /*
         * Warn when submitting a fragmented A-MPDU frame and drop it.
-         * This is an error and needs to be fixed elsewhere, but when
+         * This scenario is handled in __ieee80211_tx_prepare but extra
-         * done needs to take care of monitor interfaces (injection)
+         * caution taken here as fragmented ampdu may cause Tx stop.
-         * etc.
         */
        if (WARN_ON(tx->flags & IEEE80211_TX_CTL_AMPDU ||
                    skb_get_queue_mapping(tx->skb) >=
@@ -981,7 +980,8 @@ __ieee80211_tx_prepare(struct ieee80211_tx_data *tx,
        if (tx->flags & IEEE80211_TX_FRAGMENTED) {
                if ((tx->flags & IEEE80211_TX_UNICAST) &&
                    skb->len + FCS_LEN > local->fragmentation_threshold &&
-                    !local->ops->set_frag_threshold)
+                    !local->ops->set_frag_threshold &&
+                    !(info->flags & IEEE80211_TX_CTL_AMPDU))
                        tx->flags |= IEEE80211_TX_FRAGMENTED;
                else
                        tx->flags &= ~IEEE80211_TX_FRAGMENTED;
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 6513bc2d2707..ce62b163b82c 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -45,38 +45,37 @@ const unsigned char bridge_tunnel_header[] __aligned(2) =
 u8 *ieee80211_get_bssid(struct ieee80211_hdr *hdr, size_t len,
                        enum ieee80211_if_types type)
 {
-        u16 fc;
+        __le16 fc = hdr->frame_control;
         /* drop ACK/CTS frames and incorrect hdr len (ctrl) */
        if (len < 16)
                return NULL;
-        fc = le16_to_cpu(hdr->frame_control);
+        if (ieee80211_is_data(fc)) {
-        switch (fc & IEEE80211_FCTL_FTYPE) {
-        case IEEE80211_FTYPE_DATA:
                if (len < 24) /* drop incorrect hdr len (data) */
                        return NULL;
-                switch (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) {
-                case IEEE80211_FCTL_TODS:
+                if (ieee80211_has_a4(fc))
-                        return hdr->addr1;
-                case (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS):
                        return NULL;
-                case IEEE80211_FCTL_FROMDS:
+                if (ieee80211_has_tods(fc))
+                        return hdr->addr1;
+                if (ieee80211_has_fromds(fc))
                        return hdr->addr2;
-                case 0:
-                        return hdr->addr3;
+                return hdr->addr3;
-                }
+        }
-                break;
-        case IEEE80211_FTYPE_MGMT:
+        if (ieee80211_is_mgmt(fc)) {
                if (len < 24) /* drop incorrect hdr len (mgmt) */
                        return NULL;
                return hdr->addr3;
-        case IEEE80211_FTYPE_CTL:
+        }
-                if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)
+        if (ieee80211_is_ctl(fc)) {
+                if(ieee80211_is_pspoll(fc))
                        return hdr->addr1;
-                else if ((fc & IEEE80211_FCTL_STYPE) ==
-                                                IEEE80211_STYPE_BACK_REQ) {
+                if (ieee80211_is_back_req(fc)) {
                        switch (type) {
                        case IEEE80211_IF_TYPE_STA:
                                return hdr->addr2;
@@ -84,11 +83,9 @@ u8 *ieee80211_get_bssid(struct ieee80211_hdr *hdr, size_t len,
                        case IEEE80211_IF_TYPE_VLAN:
                                return hdr->addr1;
                        default:
-                                return NULL;
+                                break; /* fall through to the return */
                        }
                }
-                else
-                        return NULL;
        }
        return NULL;
@@ -133,14 +130,46 @@ int ieee80211_get_hdrlen(u16 fc)
 }
 EXPORT_SYMBOL(ieee80211_get_hdrlen);
-int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
+unsigned int ieee80211_hdrlen(__le16 fc)
+{
+        unsigned int hdrlen = 24;
+        if (ieee80211_is_data(fc)) {
+                if (ieee80211_has_a4(fc))
+                        hdrlen = 30;
+                if (ieee80211_is_data_qos(fc))
+                        hdrlen += IEEE80211_QOS_CTL_LEN;
+                goto out;
+        }
+        if (ieee80211_is_ctl(fc)) {
+                /*
+                 * ACK and CTS are 10 bytes, all others 16. To see how
+                 * to get this condition consider
+                 *   subtype mask:   0b0000000011110000 (0x00F0)
+                 *   ACK subtype:    0b0000000011010000 (0x00D0)
+                 *   CTS subtype:    0b0000000011000000 (0x00C0)
+                 *   bits that matter:         ^^^      (0x00E0)
+                 *   value of those: 0b0000000011000000 (0x00C0)
+                 */
+                if ((fc & cpu_to_le16(0x00E0)) == cpu_to_le16(0x00C0))
+                        hdrlen = 10;
+                else
+                        hdrlen = 16;
+        }
+out:
+        return hdrlen;
+}
+EXPORT_SYMBOL(ieee80211_hdrlen);
+unsigned int ieee80211_get_hdrlen_from_skb(const struct sk_buff *skb)
 {
-        const struct ieee80211_hdr *hdr = (const struct ieee80211_hdr *) skb->data;
+        const struct ieee80211_hdr *hdr = (const struct ieee80211_hdr *)skb->data;
-        int hdrlen;
+        unsigned int hdrlen;
        if (unlikely(skb->len < 10))
                return 0;
-        hdrlen = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
        if (unlikely(hdrlen > skb->len))
                return 0;
        return hdrlen;
diff --git a/net/mac80211/wme.c b/net/mac80211/wme.c
index 14a9ff10a1e9..d8c2f9688b25 100644
--- a/net/mac80211/wme.c
+++ b/net/mac80211/wme.c
@@ -105,11 +105,8 @@ static int classify80211(struct sk_buff *skb, struct Qdisc *qd)
 {
        struct ieee80211_local *local = wdev_priv(qd->dev->ieee80211_ptr);
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-        unsigned short fc = le16_to_cpu(hdr->frame_control);
-        int qos;
-        /* see if frame is data or non data frame */
+        if (!ieee80211_is_data(hdr->frame_control)) {
-        if (unlikely((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)) {
                /* management frames go on AC_VO queue, but are sent
                * without QoS control fields */
                return 0;
@@ -119,10 +116,7 @@ static int classify80211(struct sk_buff *skb, struct Qdisc *qd)
                /* use AC from radiotap */
        }
-        /* is this a QoS frame? */
+        if (!ieee80211_is_data_qos(hdr->frame_control)) {
-        qos = fc & IEEE80211_STYPE_QOS_DATA;
-        if (!qos) {
                skb->priority = 0; /* required for correct WPA/11i MIC */
                return ieee802_1d_to_ac[skb->priority];
        }
@@ -151,7 +145,6 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
        struct ieee80211_sched_data *q = qdisc_priv(qd);
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
-        unsigned short fc = le16_to_cpu(hdr->frame_control);
        struct Qdisc *qdisc;
        struct sta_info *sta;
        int err, queue;
@@ -185,16 +178,15 @@ static int wme_qdiscop_enqueue(struct sk_buff *skb, struct Qdisc* qd)
        /* now we know the 1d priority, fill in the QoS header if there is one
         */
-        if (WLAN_FC_IS_QOS_DATA(fc)) {
+        if (ieee80211_is_data_qos(hdr->frame_control)) {
-                u8 *p = skb->data + ieee80211_get_hdrlen(fc) - 2;
+                u8 *p = ieee80211_get_qos_ctl(hdr);
                u8 ack_policy = 0;
                tid = skb->priority & QOS_CONTROL_TAG1D_MASK;
                if (local->wifi_wme_noack_test)
                        ack_policy |= QOS_CONTROL_ACK_POLICY_NOACK <<
                                        QOS_CONTROL_ACK_POLICY_SHIFT;
                /* qos header is 2 bytes, second reserved */
-                *p = ack_policy | tid;
+                *p++ = ack_policy | tid;
-                p++;
                *p = 0;
                rcu_read_lock();
diff --git a/net/mac80211/wpa.c b/net/mac80211/wpa.c
index 9f6fd20374e1..345e10e9b313 100644
--- a/net/mac80211/wpa.c
+++ b/net/mac80211/wpa.c
@@ -24,46 +24,22 @@ static int ieee80211_get_hdr_info(const struct sk_buff *skb, u8 **sa, u8 **da,
 {
        struct ieee80211_hdr *hdr;
        size_t hdrlen;
-        u16 fc;
+        __le16 fc;
-        int a4_included;
-        u8 *pos;
-        hdr = (struct ieee80211_hdr *) skb->data;
+        hdr = (struct ieee80211_hdr *)skb->data;
-        fc = le16_to_cpu(hdr->frame_control);
+        fc = hdr->frame_control;
-        hdrlen = 24;
-        if ((fc & (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) ==
-            (IEEE80211_FCTL_FROMDS | IEEE80211_FCTL_TODS)) {
-                hdrlen += ETH_ALEN;
-                *sa = hdr->addr4;
-                *da = hdr->addr3;
-        } else if (fc & IEEE80211_FCTL_FROMDS) {
-                *sa = hdr->addr3;
-                *da = hdr->addr1;
-        } else if (fc & IEEE80211_FCTL_TODS) {
-                *sa = hdr->addr2;
-                *da = hdr->addr3;
-        } else {
-                *sa = hdr->addr2;
-                *da = hdr->addr1;
-        }
-        if (fc & 0x80)
+        hdrlen = ieee80211_hdrlen(fc);
-                hdrlen += 2;
+        *sa = ieee80211_get_SA(hdr);
+        *da = ieee80211_get_DA(hdr);
        *data = skb->data + hdrlen;
        *data_len = skb->len - hdrlen;
-        a4_included = (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
+        if (ieee80211_is_data_qos(fc))
-                (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS);
+                *qos_tid = (*ieee80211_get_qos_ctl(hdr) & 0x0f) | 0x80;
-        if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA &&
+        else
-            fc & IEEE80211_STYPE_QOS_DATA) {
-                pos = (u8 *) &hdr->addr4;
-                if (a4_included)
-                        pos += 6;
-                *qos_tid = pos[0] & 0x0f;
-                *qos_tid |= 0x80; /* qos_included flag */
-        } else
                *qos_tid = 0;
        return skb->len < hdrlen ? -1 : 0;
@@ -186,8 +162,8 @@ static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
        struct ieee80211_key *key = tx->key;
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-        int hdrlen, len, tail;
+        unsigned int hdrlen;
-        u16 fc;
+        int len, tail;
        u8 *pos;
        info->control.icv_len = TKIP_ICV_LEN;
@@ -200,8 +176,7 @@ static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
                return 0;
        }
-        fc = le16_to_cpu(hdr->frame_control);
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
-        hdrlen = ieee80211_get_hdrlen(fc);
        len = skb->len - hdrlen;
        if (tx->key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE)
@@ -223,14 +198,8 @@ static int tkip_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
                key->u.tkip.tx.iv32++;
        if (tx->key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE) {
-                hdr = (struct ieee80211_hdr *)skb->data;
                /* hwaccel - with preallocated room for IV */
-                ieee80211_tkip_add_iv(pos, key,
+                ieee80211_tkip_add_iv(pos, key, key->u.tkip.tx.iv16);
-                                      (u8) (key->u.tkip.tx.iv16 >> 8),
-                                      (u8) (((key->u.tkip.tx.iv16 >> 8) | 0x20) &
-                                            0x7f),
-                                      (u8) key->u.tkip.tx.iv16);
                info->control.hw_key = &tx->key->conf;
                return 0;
@@ -272,14 +241,12 @@ ieee80211_rx_result
 ieee80211_crypto_tkip_decrypt(struct ieee80211_rx_data *rx)
 {
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) rx->skb->data;
-        u16 fc;
        int hdrlen, res, hwaccel = 0, wpa_test = 0;
        struct ieee80211_key *key = rx->key;
        struct sk_buff *skb = rx->skb;
        DECLARE_MAC_BUF(mac);
-        fc = le16_to_cpu(hdr->frame_control);
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
-        hdrlen = ieee80211_get_hdrlen(fc);
        if ((rx->fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)
                return RX_CONTINUE;
@@ -427,7 +394,6 @@ static int ccmp_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
        struct ieee80211_key *key = tx->key;
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        int hdrlen, len, tail;
-        u16 fc;
        u8 *pos, *pn, *b_0, *aad, *scratch;
        int i;
@@ -446,8 +412,7 @@ static int ccmp_encrypt_skb(struct ieee80211_tx_data *tx, struct sk_buff *skb)
        b_0 = scratch + 3 * AES_BLOCK_LEN;
        aad = scratch + 4 * AES_BLOCK_LEN;
-        fc = le16_to_cpu(hdr->frame_control);
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
-        hdrlen = ieee80211_get_hdrlen(fc);
        len = skb->len - hdrlen;
        if (key->flags & KEY_FLAG_UPLOADED_TO_HARDWARE)
@@ -516,7 +481,6 @@ ieee80211_rx_result
 ieee80211_crypto_ccmp_decrypt(struct ieee80211_rx_data *rx)
 {
        struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) rx->skb->data;
-        u16 fc;
        int hdrlen;
        struct ieee80211_key *key = rx->key;
        struct sk_buff *skb = rx->skb;
@@ -524,8 +488,7 @@ ieee80211_crypto_ccmp_decrypt(struct ieee80211_rx_data *rx)
        int data_len;
        DECLARE_MAC_BUF(mac);
-        fc = le16_to_cpu(hdr->frame_control);
+        hdrlen = ieee80211_hdrlen(hdr->frame_control);
-        hdrlen = ieee80211_get_hdrlen(fc);
        if ((rx->fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)
                return RX_CONTINUE;
diff --git a/net/wireless/reg.c b/net/wireless/reg.c
index 185488da2466..855bff4b3250 100644
--- a/net/wireless/reg.c
+++ b/net/wireless/reg.c
@@ -80,6 +80,23 @@ static const struct ieee80211_channel_range ieee80211_JP_channels[] = {
                                     IEEE80211_CHAN_RADAR),
 };
+static const struct ieee80211_channel_range ieee80211_EU_channels[] = {
+        /* IEEE 802.11b/g, channels 1..13 */
+        RANGE_PWR(2412, 2472, 20, 6, 0),
+        /* IEEE 802.11a, channel 36*/
+        RANGE_PWR(5180, 5180, 23, 6, IEEE80211_CHAN_PASSIVE_SCAN),
+        /* IEEE 802.11a, channel 40*/
+        RANGE_PWR(5200, 5200, 23, 6, IEEE80211_CHAN_PASSIVE_SCAN),
+        /* IEEE 802.11a, channel 44*/
+        RANGE_PWR(5220, 5220, 23, 6, IEEE80211_CHAN_PASSIVE_SCAN),
+        /* IEEE 802.11a, channels 48..64 */
+        RANGE_PWR(5240, 5320, 23, 6, IEEE80211_CHAN_NO_IBSS |
+                                     IEEE80211_CHAN_RADAR),
+        /* IEEE 802.11a, channels 100..140 */
+        RANGE_PWR(5500, 5700, 30, 6, IEEE80211_CHAN_NO_IBSS |
+                                     IEEE80211_CHAN_RADAR),
+};
 #define REGDOM(_code)                                                   \
        {                                                               \
                .code = __stringify(_code),                             \
@@ -90,6 +107,7 @@ static const struct ieee80211_channel_range ieee80211_JP_channels[] = {
 static const struct ieee80211_regdomain ieee80211_regdoms[] = {
        REGDOM(US),
        REGDOM(JP),
+        REGDOM(EU),
 };