12 files changed, 428 insertions, 40 deletions

diff --git a/drivers/net/wireless/iwmc3200wifi/cfg80211.c b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
index 2e00a4b389e6..7c4f44a9c3e6 100644
--- a/drivers/net/wireless/iwmc3200wifi/cfg80211.c
+++ b/drivers/net/wireless/iwmc3200wifi/cfg80211.c
@@ -678,6 +678,9 @@ static int iwm_cfg80211_set_txpower(struct wiphy *wiphy,
         case TX_POWER_AUTOMATIC:
                 return 0;
         case TX_POWER_FIXED:
+                if (!test_bit(IWM_STATUS_READY, &iwm->status))
+                        return 0;
+
                 ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
                                               CFG_TX_PWR_LIMIT_USR, dbm * 2);
                 if (ret < 0)
@@ -685,6 +688,7 @@ static int iwm_cfg80211_set_txpower(struct wiphy *wiphy,
 
                 return iwm_tx_power_trigger(iwm);
         default:
+                IWM_ERR(iwm, "Unsupported power type: %d\n", type);
                 return -EOPNOTSUPP;
         }
 
@@ -721,6 +725,33 @@ static int iwm_cfg80211_set_power_mgmt(struct wiphy *wiphy,
                                        CFG_POWER_INDEX, iwm->conf.power_index);
 }
 
