1 files changed, 245 insertions, 314 deletions

diff --git a/drivers/net/wireless/wl12xx/wl1251_main.c b/drivers/net/wireless/wl12xx/wl1251_main.c
index 1103256ad989..1c8226eee409 100644
--- a/drivers/net/wireless/wl12xx/wl1251_main.c
+++ b/drivers/net/wireless/wl12xx/wl1251_main.c
@@ -28,6 +28,8 @@
 #include <linux/irq.h>
 #include <linux/crc32.h>
 #include <linux/etherdevice.h>
+#include <linux/vmalloc.h>
+#include <linux/slab.h>
 
 #include "wl1251.h"
 #include "wl12xx_80211.h"
@@ -83,7 +85,7 @@ static int wl1251_fetch_firmware(struct wl1251 *wl)
         }
 
         wl->fw_len = fw->size;
-        wl->fw = kmalloc(wl->fw_len, GFP_KERNEL);
+        wl->fw = vmalloc(wl->fw_len);
 
         if (!wl->fw) {
                 wl1251_error("could not allocate memory for the firmware");
@@ -183,8 +185,11 @@ static int wl1251_chip_wakeup(struct wl1251 *wl)
                 wl1251_debug(DEBUG_BOOT, "chip id 0x%x (1251 PG12)",
                              wl->chip_id);
                 break;
-        case CHIP_ID_1251_PG10:
         case CHIP_ID_1251_PG11:
+                wl1251_debug(DEBUG_BOOT, "chip id 0x%x (1251 PG11)",
+                             wl->chip_id);
+                break;
+        case CHIP_ID_1251_PG10:
         default:
                 wl1251_error("unsupported chip id: 0x%x", wl->chip_id);
                 ret = -ENODEV;
@@ -208,9 +213,10 @@ out:
         return ret;
 }
 