+int iwm_cfg80211_set_pmksa(struct wiphy *wiphy, struct net_device *netdev,
+                           struct cfg80211_pmksa *pmksa)
+{
+        struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
+
+        return iwm_send_pmkid_update(iwm, pmksa, IWM_CMD_PMKID_ADD);
+}
+
+int iwm_cfg80211_del_pmksa(struct wiphy *wiphy, struct net_device *netdev,
+                           struct cfg80211_pmksa *pmksa)
+{
+        struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
+
+        return iwm_send_pmkid_update(iwm, pmksa, IWM_CMD_PMKID_DEL);
+}
+
+int iwm_cfg80211_flush_pmksa(struct wiphy *wiphy, struct net_device *netdev)
+{
+        struct iwm_priv *iwm = wiphy_to_iwm(wiphy);
+        struct cfg80211_pmksa pmksa;
+
+        memset(&pmksa, 0, sizeof(struct cfg80211_pmksa));
+
+        return iwm_send_pmkid_update(iwm, &pmksa, IWM_CMD_PMKID_FLUSH);
+}
+
+
 static struct cfg80211_ops iwm_cfg80211_ops = {
         .change_virtual_intf = iwm_cfg80211_change_iface,
         .add_key = iwm_cfg80211_add_key,
@@ -737,6 +768,9 @@ static struct cfg80211_ops iwm_cfg80211_ops = {
         .set_tx_power = iwm_cfg80211_set_txpower,
         .get_tx_power = iwm_cfg80211_get_txpower,
         .set_power_mgmt = iwm_cfg80211_set_power_mgmt,
+        .set_pmksa = iwm_cfg80211_set_pmksa,
+        .del_pmksa = iwm_cfg80211_del_pmksa,
+        .flush_pmksa = iwm_cfg80211_flush_pmksa,
 };
 
 static const u32 cipher_suites[] = {
@@ -782,6 +816,7 @@ struct wireless_dev *iwm_wdev_alloc(int sizeof_bus, struct device *dev)
 
         set_wiphy_dev(wdev->wiphy, dev);
         wdev->wiphy->max_scan_ssids = UMAC_WIFI_IF_PROBE_OPTION_MAX;
+        wdev->wiphy->max_num_pmkids = UMAC_MAX_NUM_PMKIDS;
         wdev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
                                        BIT(NL80211_IFTYPE_ADHOC);
         wdev->wiphy->bands[IEEE80211_BAND_2GHZ] = &iwm_band_2ghz;
diff --git a/drivers/net/wireless/iwmc3200wifi/commands.c b/drivers/net/wireless/iwmc3200wifi/commands.c
index 7e12438551ba..777584d76a88 100644
--- a/drivers/net/wireless/iwmc3200wifi/commands.c
+++ b/drivers/net/wireless/iwmc3200wifi/commands.c
@@ -99,6 +99,10 @@ int iwm_send_wifi_if_cmd(struct iwm_priv *iwm, void *payload, u16 payload_size,
         return ret;
 }
 
+static int modparam_wiwi = COEX_MODE_CM;
+module_param_named(wiwi, modparam_wiwi, int, 0644);
+MODULE_PARM_DESC(wiwi, "Wifi-WiMAX coexistence: 1=SA, 2=XOR, 3=CM (default)");
+
 static struct coex_event iwm_sta_xor_prio_tbl[COEX_EVENTS_NUM] =
 {
         {4, 3, 0, COEX_UNASSOC_IDLE_FLAGS},
@@ -122,18 +126,18 @@ static struct coex_event iwm_sta_xor_prio_tbl[COEX_EVENTS_NUM] =
 static struct coex_event iwm_sta_cm_prio_tbl[COEX_EVENTS_NUM] =
 {
         {1, 1, 0, COEX_UNASSOC_IDLE_FLAGS},
-        {4, 3, 0, COEX_UNASSOC_MANUAL_SCAN_FLAGS},
+        {4, 4, 0, COEX_UNASSOC_MANUAL_SCAN_FLAGS},
         {3, 3, 0, COEX_UNASSOC_AUTO_SCAN_FLAGS},
-        {5, 5, 0, COEX_CALIBRATION_FLAGS},
+        {6, 6, 0, COEX_CALIBRATION_FLAGS},
         {3, 3, 0, COEX_PERIODIC_CALIBRATION_FLAGS},
-        {5, 4, 0, COEX_CONNECTION_ESTAB_FLAGS},
+        {6, 5, 0, COEX_CONNECTION_ESTAB_FLAGS},
         {4, 4, 0, COEX_ASSOCIATED_IDLE_FLAGS},
         {4, 4, 0, COEX_ASSOC_MANUAL_SCAN_FLAGS},
         {4, 4, 0, COEX_ASSOC_AUTO_SCAN_FLAGS},
         {4, 4, 0, COEX_ASSOC_ACTIVE_LEVEL_FLAGS},
         {1, 1, 0, COEX_RF_ON_FLAGS},
         {1, 1, 0, COEX_RF_OFF_FLAGS},
-        {6, 6, 0, COEX_STAND_ALONE_DEBUG_FLAGS},
+        {7, 7, 0, COEX_STAND_ALONE_DEBUG_FLAGS},
         {5, 4, 0, COEX_IPAN_ASSOC_LEVEL_FLAGS},
         {1, 1, 0, COEX_RSRVD1_FLAGS},
         {1, 1, 0, COEX_RSRVD2_FLAGS}
@@ -148,7 +152,7 @@ int iwm_send_prio_table(struct iwm_priv *iwm)
 
         coex_table_cmd.flags = COEX_FLAGS_STA_TABLE_VALID_MSK;
 
-        switch (iwm->conf.coexist_mode) {
+        switch (modparam_wiwi) {
         case COEX_MODE_XOR:
         case COEX_MODE_CM:
                 coex_enabled = 1;
@@ -173,7 +177,7 @@ int iwm_send_prio_table(struct iwm_priv *iwm)
                                         COEX_FLAGS_ASSOC_WAKEUP_UMASK_MSK |
                                         COEX_FLAGS_UNASSOC_WAKEUP_UMASK_MSK;
 
-                switch (iwm->conf.coexist_mode) {
+                switch (modparam_wiwi) {
                 case COEX_MODE_XOR:
                         memcpy(coex_table_cmd.sta_prio, iwm_sta_xor_prio_tbl,
                                sizeof(iwm_sta_xor_prio_tbl));
@@ -184,7 +188,7 @@ int iwm_send_prio_table(struct iwm_priv *iwm)
                         break;
                 default:
                         IWM_ERR(iwm, "Invalid coex_mode 0x%x\n",
-                                iwm->conf.coexist_mode);
+                                modparam_wiwi);
                         break;
                 }
         } else
@@ -192,7 +196,7 @@ int iwm_send_prio_table(struct iwm_priv *iwm)
 
         return iwm_send_lmac_ptrough_cmd(iwm, COEX_PRIORITY_TABLE_CMD,
                                 &coex_table_cmd,
-                                sizeof(struct iwm_coex_prio_table_cmd), 1);
+                                sizeof(struct iwm_coex_prio_table_cmd), 0);
 }
 
 int iwm_send_init_calib_cfg(struct iwm_priv *iwm, u8 calib_requested)
@@ -396,7 +400,7 @@ int iwm_send_umac_config(struct iwm_priv *iwm, __le32 reset_flags)
                 return ret;
 
         ret = iwm_umac_set_config_fix(iwm, UMAC_PARAM_TBL_CFG_FIX,
-                                      CFG_COEX_MODE, iwm->conf.coexist_mode);
+                                      CFG_COEX_MODE, modparam_wiwi);
         if (ret < 0)
                 return ret;
 
@@ -929,3 +933,58 @@ int iwm_target_reset(struct iwm_priv *iwm)
 
         return iwm_hal_send_target_cmd(iwm, &target_cmd, NULL);
 }
+
+int iwm_send_umac_stop_resume_tx(struct iwm_priv *iwm,
+                                 struct iwm_umac_notif_stop_resume_tx *ntf)
+{
+        struct iwm_udma_wifi_cmd udma_cmd = UDMA_UMAC_INIT;
+        struct iwm_umac_cmd umac_cmd;
+        struct iwm_umac_cmd_stop_resume_tx stp_res_cmd;
+        struct iwm_sta_info *sta_info;
+        u8 sta_id = STA_ID_N_COLOR_ID(ntf->sta_id);
+        int i;
+
+        sta_info = &iwm->sta_table[sta_id];
+        if (!sta_info->valid) {
+                IWM_ERR(iwm, "Invalid STA: %d\n", sta_id);
+                return -EINVAL;
+        }
+
+        umac_cmd.id = UMAC_CMD_OPCODE_STOP_RESUME_STA_TX;
+        umac_cmd.resp = 0;
+
+        stp_res_cmd.flags = ntf->flags;
+        stp_res_cmd.sta_id = ntf->sta_id;
+        stp_res_cmd.stop_resume_tid_msk = ntf->stop_resume_tid_msk;
+        for (i = 0; i < IWM_UMAC_TID_NR; i++)
+                stp_res_cmd.last_seq_num[i] =
+                        sta_info->tid_info[i].last_seq_num;
+
+        return iwm_hal_send_umac_cmd(iwm, &udma_cmd, &umac_cmd, &stp_res_cmd,
+                                 sizeof(struct iwm_umac_cmd_stop_resume_tx));
+
+}
+
+int iwm_send_pmkid_update(struct iwm_priv *iwm,
+                          struct cfg80211_pmksa *pmksa, u32 command)
+{
+        struct iwm_umac_pmkid_update update;
+        int ret;
+
+        memset(&update, 0, sizeof(struct iwm_umac_pmkid_update));
+
+        update.command = cpu_to_le32(command);
+        if (pmksa->bssid)
+                memcpy(&update.bssid, pmksa->bssid, ETH_ALEN);
+        if (pmksa->pmkid)
+                memcpy(&update.pmkid, pmksa->pmkid, WLAN_PMKID_LEN);
+
+        ret = iwm_send_wifi_if_cmd(iwm, &update,
+                                   sizeof(struct iwm_umac_pmkid_update), 0);
+        if (ret) {
+                IWM_ERR(iwm, "PMKID update command failed\n");
+                return ret;
+        }
+
+        return 0;
+}
diff --git a/drivers/net/wireless/iwmc3200wifi/commands.h b/drivers/net/wireless/iwmc3200wifi/commands.h
index b36be2b23a3c..06af0552cd75 100644
--- a/drivers/net/wireless/iwmc3200wifi/commands.h
+++ b/drivers/net/wireless/iwmc3200wifi/commands.h
@@ -450,6 +450,25 @@ struct iwm_umac_cmd_stats_req {
         __le32 flags;
 } __attribute__ ((packed));
 
+struct iwm_umac_cmd_stop_resume_tx {
+        u8 flags;
+        u8 sta_id;
+        __le16 stop_resume_tid_msk;
+        __le16 last_seq_num[IWM_UMAC_TID_NR];
+        u16 reserved;
+} __attribute__ ((packed));
+
+#define IWM_CMD_PMKID_ADD   1
+#define IWM_CMD_PMKID_DEL   2
+#define IWM_CMD_PMKID_FLUSH 3
+
+struct iwm_umac_pmkid_update {
+        __le32 command;
+        u8 bssid[ETH_ALEN];
+        __le16 reserved;
+        u8 pmkid[WLAN_PMKID_LEN];
+} __attribute__ ((packed));
+
 /* LMAC commands */
 int iwm_read_mac(struct iwm_priv *iwm, u8 *mac);
 int iwm_send_prio_table(struct iwm_priv *iwm);
@@ -478,6 +497,10 @@ int iwm_send_umac_channel_list(struct iwm_priv *iwm);
 int iwm_scan_ssids(struct iwm_priv *iwm, struct cfg80211_ssid *ssids,
                    int ssid_num);
 int iwm_scan_one_ssid(struct iwm_priv *iwm, u8 *ssid, int ssid_len);
+int iwm_send_umac_stop_resume_tx(struct iwm_priv *iwm,
+                                 struct iwm_umac_notif_stop_resume_tx *ntf);
+int iwm_send_pmkid_update(struct iwm_priv *iwm,
+                          struct cfg80211_pmksa *pmksa, u32 command);
 
 /* UDMA commands */
 int iwm_target_reset(struct iwm_priv *iwm);
diff --git a/drivers/net/wireless/iwmc3200wifi/debugfs.c b/drivers/net/wireless/iwmc3200wifi/debugfs.c
index 1465379f900a..be992ca41cf1 100644
--- a/drivers/net/wireless/iwmc3200wifi/debugfs.c
+++ b/drivers/net/wireless/iwmc3200wifi/debugfs.c
@@ -158,6 +158,29 @@ static ssize_t iwm_debugfs_txq_read(struct file *filp, char __user *buffer,
                 }
 
                 spin_unlock_irqrestore(&txq->queue.lock, flags);
+
+                spin_lock_irqsave(&txq->stopped_queue.lock, flags);
+
+                len += snprintf(buf + len, buf_len - len,
+                                "\tStopped Queue len:   %d\n",
+                                skb_queue_len(&txq->stopped_queue));
+                for (j = 0; j < skb_queue_len(&txq->stopped_queue); j++) {
+                        struct iwm_tx_info *tx_info;
+
+                        skb = skb->next;
+                        tx_info = skb_to_tx_info(skb);
+
+                        len += snprintf(buf + len, buf_len - len,
+                                        "\tSKB #%d\n", j);
+                        len += snprintf(buf + len, buf_len - len,
+                                        "\t\tsta:   %d\n", tx_info->sta);
+                        len += snprintf(buf + len, buf_len - len,
+                                        "\t\tcolor: %d\n", tx_info->color);
+                        len += snprintf(buf + len, buf_len - len,
+                                        "\t\ttid:   %d\n", tx_info->tid);
+                }
+
+                spin_unlock_irqrestore(&txq->stopped_queue.lock, flags);
         }
 
         ret = simple_read_from_buffer(buffer, len, ppos, buf, buf_len);
diff --git a/drivers/net/wireless/iwmc3200wifi/eeprom.c b/drivers/net/wireless/iwmc3200wifi/eeprom.c
index 365910fbe01e..8091421ee5e5 100644
--- a/drivers/net/wireless/iwmc3200wifi/eeprom.c
+++ b/drivers/net/wireless/iwmc3200wifi/eeprom.c
@@ -66,6 +66,10 @@ static struct iwm_eeprom_entry eeprom_map[] = {
         [IWM_EEPROM_SKU_CAP] =
         {"SKU capabilities", IWM_EEPROM_SKU_CAP_OFF, IWM_EEPROM_SKU_CAP_LEN},
 
+        [IWM_EEPROM_FAT_CHANNELS_CAP] =
+        {"HT channels capabilities", IWM_EEPROM_FAT_CHANNELS_CAP_OFF,
+         IWM_EEPROM_FAT_CHANNELS_CAP_LEN},
+
         [IWM_EEPROM_CALIB_RXIQ_OFFSET] =
         {"RX IQ offset", IWM_EEPROM_CALIB_RXIQ_OFF, IWM_EEPROM_INDIRECT_LEN},
 
@@ -146,6 +150,52 @@ u8 *iwm_eeprom_access(struct iwm_priv *iwm, u8 eeprom_id)
         return iwm->eeprom + eeprom_map[eeprom_id].offset;
 }
 