+#define WL1251_IRQ_LOOP_COUNT 10
 static void wl1251_irq_work(struct work_struct *work)
 {
-        u32 intr;
+        u32 intr, ctr = WL1251_IRQ_LOOP_COUNT;
         struct wl1251 *wl =
                 container_of(work, struct wl1251, irq_work);
         int ret;
@@ -231,78 +237,86 @@ static void wl1251_irq_work(struct work_struct *work)
         intr = wl1251_reg_read32(wl, ACX_REG_INTERRUPT_CLEAR);
         wl1251_debug(DEBUG_IRQ, "intr: 0x%x", intr);
 
-        if (wl->data_path) {
-                wl->rx_counter =
-                        wl1251_mem_read32(wl, wl->data_path->rx_control_addr);
-
-                /* We handle a frmware bug here */
-                switch ((wl->rx_counter - wl->rx_handled) & 0xf) {
-                case 0:
-                        wl1251_debug(DEBUG_IRQ, "RX: FW and host in sync");
-                        intr &= ~WL1251_ACX_INTR_RX0_DATA;
-                        intr &= ~WL1251_ACX_INTR_RX1_DATA;
-                        break;
-                case 1:
-                        wl1251_debug(DEBUG_IRQ, "RX: FW +1");
-                        intr |= WL1251_ACX_INTR_RX0_DATA;
-                        intr &= ~WL1251_ACX_INTR_RX1_DATA;
-                        break;
-                case 2:
-                        wl1251_debug(DEBUG_IRQ, "RX: FW +2");
-                        intr |= WL1251_ACX_INTR_RX0_DATA;
-                        intr |= WL1251_ACX_INTR_RX1_DATA;
-                        break;
-                default:
-                        wl1251_warning("RX: FW and host out of sync: %d",
-                                       wl->rx_counter - wl->rx_handled);
-                        break;
-                }
-
-                wl->rx_handled = wl->rx_counter;
+        do {
+                if (wl->data_path) {
+                        wl->rx_counter = wl1251_mem_read32(
+                                wl, wl->data_path->rx_control_addr);
+
+                        /* We handle a frmware bug here */
+                        switch ((wl->rx_counter - wl->rx_handled) & 0xf) {
+                        case 0:
+                                wl1251_debug(DEBUG_IRQ,
+                                             "RX: FW and host in sync");
+                                intr &= ~WL1251_ACX_INTR_RX0_DATA;
+                                intr &= ~WL1251_ACX_INTR_RX1_DATA;
+                                break;
+                        case 1:
+                                wl1251_debug(DEBUG_IRQ, "RX: FW +1");
+                                intr |= WL1251_ACX_INTR_RX0_DATA;
+                                intr &= ~WL1251_ACX_INTR_RX1_DATA;
+                                break;
+                        case 2:
+                                wl1251_debug(DEBUG_IRQ, "RX: FW +2");
+                                intr |= WL1251_ACX_INTR_RX0_DATA;
+                                intr |= WL1251_ACX_INTR_RX1_DATA;
+                                break;
+                        default:
+                                wl1251_warning(
+                                        "RX: FW and host out of sync: %d",
+                                        wl->rx_counter - wl->rx_handled);
+                                break;
+                        }
 
+                        wl->rx_handled = wl->rx_counter;
 
-                wl1251_debug(DEBUG_IRQ, "RX counter: %d", wl->rx_counter);
-        }
+                        wl1251_debug(DEBUG_IRQ, "RX counter: %d",
+                                     wl->rx_counter);
+                }
 
-        intr &= wl->intr_mask;
+                intr &= wl->intr_mask;
 
-        if (intr == 0) {
-                wl1251_debug(DEBUG_IRQ, "INTR is 0");
-                wl1251_reg_write32(wl, ACX_REG_INTERRUPT_MASK,
-                                   ~(wl->intr_mask));
+                if (intr == 0) {
+                        wl1251_debug(DEBUG_IRQ, "INTR is 0");
+                        goto out_sleep;
+                }
 
-                goto out_sleep;
-        }
+                if (intr & WL1251_ACX_INTR_RX0_DATA) {
+                        wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_RX0_DATA");
+                        wl1251_rx(wl);
+                }
 
-        if (intr & WL1251_ACX_INTR_RX0_DATA) {
-                wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_RX0_DATA");
-                wl1251_rx(wl);
-        }
+                if (intr & WL1251_ACX_INTR_RX1_DATA) {
+                        wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_RX1_DATA");
+                        wl1251_rx(wl);
+                }
 
-        if (intr & WL1251_ACX_INTR_RX1_DATA) {
-                wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_RX1_DATA");
-                wl1251_rx(wl);
-        }
+                if (intr & WL1251_ACX_INTR_TX_RESULT) {
+                        wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_TX_RESULT");
+                        wl1251_tx_complete(wl);
+                }
 
-        if (intr & WL1251_ACX_INTR_TX_RESULT) {
-                wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_TX_RESULT");
-                wl1251_tx_complete(wl);
-        }
+                if (intr & (WL1251_ACX_INTR_EVENT_A |
+                            WL1251_ACX_INTR_EVENT_B)) {
+                        wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_EVENT (0x%x)",
+                                     intr);
+                        if (intr & WL1251_ACX_INTR_EVENT_A)
+                                wl1251_event_handle(wl, 0);
+                        else
+                                wl1251_event_handle(wl, 1);
+                }
 
-        if (intr & (WL1251_ACX_INTR_EVENT_A | WL1251_ACX_INTR_EVENT_B)) {
-                wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_EVENT (0x%x)", intr);
-                if (intr & WL1251_ACX_INTR_EVENT_A)
-                        wl1251_event_handle(wl, 0);
-                else
-                        wl1251_event_handle(wl, 1);
-        }
+                if (intr & WL1251_ACX_INTR_INIT_COMPLETE)
+                        wl1251_debug(DEBUG_IRQ,
+                                     "WL1251_ACX_INTR_INIT_COMPLETE");
 
-        if (intr & WL1251_ACX_INTR_INIT_COMPLETE)
-                wl1251_debug(DEBUG_IRQ, "WL1251_ACX_INTR_INIT_COMPLETE");
+                if (--ctr == 0)
+                        break;
 
-        wl1251_reg_write32(wl, ACX_REG_INTERRUPT_MASK, ~(wl->intr_mask));
+                intr = wl1251_reg_read32(wl, ACX_REG_INTERRUPT_CLEAR);
+        } while (intr);
 
 out_sleep:
+        wl1251_reg_write32(wl, ACX_REG_INTERRUPT_MASK, ~(wl->intr_mask));
         wl1251_ps_elp_sleep(wl);
 
 out:
@@ -382,6 +396,7 @@ static int wl1251_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
          * the queue here, otherwise the queue will get too long.
          */
         if (skb_queue_len(&wl->tx_queue) >= WL1251_TX_QUEUE_MAX_LENGTH) {
+                wl1251_debug(DEBUG_TX, "op_tx: tx_queue full, stop queues");
                 ieee80211_stop_queues(wl->hw);
 
                 /*
@@ -497,17 +512,23 @@ static void wl1251_op_stop(struct ieee80211_hw *hw)
 }
 
 static int wl1251_op_add_interface(struct ieee80211_hw *hw,
-                                   struct ieee80211_if_init_conf *conf)
+                                   struct ieee80211_vif *vif)
 {
         struct wl1251 *wl = hw->priv;
         int ret = 0;
 
         wl1251_debug(DEBUG_MAC80211, "mac80211 add interface type %d mac %pM",
-                     conf->type, conf->mac_addr);
+                     vif->type, vif->addr);
 
         mutex_lock(&wl->mutex);
+        if (wl->vif) {
+                ret = -EBUSY;
+                goto out;
+        }
+
+        wl->vif = vif;
 
-        switch (conf->type) {
+        switch (vif->type) {
         case NL80211_IFTYPE_STATION:
                 wl->bss_type = BSS_TYPE_STA_BSS;
                 break;
@@ -519,8 +540,8 @@ static int wl1251_op_add_interface(struct ieee80211_hw *hw,
                 goto out;
         }
 
-        if (memcmp(wl->mac_addr, conf->mac_addr, ETH_ALEN)) {
-                memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
+        if (memcmp(wl->mac_addr, vif->addr, ETH_ALEN)) {
+                memcpy(wl->mac_addr, vif->addr, ETH_ALEN);
                 SET_IEEE80211_PERM_ADDR(wl->hw, wl->mac_addr);
                 ret = wl1251_acx_station_id(wl);
                 if (ret < 0)
@@ -533,44 +554,35 @@ out:
 }
 
 static void wl1251_op_remove_interface(struct ieee80211_hw *hw,
-                                         struct ieee80211_if_init_conf *conf)
+                                         struct ieee80211_vif *vif)
 {
+        struct wl1251 *wl = hw->priv;
+
+        mutex_lock(&wl->mutex);
         wl1251_debug(DEBUG_MAC80211, "mac80211 remove interface");
+        wl->vif = NULL;
+        mutex_unlock(&wl->mutex);
 }
 
-static int wl1251_build_null_data(struct wl1251 *wl)
+static int wl1251_build_qos_null_data(struct wl1251 *wl)
 {
-        struct wl12xx_null_data_template template;
+        struct ieee80211_qos_hdr template;
 
-        if (!is_zero_ether_addr(wl->bssid)) {
-                memcpy(template.header.da, wl->bssid, ETH_ALEN);
-                memcpy(template.header.bssid, wl->bssid, ETH_ALEN);
-        } else {
-                memset(template.header.da, 0xff, ETH_ALEN);
-                memset(template.header.bssid, 0xff, ETH_ALEN);
-        }
-
-        memcpy(template.header.sa, wl->mac_addr, ETH_ALEN);
-        template.header.frame_ctl = cpu_to_le16(IEEE80211_FTYPE_DATA |
-                                                IEEE80211_STYPE_NULLFUNC);
+        memset(&template, 0, sizeof(template));
 
-        return wl1251_cmd_template_set(wl, CMD_NULL_DATA, &template,
-                                       sizeof(template));
-
-}
+        memcpy(template.addr1, wl->bssid, ETH_ALEN);
+        memcpy(template.addr2, wl->mac_addr, ETH_ALEN);
+        memcpy(template.addr3, wl->bssid, ETH_ALEN);
 
-static int wl1251_build_ps_poll(struct wl1251 *wl, u16 aid)
-{
-        struct wl12xx_ps_poll_template template;
+        template.frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+                                             IEEE80211_STYPE_QOS_NULLFUNC |
+                                             IEEE80211_FCTL_TODS);
 
-        memcpy(template.bssid, wl->bssid, ETH_ALEN);
-        memcpy(template.ta, wl->mac_addr, ETH_ALEN);
-        template.aid = aid;
-        template.fc = cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_PSPOLL);
+        /* FIXME: not sure what priority to use here */
+        template.qos_ctrl = cpu_to_le16(0);
 
-        return wl1251_cmd_template_set(wl, CMD_PS_POLL, &template,
+        return wl1251_cmd_template_set(wl, CMD_QOS_NULL_DATA, &template,
                                        sizeof(template));
-
 }
 
 static int wl1251_op_config(struct ieee80211_hw *hw, u32 changed)
@@ -601,35 +613,39 @@ static int wl1251_op_config(struct ieee80211_hw *hw, u32 changed)
                         goto out_sleep;
         }
 