+int iwm_eeprom_fat_channels(struct iwm_priv *iwm)
+{
+        struct wiphy *wiphy = iwm_to_wiphy(iwm);
+        struct ieee80211_supported_band *band;
+        u16 *channels, i;
+
+        channels = (u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_FAT_CHANNELS_CAP);
+        if (IS_ERR(channels))
+                return PTR_ERR(channels);
+
+        band = wiphy->bands[IEEE80211_BAND_2GHZ];
+        band->ht_cap.ht_supported = true;
+
+        for (i = 0; i < IWM_EEPROM_FAT_CHANNELS_24; i++)
+                if (!(channels[i] & IWM_EEPROM_FAT_CHANNEL_ENABLED))
+                        band->ht_cap.ht_supported = false;
+
+        band = wiphy->bands[IEEE80211_BAND_5GHZ];
+        band->ht_cap.ht_supported = true;
+        for (i = IWM_EEPROM_FAT_CHANNELS_24; i < IWM_EEPROM_FAT_CHANNELS; i++)
+                if (!(channels[i] & IWM_EEPROM_FAT_CHANNEL_ENABLED))
+                        band->ht_cap.ht_supported = false;
+
+        return 0;
+}
+
+u32 iwm_eeprom_wireless_mode(struct iwm_priv *iwm)
+{
+        u16 sku_cap;
+        u32 wireless_mode = 0;
+
+        sku_cap = *((u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_SKU_CAP));
+
+        if (sku_cap & IWM_EEPROM_SKU_CAP_BAND_24GHZ)
+                wireless_mode |= WIRELESS_MODE_11G;
+
+        if (sku_cap & IWM_EEPROM_SKU_CAP_BAND_52GHZ)
+                wireless_mode |= WIRELESS_MODE_11A;
+
+        if (sku_cap & IWM_EEPROM_SKU_CAP_11N_ENABLE)
+                wireless_mode |= WIRELESS_MODE_11N;
+
+        return wireless_mode;
+}
+
+
 int iwm_eeprom_init(struct iwm_priv *iwm)
 {
         int i, ret = 0;
diff --git a/drivers/net/wireless/iwmc3200wifi/eeprom.h b/drivers/net/wireless/iwmc3200wifi/eeprom.h
index cdb31a6a1f5f..4e3a3fdab0d3 100644
--- a/drivers/net/wireless/iwmc3200wifi/eeprom.h
+++ b/drivers/net/wireless/iwmc3200wifi/eeprom.h
@@ -48,6 +48,7 @@ enum {
         IWM_EEPROM_CARD_ID,
         IWM_EEPROM_RADIO_CONF,
         IWM_EEPROM_SKU_CAP,
+        IWM_EEPROM_FAT_CHANNELS_CAP,
 
         IWM_EEPROM_INDIRECT_OFFSET,
         IWM_EEPROM_CALIB_RXIQ_OFFSET = IWM_EEPROM_INDIRECT_OFFSET,
@@ -58,14 +59,15 @@ enum {
         IWM_EEPROM_LAST,
 };
 
-#define IWM_EEPROM_SIG_OFF             0x00
-#define IWM_EEPROM_VERSION_OFF        (0x54 << 1)
-#define IWM_EEPROM_OEM_HW_VERSION_OFF (0x56 << 1)
-#define IWM_EEPROM_MAC_VERSION_OFF    (0x30 << 1)
-#define IWM_EEPROM_CARD_ID_OFF        (0x5d << 1)
-#define IWM_EEPROM_RADIO_CONF_OFF     (0x58 << 1)
-#define IWM_EEPROM_SKU_CAP_OFF        (0x55 << 1)
-#define IWM_EEPROM_CALIB_CONFIG_OFF   (0x7c << 1)
+#define IWM_EEPROM_SIG_OFF                 0x00
+#define IWM_EEPROM_VERSION_OFF            (0x54 << 1)
+#define IWM_EEPROM_OEM_HW_VERSION_OFF     (0x56 << 1)
+#define IWM_EEPROM_MAC_VERSION_OFF        (0x30 << 1)
+#define IWM_EEPROM_CARD_ID_OFF            (0x5d << 1)
+#define IWM_EEPROM_RADIO_CONF_OFF         (0x58 << 1)
+#define IWM_EEPROM_SKU_CAP_OFF            (0x55 << 1)
+#define IWM_EEPROM_CALIB_CONFIG_OFF       (0x7c << 1)
+#define IWM_EEPROM_FAT_CHANNELS_CAP_OFF   (0xde << 1)
 
 #define IWM_EEPROM_SIG_LEN              4
 #define IWM_EEPROM_VERSION_LEN          2
@@ -74,6 +76,7 @@ enum {
 #define IWM_EEPROM_CARD_ID_LEN          2
 #define IWM_EEPROM_RADIO_CONF_LEN       2
 #define IWM_EEPROM_SKU_CAP_LEN          2
+#define IWM_EEPROM_FAT_CHANNELS_CAP_LEN 40
 #define IWM_EEPROM_INDIRECT_LEN         2
 
 #define IWM_MAX_EEPROM_DATA_LEN         240
@@ -87,6 +90,14 @@ enum {
 #define IWM_EEPROM_SKU_CAP_BAND_52GHZ           (1 << 5)
 #define IWM_EEPROM_SKU_CAP_11N_ENABLE           (1 << 6)
 
+#define IWM_EEPROM_FAT_CHANNELS 20
+/* 2.4 gHz FAT primary channels: 1, 2, 3, 4, 5, 6, 7, 8, 9 */
+#define IWM_EEPROM_FAT_CHANNELS_24 9
+/* 5.2 gHz FAT primary channels: 36,44,52,60,100,108,116,124,132,149,157 */
+#define IWM_EEPROM_FAT_CHANNELS_52 11
+
+#define IWM_EEPROM_FAT_CHANNEL_ENABLED (1 << 0)
+
 enum {
         IWM_EEPROM_CALIB_CAL_HDR,
         IWM_EEPROM_CALIB_TX_POWER,
@@ -110,5 +121,7 @@ struct iwm_eeprom_entry {
 int iwm_eeprom_init(struct iwm_priv *iwm);
 void iwm_eeprom_exit(struct iwm_priv *iwm);
 u8 *iwm_eeprom_access(struct iwm_priv *iwm, u8 eeprom_id);
+int iwm_eeprom_fat_channels(struct iwm_priv *iwm);
+u32 iwm_eeprom_wireless_mode(struct iwm_priv *iwm);
 
 #endif
diff --git a/drivers/net/wireless/iwmc3200wifi/iwm.h b/drivers/net/wireless/iwmc3200wifi/iwm.h
index a9bf6bc97bea..5a26bb05a33a 100644
--- a/drivers/net/wireless/iwmc3200wifi/iwm.h
+++ b/drivers/net/wireless/iwmc3200wifi/iwm.h
@@ -81,7 +81,6 @@ struct iwm_conf {
         u32 assoc_timeout;
         u32 roam_timeout;
         u32 wireless_mode;
-        u32 coexist_mode;
 
         u8 ibss_band;
         u8 ibss_channel;
@@ -131,11 +130,18 @@ struct iwm_notif {
         unsigned long buf_size;
 };
 
+struct iwm_tid_info {
+        __le16 last_seq_num;
+        bool stopped;
+        struct mutex mutex;
+};
+
 struct iwm_sta_info {
         u8 addr[ETH_ALEN];
         bool valid;
         bool qos;
         u8 color;
+        struct iwm_tid_info tid_info[IWM_UMAC_TID_NR];
 };
 
 struct iwm_tx_info {
@@ -185,6 +191,8 @@ struct iwm_key {
 struct iwm_tx_queue {
         int id;
         struct sk_buff_head queue;
+        struct sk_buff_head stopped_queue;
+        spinlock_t lock;
         struct workqueue_struct *wq;
         struct work_struct worker;
         u8 concat_buf[IWM_HAL_CONCATENATE_BUF_SIZE];
@@ -341,6 +349,7 @@ int iwm_up(struct iwm_priv *iwm);
 int iwm_down(struct iwm_priv *iwm);
 
 /* TX API */
+u16 iwm_tid_to_queue(u16 tid);
 void iwm_tx_credit_inc(struct iwm_priv *iwm, int id, int total_freed_pages);
 void iwm_tx_worker(struct work_struct *work);
 int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev);
diff --git a/drivers/net/wireless/iwmc3200wifi/main.c b/drivers/net/wireless/iwmc3200wifi/main.c
index 75f105a59543..7f34d6dd3c41 100644
--- a/drivers/net/wireless/iwmc3200wifi/main.c
+++ b/drivers/net/wireless/iwmc3200wifi/main.c
@@ -68,7 +68,6 @@ static struct iwm_conf def_iwm_conf = {
         .ct_kill_exit           = 110,
         .reset_on_fatal_err     = 1,
         .auto_connect           = 1,
-        .wimax_not_present      = 0,
         .enable_qos             = 1,
         .mode                   = UMAC_MODE_BSS,
 
@@ -80,8 +79,8 @@ static struct iwm_conf def_iwm_conf = {
 
         .assoc_timeout          = 2,
         .roam_timeout           = 10,
-        .wireless_mode          = WIRELESS_MODE_11A | WIRELESS_MODE_11G,
-        .coexist_mode           = COEX_MODE_CM,
+        .wireless_mode          = WIRELESS_MODE_11A | WIRELESS_MODE_11G |
+                                  WIRELESS_MODE_11N,
 
         /* IBSS */
         .ibss_band              = UMAC_BAND_2GHZ,
@@ -94,6 +93,10 @@ static int modparam_reset;
 module_param_named(reset, modparam_reset, bool, 0644);
 MODULE_PARM_DESC(reset, "reset on firmware errors (default 0 [not reset])");
 
+static int modparam_wimax_enable = 1;
+module_param_named(wimax_enable, modparam_wimax_enable, bool, 0644);
+MODULE_PARM_DESC(wimax_enable, "Enable wimax core (default 1 [wimax enabled])");
+
 int iwm_mode_to_nl80211_iftype(int mode)
 {
         switch (mode) {
@@ -247,7 +250,7 @@ static void iwm_watchdog(unsigned long data)
 
 int iwm_priv_init(struct iwm_priv *iwm)
 {
-        int i;
+        int i, j;
         char name[32];
 
         iwm->status = 0;
@@ -291,6 +294,8 @@ int iwm_priv_init(struct iwm_priv *iwm)
                         return -EAGAIN;
 
                 skb_queue_head_init(&iwm->txq[i].queue);
+                skb_queue_head_init(&iwm->txq[i].stopped_queue);
+                spin_lock_init(&iwm->txq[i].lock);
         }
 
         for (i = 0; i < IWM_NUM_KEYS; i++)
@@ -298,6 +303,12 @@ int iwm_priv_init(struct iwm_priv *iwm)
 
         iwm->default_key = -1;
 
+        for (i = 0; i < IWM_STA_TABLE_NUM; i++)
+                for (j = 0; j < IWM_UMAC_TID_NR; j++) {
+                        mutex_init(&iwm->sta_table[i].tid_info[j].mutex);
+                        iwm->sta_table[i].tid_info[j].stopped = false;
+                }
+
         init_timer(&iwm->watchdog);
         iwm->watchdog.function = iwm_watchdog;
         iwm->watchdog.data = (unsigned long)iwm;
@@ -478,7 +489,7 @@ static int iwm_config_boot_params(struct iwm_priv *iwm)
         int ret;
 
         /* check Wimax is off and config debug monitor */
-        if (iwm->conf.wimax_not_present) {
+        if (!modparam_wimax_enable) {
                 u32 data1 = 0x1f;
                 u32 addr1 = 0x606BE258;
 
@@ -571,6 +582,7 @@ void iwm_link_off(struct iwm_priv *iwm)
 
         for (i = 0; i < IWM_TX_QUEUES; i++) {
                 skb_queue_purge(&iwm->txq[i].queue);
+                skb_queue_purge(&iwm->txq[i].stopped_queue);
 
                 iwm->txq[i].concat_count = 0;
                 iwm->txq[i].concat_ptr = iwm->txq[i].concat_buf;
@@ -630,6 +642,7 @@ static int __iwm_up(struct iwm_priv *iwm)
         int ret;
         struct iwm_notif *notif_reboot, *notif_ack = NULL;
         struct wiphy *wiphy = iwm_to_wiphy(iwm);
+        u32 wireless_mode;
 
         ret = iwm_bus_enable(iwm);
         if (ret) {
@@ -691,6 +704,27 @@ static int __iwm_up(struct iwm_priv *iwm)
                 goto err_disable;
         }
 
+        ret = iwm_eeprom_fat_channels(iwm);
+        if (ret) {
+                IWM_ERR(iwm, "Couldnt read HT channels EEPROM entries\n");
+                goto err_fw;
+        }
+
+        /*
+         * Read our SKU capabilities.
+         * If it's valid, we AND the configured wireless mode with the
+         * device EEPROM value as the current profile wireless mode.
+         */
+        wireless_mode = iwm_eeprom_wireless_mode(iwm);
+        if (wireless_mode) {
+                iwm->conf.wireless_mode &= wireless_mode;
+                if (iwm->umac_profile)
+                        iwm->umac_profile->wireless_mode =
+                                        iwm->conf.wireless_mode;
+        } else
+                IWM_ERR(iwm, "Wrong SKU capabilities: 0x%x\n",
+                        *((u16 *)iwm_eeprom_access(iwm, IWM_EEPROM_SKU_CAP)));
+
         snprintf(wiphy->fw_version, sizeof(wiphy->fw_version), "L%s_U%s",
                  iwm->lmac_version, iwm->umac_version);
 
diff --git a/drivers/net/wireless/iwmc3200wifi/netdev.c b/drivers/net/wireless/iwmc3200wifi/netdev.c
index 4f8dbdd7b917..e4f0f8705f65 100644
--- a/drivers/net/wireless/iwmc3200wifi/netdev.c
+++ b/drivers/net/wireless/iwmc3200wifi/netdev.c
@@ -76,6 +76,14 @@ static int iwm_stop(struct net_device *ndev)
  */
 static const u16 iwm_1d_to_queue[8] = { 1, 0, 0, 1, 2, 2, 3, 3 };
 
+u16 iwm_tid_to_queue(u16 tid)
+{
+        if (tid > IWM_UMAC_TID_NR - 2)
+                return -EINVAL;
+
+        return iwm_1d_to_queue[tid];
+}
+
 static u16 iwm_select_queue(struct net_device *dev, struct sk_buff *skb)
 {
         skb->priority = cfg80211_classify8021d(skb);
diff --git a/drivers/net/wireless/iwmc3200wifi/rx.c b/drivers/net/wireless/iwmc3200wifi/rx.c
index bdb1d7e7979d..72c27a3e5528 100644
--- a/drivers/net/wireless/iwmc3200wifi/rx.c
+++ b/drivers/net/wireless/iwmc3200wifi/rx.c
@@ -1087,6 +1087,71 @@ static int iwm_ntf_channel_info_list(struct iwm_priv *iwm, u8 *buf,
         return 0;
 }
 
+static int iwm_ntf_stop_resume_tx(struct iwm_priv *iwm, u8 *buf,
+                                  unsigned long buf_size,
+                                  struct iwm_wifi_cmd *cmd)
+{
+        struct iwm_umac_notif_stop_resume_tx *stp_res_tx =
+                (struct iwm_umac_notif_stop_resume_tx *)buf;
+        struct iwm_sta_info *sta_info;
+        struct iwm_tid_info *tid_info;
+        u8 sta_id = STA_ID_N_COLOR_ID(stp_res_tx->sta_id);
+        u16 tid_msk = le16_to_cpu(stp_res_tx->stop_resume_tid_msk);
+        int bit, ret = 0;
+        bool stop = false;
+
+        IWM_DBG_NTF(iwm, DBG, "stop/resume notification:\n"
+                    "\tflags:       0x%x\n"
+                    "\tSTA id:      %d\n"
+                    "\tTID bitmask: 0x%x\n",
+                    stp_res_tx->flags, stp_res_tx->sta_id,
+                    stp_res_tx->stop_resume_tid_msk);
+
+        if (stp_res_tx->flags & UMAC_STOP_TX_FLAG)
+                stop = true;
+
+        sta_info = &iwm->sta_table[sta_id];
+        if (!sta_info->valid) {
+                IWM_ERR(iwm, "Stoping an invalid STA: %d %d\n",
+                        sta_id, stp_res_tx->sta_id);
+                return -EINVAL;
+        }
+
+        for_each_bit(bit, (unsigned long *)&tid_msk, IWM_UMAC_TID_NR) {
+                tid_info = &sta_info->tid_info[bit];
+
+                mutex_lock(&tid_info->mutex);
+                tid_info->stopped = stop;
+                mutex_unlock(&tid_info->mutex);
+
+                if (!stop) {
+                        struct iwm_tx_queue *txq;
+                        u16 queue = iwm_tid_to_queue(bit);
+
+                        if (queue < 0)
+                                continue;
+
+                        txq = &iwm->txq[queue];
+                        /*
+                         * If we resume, we have to move our SKBs
+                         * back to the tx queue and queue some work.
+                         */
+                        spin_lock_bh(&txq->lock);
+                        skb_queue_splice_init(&txq->queue, &txq->stopped_queue);
+                        spin_unlock_bh(&txq->lock);
+
+                        queue_work(txq->wq, &txq->worker);
+                }
+
+        }
+
+        /* We send an ACK only for the stop case */
+        if (stop)
+                ret = iwm_send_umac_stop_resume_tx(iwm, stp_res_tx);
+
+        return ret;
+}
+
 static int iwm_ntf_wifi_if_wrapper(struct iwm_priv *iwm, u8 *buf,
                                    unsigned long buf_size,
                                    struct iwm_wifi_cmd *cmd)
@@ -1371,6 +1436,7 @@ static const iwm_handler iwm_umac_handlers[] =
         [UMAC_NOTIFY_OPCODE_STATS]              = iwm_ntf_statistics,
         [UMAC_CMD_OPCODE_EEPROM_PROXY]          = iwm_ntf_eeprom_proxy,
         [UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST]    = iwm_ntf_channel_info_list,
+        [UMAC_CMD_OPCODE_STOP_RESUME_STA_TX]    = iwm_ntf_stop_resume_tx,
         [REPLY_RX_MPDU_CMD]                     = iwm_ntf_rx_packet,
         [UMAC_CMD_OPCODE_WIFI_IF_WRAPPER]       = iwm_ntf_wifi_if_wrapper,
 };
diff --git a/drivers/net/wireless/iwmc3200wifi/tx.c b/drivers/net/wireless/iwmc3200wifi/tx.c
index e3b4f7902daf..55905f02309c 100644
--- a/drivers/net/wireless/iwmc3200wifi/tx.c
+++ b/drivers/net/wireless/iwmc3200wifi/tx.c
@@ -329,7 +329,7 @@ static int iwm_tx_build_packet(struct iwm_priv *iwm, struct sk_buff *skb,
 
         memcpy(buf + sizeof(*hdr), skb->data, skb->len);
 
-        return 0;
+        return umac_cmd.seq_num;
 }
 
 static int iwm_tx_send_concat_packets(struct iwm_priv *iwm,
@@ -354,16 +354,15 @@ static int iwm_tx_send_concat_packets(struct iwm_priv *iwm,
         return ret;
 }
 
-#define CONFIG_IWM_TX_CONCATENATED 1
-
 void iwm_tx_worker(struct work_struct *work)
 {
         struct iwm_priv *iwm;
         struct iwm_tx_info *tx_info = NULL;
         struct sk_buff *skb;
-        int cmdlen, ret;
         struct iwm_tx_queue *txq;
-        int pool_id;
+        struct iwm_sta_info *sta_info;
+        struct iwm_tid_info *tid_info;
+        int cmdlen, ret, pool_id;
 
         txq = container_of(work, struct iwm_tx_queue, worker);
         iwm = container_of(txq, struct iwm_priv, txq[txq->id]);
@@ -373,19 +372,46 @@ void iwm_tx_worker(struct work_struct *work)
         while (!test_bit(pool_id, &iwm->tx_credit.full_pools_map) &&
                !skb_queue_empty(&txq->queue)) {
 
+                spin_lock_bh(&txq->lock);
                 skb = skb_dequeue(&txq->queue);
+                spin_unlock_bh(&txq->lock);
+
                 tx_info = skb_to_tx_info(skb);
+                sta_info = &iwm->sta_table[tx_info->sta];
+                if (!sta_info->valid) {
+                        IWM_ERR(iwm, "Trying to send a frame to unknown STA\n");
+                        kfree_skb(skb);
+                        continue;
+                }
+
+                tid_info = &sta_info->tid_info[tx_info->tid];
+
+                mutex_lock(&tid_info->mutex);
+
+                /*
+                 * If the RAxTID is stopped, we queue the skb to the stopped
+                 * queue.
+                 * Whenever we'll get a UMAC notification to resume the tx flow
+                 * for this RAxTID, we'll merge back the stopped queue into the
+                 * regular queue. See iwm_ntf_stop_resume_tx() from rx.c.
+                 */
+                if (tid_info->stopped) {
+                        IWM_DBG_TX(iwm, DBG, "%dx%d stopped\n",
+                                   tx_info->sta, tx_info->tid);
+                        spin_lock_bh(&txq->lock);
+                        skb_queue_tail(&txq->stopped_queue, skb);
+                        spin_unlock_bh(&txq->lock);
+
+                        mutex_unlock(&tid_info->mutex);
+                        continue;
+                }
+
                 cmdlen = IWM_UDMA_HDR_LEN + skb->len;
 
                 IWM_DBG_TX(iwm, DBG, "Tx frame on queue %d: skb: 0x%p, sta: "
                            "%d, color: %d\n", txq->id, skb, tx_info->sta,
                            tx_info->color);
 
-#if !CONFIG_IWM_TX_CONCATENATED
-                /* temporarily keep this to comparing the performance */
-                ret = iwm_send_packet(iwm, skb, pool_id);
-#else
-
                 if (txq->concat_count + cmdlen > IWM_HAL_CONCATENATE_BUF_SIZE)
                         iwm_tx_send_concat_packets(iwm, txq);
 
@@ -393,14 +419,21 @@ void iwm_tx_worker(struct work_struct *work)
                 if (ret) {
                         IWM_DBG_TX(iwm, DBG, "not enough tx_credit for queue "
                                    "%d, Tx worker stopped\n", txq->id);
+                        spin_lock_bh(&txq->lock);
                         skb_queue_head(&txq->queue, skb);
+                        spin_unlock_bh(&txq->lock);
+
+                        mutex_unlock(&tid_info->mutex);
                         break;
                 }
 
                 txq->concat_ptr = txq->concat_buf + txq->concat_count;
-                iwm_tx_build_packet(iwm, skb, pool_id, txq->concat_ptr);
+                tid_info->last_seq_num =
+                        iwm_tx_build_packet(iwm, skb, pool_id, txq->concat_ptr);
                 txq->concat_count += ALIGN(cmdlen, 16);
-#endif
+
+                mutex_unlock(&tid_info->mutex);
+
                 kfree_skb(skb);
         }
 
@@ -419,14 +452,14 @@ int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
         struct iwm_priv *iwm = ndev_to_iwm(netdev);
         struct net_device *ndev = iwm_to_ndev(iwm);
         struct wireless_dev *wdev = iwm_to_wdev(iwm);
-        u8 *dst_addr;
         struct iwm_tx_info *tx_info;
         struct iwm_tx_queue *txq;
         struct iwm_sta_info *sta_info;
-        u8 sta_id;
+        u8 *dst_addr, sta_id;
         u16 queue;
         int ret;
 
+
         if (!test_bit(IWM_STATUS_ASSOCIATED, &iwm->status)) {
                 IWM_DBG_TX(iwm, DBG, "LINK: stop netif_all_queues: "
                            "not associated\n");
@@ -440,7 +473,8 @@ int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
         txq = &iwm->txq[queue];
 
         /* No free space for Tx, tx_worker is too slow */
-        if (skb_queue_len(&txq->queue) > IWM_TX_LIST_SIZE) {
+        if ((skb_queue_len(&txq->queue) > IWM_TX_LIST_SIZE) ||
+            (skb_queue_len(&txq->stopped_queue) > IWM_TX_LIST_SIZE)) {
                 IWM_DBG_TX(iwm, DBG, "LINK: stop netif_subqueue[%d]\n", queue);
                 netif_stop_subqueue(netdev, queue);
                 return NETDEV_TX_BUSY;
@@ -477,7 +511,9 @@ int iwm_xmit_frame(struct sk_buff *skb, struct net_device *netdev)
         else
                 tx_info->tid = IWM_UMAC_MGMT_TID;
 
+        spin_lock_bh(&iwm->txq[queue].lock);
         skb_queue_tail(&iwm->txq[queue].queue, skb);
+        spin_unlock_bh(&iwm->txq[queue].lock);
 
         queue_work(iwm->txq[queue].wq, &iwm->txq[queue].worker);
 
diff --git a/drivers/net/wireless/iwmc3200wifi/umac.h b/drivers/net/wireless/iwmc3200wifi/umac.h
index be903543bb47..7f54a145ca65 100644
--- a/drivers/net/wireless/iwmc3200wifi/umac.h
+++ b/drivers/net/wireless/iwmc3200wifi/umac.h
@@ -83,6 +83,20 @@ struct iwm_udma_out_wifi_hdr {
         ((UMAC_HDI_ACT_TBL_IDX_RA_UMAC << UMAC_HDI_ACT_TBL_IDX_RA_POS) |\
         (UMAC_HDI_ACT_TBL_IDX_TID_LMAC << UMAC_HDI_ACT_TBL_IDX_TID_POS))
 
+/* STA ID and color */
+#define STA_ID_SEED                        (0x0f)
+#define STA_ID_POS                         (0)
+#define STA_ID_MSK                         (STA_ID_SEED << STA_ID_POS)
+
+#define STA_COLOR_SEED                     (0x7)
+#define STA_COLOR_POS                      (4)
+#define STA_COLOR_MSK                      (STA_COLOR_SEED << STA_COLOR_POS)
+
+#define STA_ID_N_COLOR_COLOR(id_n_color) \
+        (((id_n_color) & STA_COLOR_MSK) >> STA_COLOR_POS)
+#define STA_ID_N_COLOR_ID(id_n_color) \
+        (((id_n_color) & STA_ID_MSK) >> STA_ID_POS)
+
 /* iwm_umac_notif_alive.page_grp_state Group number -- bits [3:0] */
 #define UMAC_ALIVE_PAGE_STS_GRP_NUM_POS         0
 #define UMAC_ALIVE_PAGE_STS_GRP_NUM_SEED        0xF
@@ -260,6 +274,9 @@ struct iwm_udma_out_wifi_hdr {
 #define UMAC_CMD_OPCODE_GET_CHAN_INFO_LIST      0x16
 #define UMAC_CMD_OPCODE_SET_PARAM_LIST          0x17
 #define UMAC_CMD_OPCODE_GET_PARAM_LIST          0x18
+#define UMAC_CMD_OPCODE_STOP_RESUME_STA_TX      0x19
+#define UMAC_CMD_OPCODE_TEST_BLOCK_ACK          0x1A
+
 #define UMAC_CMD_OPCODE_BASE_WRAPPER            0xFA
 #define UMAC_CMD_OPCODE_LMAC_WRAPPER            0xFB
 #define UMAC_CMD_OPCODE_HW_TEST_WRAPPER         0xFC
@@ -281,6 +298,7 @@ struct iwm_udma_out_wifi_hdr {
 #define UMAC_WIFI_IF_CMD_GLOBAL_TX_KEY_ID                0x1B
 #define UMAC_WIFI_IF_CMD_SET_HOST_EXTENDED_IE            0x1C
 #define UMAC_WIFI_IF_CMD_GET_SUPPORTED_CHANNELS          0x1E
+#define UMAC_WIFI_IF_CMD_PMKID_UPDATE                    0x1F
 #define UMAC_WIFI_IF_CMD_TX_PWR_TRIGGER                  0x20
 
 /* UMAC WiFi interface ports */
@@ -691,13 +709,13 @@ struct iwm_umac_notif_rx_ticket {
 #define UMAC_PHY_NUM_CHAINS     3
 
 #define IWM_UMAC_MGMT_TID       8
-#define IWM_UMAC_TID_NR         8
+#define IWM_UMAC_TID_NR         9 /* 8 TIDs + MGMT */
 
 struct iwm_umac_notif_stats {
         struct iwm_umac_wifi_in_hdr hdr;
         __le32 flags;
         __le32 timestamp;
-        __le16 tid_load[IWM_UMAC_TID_NR + 2]; /* 1 non-QoS + 1 dword align */
+        __le16 tid_load[IWM_UMAC_TID_NR + 1]; /* 1 non-QoS + 1 dword align */
         __le16 tx_rate[UMAC_NTF_RATE_SAMPLE_NR];
         __le16 rx_rate[UMAC_NTF_RATE_SAMPLE_NR];
         __le32 chain_energy[UMAC_PHY_NUM_CHAINS];
@@ -742,6 +760,20 @@ struct iwm_umac_notif_stats {
         __le32 roam_ap_loadblance;
 } __attribute__ ((packed));
 
+#define UMAC_STOP_TX_FLAG    0x1
+#define UMAC_RESUME_TX_FLAG  0x2
+
+#define LAST_SEQ_NUM_INVALID     0xFFFF
+
+struct iwm_umac_notif_stop_resume_tx {
+        struct iwm_umac_wifi_in_hdr hdr;
+        u8 flags; /* UMAC_*_TX_FLAG_* */
+        u8 sta_id;
+        __le16 stop_resume_tid_msk; /* tid bitmask */
+} __attribute__ ((packed));
+
+#define UMAC_MAX_NUM_PMKIDS 4
+
 /* WiFi interface wrapper header */
 struct iwm_umac_wifi_if {
         u8 oid;