-        ret = wl1251_build_null_data(wl);
-        if (ret < 0)
-                goto out_sleep;
-
         if (conf->flags & IEEE80211_CONF_PS && !wl->psm_requested) {
                 wl1251_debug(DEBUG_PSM, "psm enabled");
 
                 wl->psm_requested = true;
 
+                wl->dtim_period = conf->ps_dtim_period;
+
+                ret = wl1251_acx_wr_tbtt_and_dtim(wl, wl->beacon_int,
+                                                  wl->dtim_period);
+
                 /*
-                 * We enter PSM only if we're already associated.
-                 * If we're not, we'll enter it when joining an SSID,
-                 * through the bss_info_changed() hook.
+                 * mac80211 enables PSM only if we're already associated.
                  */
                 ret = wl1251_ps_set_mode(wl, STATION_POWER_SAVE_MODE);
+                if (ret < 0)
+                        goto out_sleep;
         } else if (!(conf->flags & IEEE80211_CONF_PS) &&
                    wl->psm_requested) {
                 wl1251_debug(DEBUG_PSM, "psm disabled");
 
                 wl->psm_requested = false;
 
-                if (wl->psm)
+                if (wl->psm) {
                         ret = wl1251_ps_set_mode(wl, STATION_ACTIVE_MODE);
+                        if (ret < 0)
+                                goto out_sleep;
+                }
         }
 
         if (conf->power_level != wl->power_level) {
                 ret = wl1251_acx_tx_power(wl, conf->power_level);
                 if (ret < 0)
-                        goto out;
+                        goto out_sleep;
 
                 wl->power_level = conf->power_level;
         }
@@ -840,199 +856,61 @@ out:
         return ret;
 }
 
-static int wl1251_build_basic_rates(char *rates)
-{
-        u8 index = 0;
-
-        rates[index++] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_1MB;
-        rates[index++] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_2MB;
-        rates[index++] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_5MB;
-        rates[index++] = IEEE80211_BASIC_RATE_MASK | IEEE80211_CCK_RATE_11MB;
-
-        return index;
-}
-
-static int wl1251_build_extended_rates(char *rates)
+static int wl1251_op_hw_scan(struct ieee80211_hw *hw,
+                             struct cfg80211_scan_request *req)
 {
-        u8 index = 0;
-
-        rates[index++] = IEEE80211_OFDM_RATE_6MB;
-        rates[index++] = IEEE80211_OFDM_RATE_9MB;
-        rates[index++] = IEEE80211_OFDM_RATE_12MB;
-        rates[index++] = IEEE80211_OFDM_RATE_18MB;
-        rates[index++] = IEEE80211_OFDM_RATE_24MB;
-        rates[index++] = IEEE80211_OFDM_RATE_36MB;
-        rates[index++] = IEEE80211_OFDM_RATE_48MB;
-        rates[index++] = IEEE80211_OFDM_RATE_54MB;
-
-        return index;
-}
-
+        struct wl1251 *wl = hw->priv;
+        struct sk_buff *skb;
+        size_t ssid_len = 0;
+        u8 *ssid = NULL;
+        int ret;
 
-static int wl1251_build_probe_req(struct wl1251 *wl, u8 *ssid, size_t ssid_len)
-{
-        struct wl12xx_probe_req_template template;
-        struct wl12xx_ie_rates *rates;
-        char *ptr;
-        u16 size;
-
-        ptr = (char *)&template;
-        size = sizeof(struct ieee80211_header);
-
-        memset(template.header.da, 0xff, ETH_ALEN);
-        memset(template.header.bssid, 0xff, ETH_ALEN);
-        memcpy(template.header.sa, wl->mac_addr, ETH_ALEN);
-        template.header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-
-        /* IEs */
-        /* SSID */
-        template.ssid.header.id = WLAN_EID_SSID;
-        template.ssid.header.len = ssid_len;
-        if (ssid_len && ssid)
-                memcpy(template.ssid.ssid, ssid, ssid_len);
-        size += sizeof(struct wl12xx_ie_header) + ssid_len;
-        ptr += size;
-
-        /* Basic Rates */
-        rates = (struct wl12xx_ie_rates *)ptr;
-        rates->header.id = WLAN_EID_SUPP_RATES;
-        rates->header.len = wl1251_build_basic_rates(rates->rates);
-        size += sizeof(struct wl12xx_ie_header) + rates->header.len;
-        ptr += sizeof(struct wl12xx_ie_header) + rates->header.len;
-
-        /* Extended rates */
-        rates = (struct wl12xx_ie_rates *)ptr;
-        rates->header.id = WLAN_EID_EXT_SUPP_RATES;
-        rates->header.len = wl1251_build_extended_rates(rates->rates);
-        size += sizeof(struct wl12xx_ie_header) + rates->header.len;
-
-        wl1251_dump(DEBUG_SCAN, "PROBE REQ: ", &template, size);
-
-        return wl1251_cmd_template_set(wl, CMD_PROBE_REQ, &template,
-                                      size);
-}
+        wl1251_debug(DEBUG_MAC80211, "mac80211 hw scan");
 
-static int wl1251_hw_scan(struct wl1251 *wl, u8 *ssid, size_t len,
-                          u8 active_scan, u8 high_prio, u8 num_channels,
-                          u8 probe_requests)
-{
-        struct wl1251_cmd_trigger_scan_to *trigger = NULL;
-        struct cmd_scan *params = NULL;
-        int i, ret;
-        u16 scan_options = 0;
-
-        if (wl->scanning)
-                return -EINVAL;
-
-        params = kzalloc(sizeof(*params), GFP_KERNEL);
-        if (!params)
-                return -ENOMEM;
-
-        params->params.rx_config_options = cpu_to_le32(CFG_RX_ALL_GOOD);
-        params->params.rx_filter_options =
-                cpu_to_le32(CFG_RX_PRSP_EN | CFG_RX_MGMT_EN | CFG_RX_BCN_EN);
-
-        /* High priority scan */
-        if (!active_scan)
-                scan_options |= SCAN_PASSIVE;
-        if (high_prio)
-                scan_options |= SCAN_PRIORITY_HIGH;
-        params->params.scan_options = scan_options;
-
-        params->params.num_channels = num_channels;
-        params->params.num_probe_requests = probe_requests;
-        params->params.tx_rate = cpu_to_le16(1 << 1); /* 2 Mbps */
-        params->params.tid_trigger = 0;
-
-        for (i = 0; i < num_channels; i++) {
-                params->channels[i].min_duration = cpu_to_le32(30000);
-                params->channels[i].max_duration = cpu_to_le32(60000);
-                memset(&params->channels[i].bssid_lsb, 0xff, 4);
-                memset(&params->channels[i].bssid_msb, 0xff, 2);
-                params->channels[i].early_termination = 0;
-                params->channels[i].tx_power_att = 0;
-                params->channels[i].channel = i + 1;
-                memset(params->channels[i].pad, 0, 3);
+        if (req->n_ssids) {
+                ssid = req->ssids[0].ssid;
+                ssid_len = req->ssids[0].ssid_len;
         }
 
-        for (i = num_channels; i < SCAN_MAX_NUM_OF_CHANNELS; i++)
-                memset(&params->channels[i], 0,
-                       sizeof(struct basic_scan_channel_parameters));
-
-        if (len && ssid) {
-                params->params.ssid_len = len;
-                memcpy(params->params.ssid, ssid, len);
-        } else {
-                params->params.ssid_len = 0;
-                memset(params->params.ssid, 0, 32);
-        }
+        mutex_lock(&wl->mutex);
 
-        ret = wl1251_build_probe_req(wl, ssid, len);
-        if (ret < 0) {
-                wl1251_error("PROBE request template failed");
+        if (wl->scanning) {
+                wl1251_debug(DEBUG_SCAN, "scan already in progress");
+                ret = -EINVAL;
                 goto out;
         }
 
-        trigger = kzalloc(sizeof(*trigger), GFP_KERNEL);
-        if (!trigger)
+        ret = wl1251_ps_elp_wakeup(wl);
+        if (ret < 0)
                 goto out;
 
-        trigger->timeout = 0;
-
-        ret = wl1251_cmd_send(wl, CMD_TRIGGER_SCAN_TO, trigger,
-                              sizeof(*trigger));
-        if (ret < 0) {
-                wl1251_error("trigger scan to failed for hw scan");
+        skb = ieee80211_probereq_get(wl->hw, wl->vif, ssid, ssid_len,
+                                     req->ie, req->ie_len);
+        if (!skb) {
+                ret = -ENOMEM;
                 goto out;
         }
 
-        wl1251_dump(DEBUG_SCAN, "SCAN: ", params, sizeof(*params));
-
-        wl->scanning = true;
+        ret = wl1251_cmd_template_set(wl, CMD_PROBE_REQ, skb->data,
+                                      skb->len);
+        dev_kfree_skb(skb);
+        if (ret < 0)
+                goto out_sleep;
 
-        ret = wl1251_cmd_send(wl, CMD_SCAN, params, sizeof(*params));
+        ret = wl1251_cmd_trigger_scan_to(wl, 0);
         if (ret < 0)
-                wl1251_error("SCAN failed");
+                goto out_sleep;
 
-        wl1251_mem_read(wl, wl->cmd_box_addr, params, sizeof(*params));
+        wl->scanning = true;
 
-        if (params->header.status != CMD_STATUS_SUCCESS) {
-                wl1251_error("TEST command answer error: %d",
-                             params->header.status);
+        ret = wl1251_cmd_scan(wl, ssid, ssid_len, req->channels,
+                              req->n_channels, WL1251_SCAN_NUM_PROBES);
+        if (ret < 0) {
                 wl->scanning = false;
-                ret = -EIO;
-                goto out;
-        }
-
-out:
-        kfree(params);
-        return ret;
-
-}
-
-static int wl1251_op_hw_scan(struct ieee80211_hw *hw,
-                             struct cfg80211_scan_request *req)
-{
-        struct wl1251 *wl = hw->priv;
-        int ret;
-        u8 *ssid = NULL;
-        size_t ssid_len = 0;
-
-        wl1251_debug(DEBUG_MAC80211, "mac80211 hw scan");
-
-        if (req->n_ssids) {
-                ssid = req->ssids[0].ssid;
-                ssid_len = req->ssids[0].ssid_len;
+                goto out_sleep;
         }
 
-        mutex_lock(&wl->mutex);
-
-        ret = wl1251_ps_elp_wakeup(wl);
-        if (ret < 0)
-                goto out;
-
-        ret = wl1251_hw_scan(hw->priv, ssid, ssid_len, 1, 0, 13, 3);
-
+out_sleep:
         wl1251_ps_elp_sleep(wl);
 
 out:
@@ -1069,9 +947,8 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
                                        struct ieee80211_bss_conf *bss_conf,
                                        u32 changed)
 {
-        enum wl1251_cmd_ps_mode mode;
         struct wl1251 *wl = hw->priv;
-        struct sk_buff *beacon;
+        struct sk_buff *beacon, *skb;
         int ret;
 
         wl1251_debug(DEBUG_MAC80211, "mac80211 bss info changed");
@@ -1082,30 +959,49 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
         if (ret < 0)
                 goto out;
 
+        if (changed & BSS_CHANGED_BSSID) {
+                memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
+
+                skb = ieee80211_nullfunc_get(wl->hw, wl->vif);
+                if (!skb)
+                        goto out_sleep;
+
+                ret = wl1251_cmd_template_set(wl, CMD_NULL_DATA,
+                                              skb->data, skb->len);
+                dev_kfree_skb(skb);
+                if (ret < 0)
+                        goto out_sleep;
+
+                ret = wl1251_build_qos_null_data(wl);
+                if (ret < 0)
+                        goto out;
+
+                if (wl->bss_type != BSS_TYPE_IBSS) {
+                        ret = wl1251_join(wl, wl->bss_type, wl->channel,
+                                          wl->beacon_int, wl->dtim_period);
+                        if (ret < 0)
+                                goto out_sleep;
+                }
+        }
+
         if (changed & BSS_CHANGED_ASSOC) {
                 if (bss_conf->assoc) {
                         wl->beacon_int = bss_conf->beacon_int;
-                        wl->dtim_period = bss_conf->dtim_period;
-
-                        /* FIXME: call join */
 
-                        wl->aid = bss_conf->aid;
+                        skb = ieee80211_pspoll_get(wl->hw, wl->vif);
+                        if (!skb)
+                                goto out_sleep;
 
-                        ret = wl1251_build_ps_poll(wl, wl->aid);
+                        ret = wl1251_cmd_template_set(wl, CMD_PS_POLL,
+                                                      skb->data,
+                                                      skb->len);
+                        dev_kfree_skb(skb);
                         if (ret < 0)
                                 goto out_sleep;
 
-                        ret = wl1251_acx_aid(wl, wl->aid);
+                        ret = wl1251_acx_aid(wl, bss_conf->aid);
                         if (ret < 0)
                                 goto out_sleep;
-
-                        /* If we want to go in PSM but we're not there yet */
-                        if (wl->psm_requested && !wl->psm) {
-                                mode = STATION_POWER_SAVE_MODE;
-                                ret = wl1251_ps_set_mode(wl, mode);
-                                if (ret < 0)
-                                        goto out_sleep;
-                        }
                 } else {
                         /* use defaults when not associated */
                         wl->beacon_int = WL1251_DEFAULT_BEACON_INT;
@@ -1137,23 +1033,6 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
                         ret = wl1251_acx_cts_protect(wl, CTSPROTECT_DISABLE);
                 if (ret < 0) {
                         wl1251_warning("Set ctsprotect failed %d", ret);
-                        goto out;
-                }
-        }
-
-        if (changed & BSS_CHANGED_BSSID) {
-                memcpy(wl->bssid, bss_conf->bssid, ETH_ALEN);
-
-                ret = wl1251_build_null_data(wl);
-                if (ret < 0)
-                        goto out;
-
-                if (wl->bss_type != BSS_TYPE_IBSS) {
-                        ret = wl1251_join(wl, wl->bss_type, wl->channel,
-                                          wl->beacon_int, wl->dtim_period);
-                        if (ret < 0)
-                                goto out_sleep;
-                        wl1251_warning("Set ctsprotect failed %d", ret);
                         goto out_sleep;
                 }
         }
@@ -1165,7 +1044,7 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
 
                 if (ret < 0) {
                         dev_kfree_skb(beacon);
-                        goto out;
+                        goto out_sleep;
                 }
 
                 ret = wl1251_cmd_template_set(wl, CMD_PROBE_RESP, beacon->data,
@@ -1174,13 +1053,13 @@ static void wl1251_op_bss_info_changed(struct ieee80211_hw *hw,
                 dev_kfree_skb(beacon);
 
                 if (ret < 0)
-                        goto out;
+                        goto out_sleep;
 
                 ret = wl1251_join(wl, wl->bss_type, wl->beacon_int,
                                   wl->channel, wl->dtim_period);
 
                 if (ret < 0)
-                        goto out;
+                        goto out_sleep;
         }
 
 out_sleep:
@@ -1251,6 +1130,49 @@ static struct ieee80211_channel wl1251_channels[] = {
         { .hw_value = 13, .center_freq = 2472},
 };
 
+static int wl1251_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
+                             const struct ieee80211_tx_queue_params *params)
+{
+        enum wl1251_acx_ps_scheme ps_scheme;
+        struct wl1251 *wl = hw->priv;
+        int ret;
+
+        mutex_lock(&wl->mutex);
+
+        wl1251_debug(DEBUG_MAC80211, "mac80211 conf tx %d", queue);
+
+        ret = wl1251_ps_elp_wakeup(wl);
+        if (ret < 0)
+                goto out;
+
+        /* mac80211 uses units of 32 usec */
+        ret = wl1251_acx_ac_cfg(wl, wl1251_tx_get_queue(queue),
+                                params->cw_min, params->cw_max,
+                                params->aifs, params->txop * 32);
+        if (ret < 0)
+                goto out_sleep;
+
+        if (params->uapsd)
+                ps_scheme = WL1251_ACX_PS_SCHEME_UPSD_TRIGGER;
+        else
+                ps_scheme = WL1251_ACX_PS_SCHEME_LEGACY;
+
+        ret = wl1251_acx_tid_cfg(wl, wl1251_tx_get_queue(queue),
+                                 CHANNEL_TYPE_EDCF,
+                                 wl1251_tx_get_queue(queue), ps_scheme,
+                                 WL1251_ACX_ACK_POLICY_LEGACY);
+        if (ret < 0)
+                goto out_sleep;
+
+out_sleep:
+        wl1251_ps_elp_sleep(wl);
+
+out:
+        mutex_unlock(&wl->mutex);
+
+        return ret;
+}
+
 /* can't be const, mac80211 writes to this */
 static struct ieee80211_supported_band wl1251_band_2ghz = {
         .channels = wl1251_channels,
@@ -1271,6 +1193,7 @@ static const struct ieee80211_ops wl1251_ops = {
         .hw_scan = wl1251_op_hw_scan,
         .bss_info_changed = wl1251_op_bss_info_changed,
         .set_rts_threshold = wl1251_op_set_rts_threshold,
+        .conf_tx = wl1251_op_conf_tx,
 };
 
 static int wl1251_register_hw(struct wl1251 *wl)
@@ -1308,12 +1231,17 @@ int wl1251_init_ieee80211(struct wl1251 *wl)
         wl->hw->channel_change_time = 10000;
 
         wl->hw->flags = IEEE80211_HW_SIGNAL_DBM |
-                IEEE80211_HW_NOISE_DBM;
+                IEEE80211_HW_NOISE_DBM |
+                IEEE80211_HW_SUPPORTS_PS |
+                IEEE80211_HW_BEACON_FILTER |
+                IEEE80211_HW_SUPPORTS_UAPSD;
 
         wl->hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION);
         wl->hw->wiphy->max_scan_ssids = 1;
         wl->hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &wl1251_band_2ghz;
 
+        wl->hw->queues = 4;
+
         ret = wl1251_register_hw(wl);
         if (ret)
                 goto out;
@@ -1351,6 +1279,7 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
         skb_queue_head_init(&wl->tx_queue);
 
         INIT_WORK(&wl->filter_work, wl1251_filter_work);
+        INIT_DELAYED_WORK(&wl->elp_work, wl1251_elp_work);
         wl->channel = WL1251_DEFAULT_CHANNEL;
         wl->scanning = false;
         wl->default_key = 0;
@@ -1368,6 +1297,7 @@ struct ieee80211_hw *wl1251_alloc_hw(void)
         wl->power_level = WL1251_DEFAULT_POWER_LEVEL;
         wl->beacon_int = WL1251_DEFAULT_BEACON_INT;
         wl->dtim_period = WL1251_DEFAULT_DTIM_PERIOD;
+        wl->vif = NULL;
 
         for (i = 0; i < FW_TX_CMPLT_BLOCK_SIZE; i++)
                 wl->tx_frames[i] = NULL;
@@ -1409,7 +1339,7 @@ int wl1251_free_hw(struct wl1251 *wl)
 
         kfree(wl->target_mem_map);
         kfree(wl->data_path);
-        kfree(wl->fw);
+        vfree(wl->fw);
         wl->fw = NULL;
         kfree(wl->nvs);
         wl->nvs = NULL;
@@ -1426,4 +1356,5 @@ EXPORT_SYMBOL_GPL(wl1251_free_hw);
 MODULE_DESCRIPTION("TI wl1251 Wireles LAN Driver Core");
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Kalle Valo <kalle.valo@nokia.com>");
-MODULE_ALIAS("spi:wl12xx");
+MODULE_ALIAS("spi:wl1251");
+MODULE_FIRMWARE(WL1251_FW_NAME);