// SPDX-License-Identifier: GPL-2.0-only
/*
 * This file is part of wlcore
 *
 * Copyright (C) 2008-2010 Nokia Corporation
 * Copyright (C) 2011-2013 Texas Instruments Inc.
 */

#include <linux/module.h>
#include <linux/firmware.h>
#include <linux/etherdevice.h>
#include <linux/vmalloc.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/pm_runtime.h>
#include <linux/pm_wakeirq.h>

#include "wlcore.h"
#include "debug.h"
#include "wl12xx_80211.h"
#include "io.h"
#include "tx.h"
#include "ps.h"
#include "init.h"
#include "debugfs.h"
#include "testmode.h"
#include "vendor_cmd.h"
#include "scan.h"
#include "hw_ops.h"
#include "sysfs.h"

#define WL1271_BOOT_RETRIES 3
#define WL1271_WAKEUP_TIMEOUT 500

static char *fwlog_param;
static int fwlog_mem_blocks = -1;
static int bug_on_recovery = -1;
static int no_recovery     = -1;

static void __wl1271_op_remove_interface(struct wl1271 *wl,
                                         struct ieee80211_vif *vif,
                                         bool reset_tx_queues);
static void wlcore_op_stop_locked(struct wl1271 *wl);
static void wl1271_free_ap_keys(struct wl1271 *wl, struct wl12xx_vif *wlvif);

static int wl12xx_set_authorized(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        int ret;

        if (WARN_ON(wlvif->bss_type != BSS_TYPE_STA_BSS))
                return -EINVAL;

        if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
                return 0;

        if (test_and_set_bit(WLVIF_FLAG_STA_STATE_SENT, &wlvif->flags))
                return 0;

        ret = wl12xx_cmd_set_peer_state(wl, wlvif, wlvif->sta.hlid);
        if (ret < 0)
                return ret;

        wl1271_info("Association completed.");
        return 0;
}

static void wl1271_reg_notify(struct wiphy *wiphy,
                              struct regulatory_request *request)
{
        struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
        struct wl1271 *wl = hw->priv;

        /* copy the current dfs region */
        if (request)
                wl->dfs_region = request->dfs_region;

        wlcore_regdomain_config(wl);
}

static int wl1271_set_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif,
                                   bool enable)
{
        int ret = 0;

        /* we should hold wl->mutex */
        ret = wl1271_acx_ps_rx_streaming(wl, wlvif, enable);
        if (ret < 0)
                goto out;

        if (enable)
                set_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags);
        else
                clear_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags);
out:
        return ret;
}

/*
 * this function is being called when the rx_streaming interval
 * has beed changed or rx_streaming should be disabled
 */
int wl1271_recalc_rx_streaming(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        int ret = 0;
        int period = wl->conf.rx_streaming.interval;

        /* don't reconfigure if rx_streaming is disabled */
        if (!test_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags))
                goto out;

        /* reconfigure/disable according to new streaming_period */
        if (period &&
            test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags) &&
            (wl->conf.rx_streaming.always ||
             test_bit(WL1271_FLAG_SOFT_GEMINI, &wl->flags)))
                ret = wl1271_set_rx_streaming(wl, wlvif, true);
        else {
                ret = wl1271_set_rx_streaming(wl, wlvif, false);
                /* don't cancel_work_sync since we might deadlock */
                del_timer_sync(&wlvif->rx_streaming_timer);
        }
out:
        return ret;
}

static void wl1271_rx_streaming_enable_work(struct work_struct *work)
{
        int ret;
        struct wl12xx_vif *wlvif = container_of(work, struct wl12xx_vif,
                                                rx_streaming_enable_work);
        struct wl1271 *wl = wlvif->wl;

        mutex_lock(&wl->mutex);

        if (test_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags) ||
            !test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags) ||
            (!wl->conf.rx_streaming.always &&
             !test_bit(WL1271_FLAG_SOFT_GEMINI, &wl->flags)))
                goto out;

        if (!wl->conf.rx_streaming.interval)
                goto out;

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        ret = wl1271_set_rx_streaming(wl, wlvif, true);
        if (ret < 0)
                goto out_sleep;

        /* stop it after some time of inactivity */
        mod_timer(&wlvif->rx_streaming_timer,
                  jiffies + msecs_to_jiffies(wl->conf.rx_streaming.duration));

out_sleep:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out:
        mutex_unlock(&wl->mutex);
}

static void wl1271_rx_streaming_disable_work(struct work_struct *work)
{
        int ret;
        struct wl12xx_vif *wlvif = container_of(work, struct wl12xx_vif,
                                                rx_streaming_disable_work);
        struct wl1271 *wl = wlvif->wl;

        mutex_lock(&wl->mutex);

        if (!test_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags))
                goto out;

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        ret = wl1271_set_rx_streaming(wl, wlvif, false);
        if (ret)
                goto out_sleep;

out_sleep:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out:
        mutex_unlock(&wl->mutex);
}

static void wl1271_rx_streaming_timer(struct timer_list *t)
{
        struct wl12xx_vif *wlvif = from_timer(wlvif, t, rx_streaming_timer);
        struct wl1271 *wl = wlvif->wl;
        ieee80211_queue_work(wl->hw, &wlvif->rx_streaming_disable_work);
}

/* wl->mutex must be taken */
void wl12xx_rearm_tx_watchdog_locked(struct wl1271 *wl)
{
        /* if the watchdog is not armed, don't do anything */
        if (wl->tx_allocated_blocks == 0)
                return;

        cancel_delayed_work(&wl->tx_watchdog_work);
        ieee80211_queue_delayed_work(wl->hw, &wl->tx_watchdog_work,
                msecs_to_jiffies(wl->conf.tx.tx_watchdog_timeout));
}

static void wlcore_rc_update_work(struct work_struct *work)
{
        int ret;
        struct wl12xx_vif *wlvif = container_of(work, struct wl12xx_vif,
                                                rc_update_work);
        struct wl1271 *wl = wlvif->wl;
        struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);

        mutex_lock(&wl->mutex);

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        if (ieee80211_vif_is_mesh(vif)) {
                ret = wl1271_acx_set_ht_capabilities(wl, &wlvif->rc_ht_cap,
                                                     true, wlvif->sta.hlid);
                if (ret < 0)
                        goto out_sleep;
        } else {
                wlcore_hw_sta_rc_update(wl, wlvif);
        }

out_sleep:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out:
        mutex_unlock(&wl->mutex);
}

static void wl12xx_tx_watchdog_work(struct work_struct *work)
{
        struct delayed_work *dwork;
        struct wl1271 *wl;

        dwork = to_delayed_work(work);
        wl = container_of(dwork, struct wl1271, tx_watchdog_work);

        mutex_lock(&wl->mutex);

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        /* Tx went out in the meantime - everything is ok */
        if (unlikely(wl->tx_allocated_blocks == 0))
                goto out;

        /*
         * if a ROC is in progress, we might not have any Tx for a long
         * time (e.g. pending Tx on the non-ROC channels)
         */
        if (find_first_bit(wl->roc_map, WL12XX_MAX_ROLES) < WL12XX_MAX_ROLES) {
                wl1271_debug(DEBUG_TX, "No Tx (in FW) for %d ms due to ROC",
                             wl->conf.tx.tx_watchdog_timeout);
                wl12xx_rearm_tx_watchdog_locked(wl);
                goto out;
        }

        /*
         * if a scan is in progress, we might not have any Tx for a long
         * time
         */
        if (wl->scan.state != WL1271_SCAN_STATE_IDLE) {
                wl1271_debug(DEBUG_TX, "No Tx (in FW) for %d ms due to scan",
                             wl->conf.tx.tx_watchdog_timeout);
                wl12xx_rearm_tx_watchdog_locked(wl);
                goto out;
        }

        /*
        * AP might cache a frame for a long time for a sleeping station,
        * so rearm the timer if there's an AP interface with stations. If
        * Tx is genuinely stuck we will most hopefully discover it when all
        * stations are removed due to inactivity.
        */
        if (wl->active_sta_count) {
                wl1271_debug(DEBUG_TX, "No Tx (in FW) for %d ms. AP has "
                             " %d stations",
                              wl->conf.tx.tx_watchdog_timeout,
                              wl->active_sta_count);
                wl12xx_rearm_tx_watchdog_locked(wl);
                goto out;
        }

        wl1271_error("Tx stuck (in FW) for %d ms. Starting recovery",
                     wl->conf.tx.tx_watchdog_timeout);
        wl12xx_queue_recovery_work(wl);

out:
        mutex_unlock(&wl->mutex);
}

static void wlcore_adjust_conf(struct wl1271 *wl)
{

        if (fwlog_param) {
                if (!strcmp(fwlog_param, "continuous")) {
                        wl->conf.fwlog.mode = WL12XX_FWLOG_CONTINUOUS;
                        wl->conf.fwlog.output = WL12XX_FWLOG_OUTPUT_HOST;
                } else if (!strcmp(fwlog_param, "dbgpins")) {
                        wl->conf.fwlog.mode = WL12XX_FWLOG_CONTINUOUS;
                        wl->conf.fwlog.output = WL12XX_FWLOG_OUTPUT_DBG_PINS;
                } else if (!strcmp(fwlog_param, "disable")) {
                        wl->conf.fwlog.mem_blocks = 0;
                        wl->conf.fwlog.output = WL12XX_FWLOG_OUTPUT_NONE;
                } else {
                        wl1271_error("Unknown fwlog parameter %s", fwlog_param);
                }
        }

        if (bug_on_recovery != -1)
                wl->conf.recovery.bug_on_recovery = (u8) bug_on_recovery;

        if (no_recovery != -1)
                wl->conf.recovery.no_recovery = (u8) no_recovery;
}

static void wl12xx_irq_ps_regulate_link(struct wl1271 *wl,
                                        struct wl12xx_vif *wlvif,
                                        u8 hlid, u8 tx_pkts)
{
        bool fw_ps;

        fw_ps = test_bit(hlid, &wl->ap_fw_ps_map);

        /*
         * Wake up from high level PS if the STA is asleep with too little
         * packets in FW or if the STA is awake.
         */
        if (!fw_ps || tx_pkts < WL1271_PS_STA_MAX_PACKETS)
                wl12xx_ps_link_end(wl, wlvif, hlid);

        /*
         * Start high-level PS if the STA is asleep with enough blocks in FW.
         * Make an exception if this is the only connected link. In this
         * case FW-memory congestion is less of a problem.
         * Note that a single connected STA means 2*ap_count + 1 active links,
         * since we must account for the global and broadcast AP links
         * for each AP. The "fw_ps" check assures us the other link is a STA
         * connected to the AP. Otherwise the FW would not set the PSM bit.
         */
        else if (wl->active_link_count > (wl->ap_count*2 + 1) && fw_ps &&
                 tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
                wl12xx_ps_link_start(wl, wlvif, hlid, true);
}

static void wl12xx_irq_update_links_status(struct wl1271 *wl,
                                           struct wl12xx_vif *wlvif,
                                           struct wl_fw_status *status)
{
        unsigned long cur_fw_ps_map;
        u8 hlid;

        cur_fw_ps_map = status->link_ps_bitmap;
        if (wl->ap_fw_ps_map != cur_fw_ps_map) {
                wl1271_debug(DEBUG_PSM,
                             "link ps prev 0x%lx cur 0x%lx changed 0x%lx",
                             wl->ap_fw_ps_map, cur_fw_ps_map,
                             wl->ap_fw_ps_map ^ cur_fw_ps_map);

                wl->ap_fw_ps_map = cur_fw_ps_map;
        }

        for_each_set_bit(hlid, wlvif->ap.sta_hlid_map, wl->num_links)
                wl12xx_irq_ps_regulate_link(wl, wlvif, hlid,
                                            wl->links[hlid].allocated_pkts);
}

static int wlcore_fw_status(struct wl1271 *wl, struct wl_fw_status *status)
{
        struct wl12xx_vif *wlvif;
        u32 old_tx_blk_count = wl->tx_blocks_available;
        int avail, freed_blocks;
        int i;
        int ret;
        struct wl1271_link *lnk;

        ret = wlcore_raw_read_data(wl, REG_RAW_FW_STATUS_ADDR,
                                   wl->raw_fw_status,
                                   wl->fw_status_len, false);
        if (ret < 0)
                return ret;

        wlcore_hw_convert_fw_status(wl, wl->raw_fw_status, wl->fw_status);

        wl1271_debug(DEBUG_IRQ, "intr: 0x%x (fw_rx_counter = %d, "
                     "drv_rx_counter = %d, tx_results_counter = %d)",
                     status->intr,
                     status->fw_rx_counter,
                     status->drv_rx_counter,
                     status->tx_results_counter);

        for (i = 0; i < NUM_TX_QUEUES; i++) {
                /* prevent wrap-around in freed-packets counter */
                wl->tx_allocated_pkts[i] -=
                                (status->counters.tx_released_pkts[i] -
                                wl->tx_pkts_freed[i]) & 0xff;

                wl->tx_pkts_freed[i] = status->counters.tx_released_pkts[i];
        }


        for_each_set_bit(i, wl->links_map, wl->num_links) {
                u8 diff;
                lnk = &wl->links[i];

                /* prevent wrap-around in freed-packets counter */
                diff = (status->counters.tx_lnk_free_pkts[i] -
                       lnk->prev_freed_pkts) & 0xff;

                if (diff == 0)
                        continue;

                lnk->allocated_pkts -= diff;
                lnk->prev_freed_pkts = status->counters.tx_lnk_free_pkts[i];

                /* accumulate the prev_freed_pkts counter */
                lnk->total_freed_pkts += diff;
        }

        /* prevent wrap-around in total blocks counter */
        if (likely(wl->tx_blocks_freed <= status->total_released_blks))
                freed_blocks = status->total_released_blks -
                               wl->tx_blocks_freed;
        else
                freed_blocks = 0x100000000LL - wl->tx_blocks_freed +
                               status->total_released_blks;

        wl->tx_blocks_freed = status->total_released_blks;

        wl->tx_allocated_blocks -= freed_blocks;

        /*
         * If the FW freed some blocks:
         * If we still have allocated blocks - re-arm the timer, Tx is
         * not stuck. Otherwise, cancel the timer (no Tx currently).
         */
        if (freed_blocks) {
                if (wl->tx_allocated_blocks)
                        wl12xx_rearm_tx_watchdog_locked(wl);
                else
                        cancel_delayed_work(&wl->tx_watchdog_work);
        }

        avail = status->tx_total - wl->tx_allocated_blocks;

        /*
         * The FW might change the total number of TX memblocks before
         * we get a notification about blocks being released. Thus, the
         * available blocks calculation might yield a temporary result
         * which is lower than the actual available blocks. Keeping in
         * mind that only blocks that were allocated can be moved from
         * TX to RX, tx_blocks_available should never decrease here.
         */
        wl->tx_blocks_available = max((int)wl->tx_blocks_available,
                                      avail);

        /* if more blocks are available now, tx work can be scheduled */
        if (wl->tx_blocks_available > old_tx_blk_count)
                clear_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);

        /* for AP update num of allocated TX blocks per link and ps status */
        wl12xx_for_each_wlvif_ap(wl, wlvif) {
                wl12xx_irq_update_links_status(wl, wlvif, status);
        }

        /* update the host-chipset time offset */
        wl->time_offset = (ktime_get_boottime_ns() >> 10) -
                (s64)(status->fw_localtime);

        wl->fw_fast_lnk_map = status->link_fast_bitmap;

        return 0;
}

static void wl1271_flush_deferred_work(struct wl1271 *wl)
{
        struct sk_buff *skb;

        /* Pass all received frames to the network stack */
        while ((skb = skb_dequeue(&wl->deferred_rx_queue)))
                ieee80211_rx_ni(wl->hw, skb);

        /* Return sent skbs to the network stack */
        while ((skb = skb_dequeue(&wl->deferred_tx_queue)))
                ieee80211_tx_status_ni(wl->hw, skb);
}

static void wl1271_netstack_work(struct work_struct *work)
{
        struct wl1271 *wl =
                container_of(work, struct wl1271, netstack_work);

        do {
                wl1271_flush_deferred_work(wl);
        } while (skb_queue_len(&wl->deferred_rx_queue));
}

#define WL1271_IRQ_MAX_LOOPS 256

static int wlcore_irq_locked(struct wl1271 *wl)
{
        int ret = 0;
        u32 intr;
        int loopcount = WL1271_IRQ_MAX_LOOPS;
        bool run_tx_queue = true;
        bool done = false;
        unsigned int defer_count;
        unsigned long flags;

        /*
         * In case edge triggered interrupt must be used, we cannot iterate
         * more than once without introducing race conditions with the hardirq.
         */
        if (wl->irq_flags & (IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING))
                loopcount = 1;

        wl1271_debug(DEBUG_IRQ, "IRQ work");

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        while (!done && loopcount--) {
                smp_mb__after_atomic();

                ret = wlcore_fw_status(wl, wl->fw_status);
                if (ret < 0)
                        goto err_ret;

                wlcore_hw_tx_immediate_compl(wl);

                intr = wl->fw_status->intr;
                intr &= WLCORE_ALL_INTR_MASK;
                if (!intr) {
                        done = true;
                        continue;
                }

                if (unlikely(intr & WL1271_ACX_INTR_WATCHDOG)) {
                        wl1271_error("HW watchdog interrupt received! starting recovery.");
                        wl->watchdog_recovery = true;
                        ret = -EIO;

                        /* restarting the chip. ignore any other interrupt. */
                        goto err_ret;
                }

                if (unlikely(intr & WL1271_ACX_SW_INTR_WATCHDOG)) {
                        wl1271_error("SW watchdog interrupt received! "
                                     "starting recovery.");
                        wl->watchdog_recovery = true;
                        ret = -EIO;

                        /* restarting the chip. ignore any other interrupt. */
                        goto err_ret;
                }

                if (likely(intr & WL1271_ACX_INTR_DATA)) {
                        wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_DATA");

                        ret = wlcore_rx(wl, wl->fw_status);
                        if (ret < 0)
                                goto err_ret;

                        /* Check if any tx blocks were freed */
                        if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags)) {
                                if (spin_trylock_irqsave(&wl->wl_lock, flags)) {
                                        if (!wl1271_tx_total_queue_count(wl))
                                                run_tx_queue = false;
                                        spin_unlock_irqrestore(&wl->wl_lock, flags);
                                }

                                /*
                                 * In order to avoid starvation of the TX path,
                                 * call the work function directly.
                                 */
                                if (run_tx_queue) {
                                        ret = wlcore_tx_work_locked(wl);
                                        if (ret < 0)
                                                goto err_ret;
                                }
                        }

                        /* check for tx results */
                        ret = wlcore_hw_tx_delayed_compl(wl);
                        if (ret < 0)
                                goto err_ret;

                        /* Make sure the deferred queues don't get too long */
                        defer_count = skb_queue_len(&wl->deferred_tx_queue) +
                                      skb_queue_len(&wl->deferred_rx_queue);
                        if (defer_count > WL1271_DEFERRED_QUEUE_LIMIT)
                                wl1271_flush_deferred_work(wl);
                }

                if (intr & WL1271_ACX_INTR_EVENT_A) {
                        wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_A");
                        ret = wl1271_event_handle(wl, 0);
                        if (ret < 0)
                                goto err_ret;
                }

                if (intr & WL1271_ACX_INTR_EVENT_B) {
                        wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_EVENT_B");
                        ret = wl1271_event_handle(wl, 1);
                        if (ret < 0)
                                goto err_ret;
                }

                if (intr & WL1271_ACX_INTR_INIT_COMPLETE)
                        wl1271_debug(DEBUG_IRQ,
                                     "WL1271_ACX_INTR_INIT_COMPLETE");

                if (intr & WL1271_ACX_INTR_HW_AVAILABLE)
                        wl1271_debug(DEBUG_IRQ, "WL1271_ACX_INTR_HW_AVAILABLE");
        }

err_ret:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);

out:
        return ret;
}

static irqreturn_t wlcore_irq(int irq, void *cookie)
{
        int ret;
        unsigned long flags;
        struct wl1271 *wl = cookie;
        bool queue_tx_work = true;

        set_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);

        /* complete the ELP completion */
        if (test_bit(WL1271_FLAG_IN_ELP, &wl->flags)) {
                spin_lock_irqsave(&wl->wl_lock, flags);
                if (wl->elp_compl)
                        complete(wl->elp_compl);
                spin_unlock_irqrestore(&wl->wl_lock, flags);
        }

        if (test_bit(WL1271_FLAG_SUSPENDED, &wl->flags)) {
                /* don't enqueue a work right now. mark it as pending */
                set_bit(WL1271_FLAG_PENDING_WORK, &wl->flags);
                wl1271_debug(DEBUG_IRQ, "should not enqueue work");
                spin_lock_irqsave(&wl->wl_lock, flags);
                disable_irq_nosync(wl->irq);
                pm_wakeup_event(wl->dev, 0);
                spin_unlock_irqrestore(&wl->wl_lock, flags);
                goto out_handled;
        }

        /* TX might be handled here, avoid redundant work */
        set_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
        cancel_work_sync(&wl->tx_work);

        mutex_lock(&wl->mutex);

        ret = wlcore_irq_locked(wl);
        if (ret)
                wl12xx_queue_recovery_work(wl);

        /* In case TX was not handled in wlcore_irq_locked(), queue TX work */
        clear_bit(WL1271_FLAG_TX_PENDING, &wl->flags);
        if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags)) {
                if (spin_trylock_irqsave(&wl->wl_lock, flags)) {
                        if (!wl1271_tx_total_queue_count(wl))
                                queue_tx_work = false;
                        spin_unlock_irqrestore(&wl->wl_lock, flags);
                }
                if (queue_tx_work)
                        ieee80211_queue_work(wl->hw, &wl->tx_work);
        }

        mutex_unlock(&wl->mutex);

out_handled:
        clear_bit(WL1271_FLAG_IRQ_RUNNING, &wl->flags);

        return IRQ_HANDLED;
}

struct vif_counter_data {
        u8 counter;

        struct ieee80211_vif *cur_vif;
        bool cur_vif_running;
};

static void wl12xx_vif_count_iter(void *data, u8 *mac,
                                  struct ieee80211_vif *vif)
{
        struct vif_counter_data *counter = data;

        counter->counter++;
        if (counter->cur_vif == vif)
                counter->cur_vif_running = true;
}

/* caller must not hold wl->mutex, as it might deadlock */
static void wl12xx_get_vif_count(struct ieee80211_hw *hw,
                               struct ieee80211_vif *cur_vif,
                               struct vif_counter_data *data)
{
        memset(data, 0, sizeof(*data));
        data->cur_vif = cur_vif;

        ieee80211_iterate_active_interfaces(hw, IEEE80211_IFACE_ITER_RESUME_ALL,
                                            wl12xx_vif_count_iter, data);
}

static int wl12xx_fetch_firmware(struct wl1271 *wl, bool plt)
{
        const struct firmware *fw;
        const char *fw_name;
        enum wl12xx_fw_type fw_type;
        int ret;

        if (plt) {
                fw_type = WL12XX_FW_TYPE_PLT;
                fw_name = wl->plt_fw_name;
        } else {
                /*
                 * we can't call wl12xx_get_vif_count() here because
                 * wl->mutex is taken, so use the cached last_vif_count value
                 */
                if (wl->last_vif_count > 1 && wl->mr_fw_name) {
                        fw_type = WL12XX_FW_TYPE_MULTI;
                        fw_name = wl->mr_fw_name;
                } else {
                        fw_type = WL12XX_FW_TYPE_NORMAL;
                        fw_name = wl->sr_fw_name;
                }
        }

        if (wl->fw_type == fw_type)
                return 0;

        wl1271_debug(DEBUG_BOOT, "booting firmware %s", fw_name);

        ret = request_firmware(&fw, fw_name, wl->dev);

        if (ret < 0) {
                wl1271_error("could not get firmware %s: %d", fw_name, ret);
                return ret;
        }

        if (fw->size % 4) {
                wl1271_error("firmware size is not multiple of 32 bits: %zu",
                             fw->size);
                ret = -EILSEQ;
                goto out;
        }

        vfree(wl->fw);
        wl->fw_type = WL12XX_FW_TYPE_NONE;
        wl->fw_len = fw->size;
        wl->fw = vmalloc(wl->fw_len);

        if (!wl->fw) {
                wl1271_error("could not allocate memory for the firmware");
                ret = -ENOMEM;
                goto out;
        }

        memcpy(wl->fw, fw->data, wl->fw_len);
        ret = 0;
        wl->fw_type = fw_type;
out:
        release_firmware(fw);

        return ret;
}

void wl12xx_queue_recovery_work(struct wl1271 *wl)
{
        /* Avoid a recursive recovery */
        if (wl->state == WLCORE_STATE_ON) {
                WARN_ON(!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY,
                                  &wl->flags));

                wl->state = WLCORE_STATE_RESTARTING;
                set_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
                ieee80211_queue_work(wl->hw, &wl->recovery_work);
        }
}

size_t wl12xx_copy_fwlog(struct wl1271 *wl, u8 *memblock, size_t maxlen)
{
        size_t len;

        /* Make sure we have enough room */
        len = min_t(size_t, maxlen, PAGE_SIZE - wl->fwlog_size);

        /* Fill the FW log file, consumed by the sysfs fwlog entry */
        memcpy(wl->fwlog + wl->fwlog_size, memblock, len);
        wl->fwlog_size += len;

        return len;
}

static void wl12xx_read_fwlog_panic(struct wl1271 *wl)
{
        u32 end_of_log = 0;
        int error;

        if (wl->quirks & WLCORE_QUIRK_FWLOG_NOT_IMPLEMENTED)
                return;

        wl1271_info("Reading FW panic log");

        /*
         * Make sure the chip is awake and the logger isn't active.
         * Do not send a stop fwlog command if the fw is hanged or if
         * dbgpins are used (due to some fw bug).
         */
        error = pm_runtime_get_sync(wl->dev);
        if (error < 0) {
                pm_runtime_put_noidle(wl->dev);
                return;
        }
        if (!wl->watchdog_recovery &&
            wl->conf.fwlog.output != WL12XX_FWLOG_OUTPUT_DBG_PINS)
                wl12xx_cmd_stop_fwlog(wl);

        /* Traverse the memory blocks linked list */
        do {
                end_of_log = wlcore_event_fw_logger(wl);
                if (end_of_log == 0) {
                        msleep(100);
                        end_of_log = wlcore_event_fw_logger(wl);
                }
        } while (end_of_log != 0);
}

static void wlcore_save_freed_pkts(struct wl1271 *wl, struct wl12xx_vif *wlvif,
                                   u8 hlid, struct ieee80211_sta *sta)
{
        struct wl1271_station *wl_sta;
        u32 sqn_recovery_padding = WL1271_TX_SQN_POST_RECOVERY_PADDING;

        wl_sta = (void *)sta->drv_priv;
        wl_sta->total_freed_pkts = wl->links[hlid].total_freed_pkts;

        /*
         * increment the initial seq number on recovery to account for
         * transmitted packets that we haven't yet got in the FW status
         */
        if (wlvif->encryption_type == KEY_GEM)
                sqn_recovery_padding = WL1271_TX_SQN_POST_RECOVERY_PADDING_GEM;

        if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
                wl_sta->total_freed_pkts += sqn_recovery_padding;
}

static void wlcore_save_freed_pkts_addr(struct wl1271 *wl,
                                        struct wl12xx_vif *wlvif,
                                        u8 hlid, const u8 *addr)
{
        struct ieee80211_sta *sta;
        struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);

        if (WARN_ON(hlid == WL12XX_INVALID_LINK_ID ||
                    is_zero_ether_addr(addr)))
                return;

        rcu_read_lock();
        sta = ieee80211_find_sta(vif, addr);
        if (sta)
                wlcore_save_freed_pkts(wl, wlvif, hlid, sta);
        rcu_read_unlock();
}

static void wlcore_print_recovery(struct wl1271 *wl)
{
        u32 pc = 0;
        u32 hint_sts = 0;
        int ret;

        wl1271_info("Hardware recovery in progress. FW ver: %s",
                    wl->chip.fw_ver_str);

        /* change partitions momentarily so we can read the FW pc */
        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
        if (ret < 0)
                return;

        ret = wlcore_read_reg(wl, REG_PC_ON_RECOVERY, &pc);
        if (ret < 0)
                return;

        ret = wlcore_read_reg(wl, REG_INTERRUPT_NO_CLEAR, &hint_sts);
        if (ret < 0)
                return;

        wl1271_info("pc: 0x%x, hint_sts: 0x%08x count: %d",
                                pc, hint_sts, ++wl->recovery_count);

        wlcore_set_partition(wl, &wl->ptable[PART_WORK]);
}


static void wl1271_recovery_work(struct work_struct *work)
{
        struct wl1271 *wl =
                container_of(work, struct wl1271, recovery_work);
        struct wl12xx_vif *wlvif;
        struct ieee80211_vif *vif;
        int error;

        mutex_lock(&wl->mutex);

        if (wl->state == WLCORE_STATE_OFF || wl->plt)
                goto out_unlock;

        error = pm_runtime_get_sync(wl->dev);
        if (error < 0) {
                wl1271_warning("Enable for recovery failed");
                pm_runtime_put_noidle(wl->dev);
        }
        wlcore_disable_interrupts_nosync(wl);

        if (!test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags)) {
                if (wl->conf.fwlog.output == WL12XX_FWLOG_OUTPUT_HOST)
                        wl12xx_read_fwlog_panic(wl);
                wlcore_print_recovery(wl);
        }

        BUG_ON(wl->conf.recovery.bug_on_recovery &&
               !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags));

        clear_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags);

        if (wl->conf.recovery.no_recovery) {
                wl1271_info("No recovery (chosen on module load). Fw will remain stuck.");
                goto out_unlock;
        }

        /* Prevent spurious TX during FW restart */
        wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_FW_RESTART);

        /* reboot the chipset */
        while (!list_empty(&wl->wlvif_list)) {
                wlvif = list_first_entry(&wl->wlvif_list,
                                       struct wl12xx_vif, list);
                vif = wl12xx_wlvif_to_vif(wlvif);

                if (wlvif->bss_type == BSS_TYPE_STA_BSS &&
                    test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags)) {
                        wlcore_save_freed_pkts_addr(wl, wlvif, wlvif->sta.hlid,
                                                    vif->bss_conf.bssid);
                }

                __wl1271_op_remove_interface(wl, vif, false);
        }

        wlcore_op_stop_locked(wl);
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);

        ieee80211_restart_hw(wl->hw);

        /*
         * Its safe to enable TX now - the queues are stopped after a request
         * to restart the HW.
         */
        wlcore_wake_queues(wl, WLCORE_QUEUE_STOP_REASON_FW_RESTART);

out_unlock:
        wl->watchdog_recovery = false;
        clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags);
        mutex_unlock(&wl->mutex);
}

static int wlcore_fw_wakeup(struct wl1271 *wl)
{

        return wlcore_raw_write32(wl, HW_ACCESS_ELP_CTRL_REG, ELPCTRL_WAKE_UP);
}

static int wl1271_setup(struct wl1271 *wl)
{
        wl->raw_fw_status = kzalloc(wl->fw_status_len, GFP_KERNEL);
        if (!wl->raw_fw_status)
                goto err;

        wl->fw_status = kzalloc(sizeof(*wl->fw_status), GFP_KERNEL);
        if (!wl->fw_status)
                goto err;

        wl->tx_res_if = kzalloc(sizeof(*wl->tx_res_if), GFP_KERNEL);
        if (!wl->tx_res_if)
                goto err;

        return 0;
err:
        kfree(wl->fw_status);
        kfree(wl->raw_fw_status);
        return -ENOMEM;
}

static int wl12xx_set_power_on(struct wl1271 *wl)
{
        int ret;

        msleep(WL1271_PRE_POWER_ON_SLEEP);
        ret = wl1271_power_on(wl);
        if (ret < 0)
                goto out;
        msleep(WL1271_POWER_ON_SLEEP);
        wl1271_io_reset(wl);
        wl1271_io_init(wl);

        ret = wlcore_set_partition(wl, &wl->ptable[PART_BOOT]);
        if (ret < 0)
                goto fail;

        /* ELP module wake up */
        ret = wlcore_fw_wakeup(wl);
        if (ret < 0)
                goto fail;

out:
        return ret;

fail:
        wl1271_power_off(wl);
        return ret;
}

static int wl12xx_chip_wakeup(struct wl1271 *wl, bool plt)
{
        int ret = 0;

        ret = wl12xx_set_power_on(wl);
        if (ret < 0)
                goto out;

        /*
         * For wl127x based devices we could use the default block
         * size (512 bytes), but due to a bug in the sdio driver, we
         * need to set it explicitly after the chip is powered on.  To
         * simplify the code and since the performance impact is
         * negligible, we use the same block size for all different
         * chip types.
         *
         * Check if the bus supports blocksize alignment and, if it
         * doesn't, make sure we don't have the quirk.
         */
        if (!wl1271_set_block_size(wl))
                wl->quirks &= ~WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN;

        /* TODO: make sure the lower driver has set things up correctly */

        ret = wl1271_setup(wl);
        if (ret < 0)
                goto out;

        ret = wl12xx_fetch_firmware(wl, plt);
        if (ret < 0) {
                kfree(wl->fw_status);
                kfree(wl->raw_fw_status);
                kfree(wl->tx_res_if);
        }

out:
        return ret;
}

int wl1271_plt_start(struct wl1271 *wl, const enum plt_mode plt_mode)
{
        int retries = WL1271_BOOT_RETRIES;
        struct wiphy *wiphy = wl->hw->wiphy;

        static const char* const PLT_MODE[] = {
                "PLT_OFF",
                "PLT_ON",
                "PLT_FEM_DETECT",
                "PLT_CHIP_AWAKE"
        };

        int ret;

        mutex_lock(&wl->mutex);

        wl1271_notice("power up");

        if (wl->state != WLCORE_STATE_OFF) {
                wl1271_error("cannot go into PLT state because not "
                             "in off state: %d", wl->state);
                ret = -EBUSY;
                goto out;
        }

        /* Indicate to lower levels that we are now in PLT mode */
        wl->plt = true;
        wl->plt_mode = plt_mode;

        while (retries) {
                retries--;
                ret = wl12xx_chip_wakeup(wl, true);
                if (ret < 0)
                        goto power_off;

                if (plt_mode != PLT_CHIP_AWAKE) {
                        ret = wl->ops->plt_init(wl);
                        if (ret < 0)
                                goto power_off;
                }

                wl->state = WLCORE_STATE_ON;
                wl1271_notice("firmware booted in PLT mode %s (%s)",
                              PLT_MODE[plt_mode],
                              wl->chip.fw_ver_str);

                /* update hw/fw version info in wiphy struct */
                wiphy->hw_version = wl->chip.id;
                strncpy(wiphy->fw_version, wl->chip.fw_ver_str,
                        sizeof(wiphy->fw_version));

                goto out;

power_off:
                wl1271_power_off(wl);
        }

        wl->plt = false;
        wl->plt_mode = PLT_OFF;

        wl1271_error("firmware boot in PLT mode failed despite %d retries",
                     WL1271_BOOT_RETRIES);
out:
        mutex_unlock(&wl->mutex);

        return ret;
}

int wl1271_plt_stop(struct wl1271 *wl)
{
        int ret = 0;

        wl1271_notice("power down");

        /*
         * Interrupts must be disabled before setting the state to OFF.
         * Otherwise, the interrupt handler might be called and exit without
         * reading the interrupt status.
         */
        wlcore_disable_interrupts(wl);
        mutex_lock(&wl->mutex);
        if (!wl->plt) {
                mutex_unlock(&wl->mutex);

                /*
                 * This will not necessarily enable interrupts as interrupts
                 * may have been disabled when op_stop was called. It will,
                 * however, balance the above call to disable_interrupts().
                 */
                wlcore_enable_interrupts(wl);

                wl1271_error("cannot power down because not in PLT "
                             "state: %d", wl->state);
                ret = -EBUSY;
                goto out;
        }

        mutex_unlock(&wl->mutex);

        wl1271_flush_deferred_work(wl);
        cancel_work_sync(&wl->netstack_work);
        cancel_work_sync(&wl->recovery_work);
        cancel_delayed_work_sync(&wl->tx_watchdog_work);

        mutex_lock(&wl->mutex);
        wl1271_power_off(wl);
        wl->flags = 0;
        wl->sleep_auth = WL1271_PSM_ILLEGAL;
        wl->state = WLCORE_STATE_OFF;
        wl->plt = false;
        wl->plt_mode = PLT_OFF;
        wl->rx_counter = 0;
        mutex_unlock(&wl->mutex);

out:
        return ret;
}

static void wl1271_op_tx(struct ieee80211_hw *hw,
                         struct ieee80211_tx_control *control,
                         struct sk_buff *skb)
{
        struct wl1271 *wl = hw->priv;
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
        struct ieee80211_vif *vif = info->control.vif;
        struct wl12xx_vif *wlvif = NULL;
        unsigned long flags;
        int q, mapping;
        u8 hlid;

        if (!vif) {
                wl1271_debug(DEBUG_TX, "DROP skb with no vif");
                ieee80211_free_txskb(hw, skb);
                return;
        }

        wlvif = wl12xx_vif_to_data(vif);
        mapping = skb_get_queue_mapping(skb);
        q = wl1271_tx_get_queue(mapping);

        hlid = wl12xx_tx_get_hlid(wl, wlvif, skb, control->sta);

        spin_lock_irqsave(&wl->wl_lock, flags);

        /*
         * drop the packet if the link is invalid or the queue is stopped
         * for any reason but watermark. Watermark is a "soft"-stop so we
         * allow these packets through.
         */
        if (hlid == WL12XX_INVALID_LINK_ID ||
            (!test_bit(hlid, wlvif->links_map)) ||
             (wlcore_is_queue_stopped_locked(wl, wlvif, q) &&
              !wlcore_is_queue_stopped_by_reason_locked(wl, wlvif, q,
                        WLCORE_QUEUE_STOP_REASON_WATERMARK))) {
                wl1271_debug(DEBUG_TX, "DROP skb hlid %d q %d", hlid, q);
                ieee80211_free_txskb(hw, skb);
                goto out;
        }

        wl1271_debug(DEBUG_TX, "queue skb hlid %d q %d len %d",
                     hlid, q, skb->len);
        skb_queue_tail(&wl->links[hlid].tx_queue[q], skb);

        wl->tx_queue_count[q]++;
        wlvif->tx_queue_count[q]++;

        /*
         * The workqueue is slow to process the tx_queue and we need stop
         * the queue here, otherwise the queue will get too long.
         */
        if (wlvif->tx_queue_count[q] >= WL1271_TX_QUEUE_HIGH_WATERMARK &&
            !wlcore_is_queue_stopped_by_reason_locked(wl, wlvif, q,
                                        WLCORE_QUEUE_STOP_REASON_WATERMARK)) {
                wl1271_debug(DEBUG_TX, "op_tx: stopping queues for q %d", q);
                wlcore_stop_queue_locked(wl, wlvif, q,
                                         WLCORE_QUEUE_STOP_REASON_WATERMARK);
        }

        /*
         * The chip specific setup must run before the first TX packet -
         * before that, the tx_work will not be initialized!
         */

        if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags) &&
            !test_bit(WL1271_FLAG_TX_PENDING, &wl->flags))
                ieee80211_queue_work(wl->hw, &wl->tx_work);

out:
        spin_unlock_irqrestore(&wl->wl_lock, flags);
}

int wl1271_tx_dummy_packet(struct wl1271 *wl)
{
        unsigned long flags;
        int q;

        /* no need to queue a new dummy packet if one is already pending */
        if (test_bit(WL1271_FLAG_DUMMY_PACKET_PENDING, &wl->flags))
                return 0;

        q = wl1271_tx_get_queue(skb_get_queue_mapping(wl->dummy_packet));

        spin_lock_irqsave(&wl->wl_lock, flags);
        set_bit(WL1271_FLAG_DUMMY_PACKET_PENDING, &wl->flags);
        wl->tx_queue_count[q]++;
        spin_unlock_irqrestore(&wl->wl_lock, flags);

        /* The FW is low on RX memory blocks, so send the dummy packet asap */
        if (!test_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags))
                return wlcore_tx_work_locked(wl);

        /*
         * If the FW TX is busy, TX work will be scheduled by the threaded
         * interrupt handler function
         */
        return 0;
}

/*
 * The size of the dummy packet should be at least 1400 bytes. However, in
 * order to minimize the number of bus transactions, aligning it to 512 bytes
 * boundaries could be beneficial, performance wise
 */
#define TOTAL_TX_DUMMY_PACKET_SIZE (ALIGN(1400, 512))

static struct sk_buff *wl12xx_alloc_dummy_packet(struct wl1271 *wl)
{
        struct sk_buff *skb;
        struct ieee80211_hdr_3addr *hdr;
        unsigned int dummy_packet_size;

        dummy_packet_size = TOTAL_TX_DUMMY_PACKET_SIZE -
                            sizeof(struct wl1271_tx_hw_descr) - sizeof(*hdr);

        skb = dev_alloc_skb(TOTAL_TX_DUMMY_PACKET_SIZE);
        if (!skb) {
                wl1271_warning("Failed to allocate a dummy packet skb");
                return NULL;
        }

        skb_reserve(skb, sizeof(struct wl1271_tx_hw_descr));

        hdr = skb_put_zero(skb, sizeof(*hdr));
        hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
                                         IEEE80211_STYPE_NULLFUNC |
                                         IEEE80211_FCTL_TODS);

        skb_put_zero(skb, dummy_packet_size);

        /* Dummy packets require the TID to be management */
        skb->priority = WL1271_TID_MGMT;

        /* Initialize all fields that might be used */
        skb_set_queue_mapping(skb, 0);
        memset(IEEE80211_SKB_CB(skb), 0, sizeof(struct ieee80211_tx_info));

        return skb;
}


static int
wl1271_validate_wowlan_pattern(struct cfg80211_pkt_pattern *p)
{
        int num_fields = 0, in_field = 0, fields_size = 0;
        int i, pattern_len = 0;

        if (!p->mask) {
                wl1271_warning("No mask in WoWLAN pattern");
                return -EINVAL;
        }

        /*
         * The pattern is broken up into segments of bytes at different offsets
         * that need to be checked by the FW filter. Each segment is called
         * a field in the FW API. We verify that the total number of fields
         * required for this pattern won't exceed FW limits (8)
         * as well as the total fields buffer won't exceed the FW limit.
         * Note that if there's a pattern which crosses Ethernet/IP header
         * boundary a new field is required.
         */
        for (i = 0; i < p->pattern_len; i++) {
                if (test_bit(i, (unsigned long *)p->mask)) {
                        if (!in_field) {
                                in_field = 1;
                                pattern_len = 1;
                        } else {
                                if (i == WL1271_RX_FILTER_ETH_HEADER_SIZE) {
                                        num_fields++;
                                        fields_size += pattern_len +
                                                RX_FILTER_FIELD_OVERHEAD;
                                        pattern_len = 1;
                                } else
                                        pattern_len++;
                        }
                } else {
                        if (in_field) {
                                in_field = 0;
                                fields_size += pattern_len +
                                        RX_FILTER_FIELD_OVERHEAD;
                                num_fields++;
                        }
                }
        }

        if (in_field) {
                fields_size += pattern_len + RX_FILTER_FIELD_OVERHEAD;
                num_fields++;
        }

        if (num_fields > WL1271_RX_FILTER_MAX_FIELDS) {
                wl1271_warning("RX Filter too complex. Too many segments");
                return -EINVAL;
        }

        if (fields_size > WL1271_RX_FILTER_MAX_FIELDS_SIZE) {
                wl1271_warning("RX filter pattern is too big");
                return -E2BIG;
        }

        return 0;
}

struct wl12xx_rx_filter *wl1271_rx_filter_alloc(void)
{
        return kzalloc(sizeof(struct wl12xx_rx_filter), GFP_KERNEL);
}

void wl1271_rx_filter_free(struct wl12xx_rx_filter *filter)
{
        int i;

        if (filter == NULL)
                return;

        for (i = 0; i < filter->num_fields; i++)
                kfree(filter->fields[i].pattern);

        kfree(filter);
}

int wl1271_rx_filter_alloc_field(struct wl12xx_rx_filter *filter,
                                 u16 offset, u8 flags,
                                 const u8 *pattern, u8 len)
{
        struct wl12xx_rx_filter_field *field;

        if (filter->num_fields == WL1271_RX_FILTER_MAX_FIELDS) {
                wl1271_warning("Max fields per RX filter. can't alloc another");
                return -EINVAL;
        }

        field = &filter->fields[filter->num_fields];

        field->pattern = kmemdup(pattern, len, GFP_KERNEL);
        if (!field->pattern) {
                wl1271_warning("Failed to allocate RX filter pattern");
                return -ENOMEM;
        }

        filter->num_fields++;

        field->offset = cpu_to_le16(offset);
        field->flags = flags;
        field->len = len;

        return 0;
}

int wl1271_rx_filter_get_fields_size(struct wl12xx_rx_filter *filter)
{
        int i, fields_size = 0;

        for (i = 0; i < filter->num_fields; i++)
                fields_size += filter->fields[i].len +
                        sizeof(struct wl12xx_rx_filter_field) -
                        sizeof(u8 *);

        return fields_size;
}

void wl1271_rx_filter_flatten_fields(struct wl12xx_rx_filter *filter,
                                    u8 *buf)
{
        int i;
        struct wl12xx_rx_filter_field *field;

        for (i = 0; i < filter->num_fields; i++) {
                field = (struct wl12xx_rx_filter_field *)buf;

                field->offset = filter->fields[i].offset;
                field->flags = filter->fields[i].flags;
                field->len = filter->fields[i].len;

                memcpy(&field->pattern, filter->fields[i].pattern, field->len);
                buf += sizeof(struct wl12xx_rx_filter_field) -
                        sizeof(u8 *) + field->len;
        }
}

/*
 * Allocates an RX filter returned through f
 * which needs to be freed using rx_filter_free()
 */
static int
wl1271_convert_wowlan_pattern_to_rx_filter(struct cfg80211_pkt_pattern *p,
                                           struct wl12xx_rx_filter **f)
{
        int i, j, ret = 0;
        struct wl12xx_rx_filter *filter;
        u16 offset;
        u8 flags, len;

        filter = wl1271_rx_filter_alloc();
        if (!filter) {
                wl1271_warning("Failed to alloc rx filter");
                ret = -ENOMEM;
                goto err;
        }

        i = 0;
        while (i < p->pattern_len) {
                if (!test_bit(i, (unsigned long *)p->mask)) {
                        i++;
                        continue;
                }

                for (j = i; j < p->pattern_len; j++) {
                        if (!test_bit(j, (unsigned long *)p->mask))
                                break;

                        if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE &&
                            j >= WL1271_RX_FILTER_ETH_HEADER_SIZE)
                                break;
                }

                if (i < WL1271_RX_FILTER_ETH_HEADER_SIZE) {
                        offset = i;
                        flags = WL1271_RX_FILTER_FLAG_ETHERNET_HEADER;
                } else {
                        offset = i - WL1271_RX_FILTER_ETH_HEADER_SIZE;
                        flags = WL1271_RX_FILTER_FLAG_IP_HEADER;
                }

                len = j - i;

                ret = wl1271_rx_filter_alloc_field(filter,
                                                   offset,
                                                   flags,
                                                   &p->pattern[i], len);
                if (ret)
                        goto err;

                i = j;
        }

        filter->action = FILTER_SIGNAL;

        *f = filter;
        return 0;

err:
        wl1271_rx_filter_free(filter);
        *f = NULL;

        return ret;
}

static int wl1271_configure_wowlan(struct wl1271 *wl,
                                   struct cfg80211_wowlan *wow)
{
        int i, ret;

        if (!wow || wow->any || !wow->n_patterns) {
                ret = wl1271_acx_default_rx_filter_enable(wl, 0,
                                                          FILTER_SIGNAL);
                if (ret)
                        goto out;

                ret = wl1271_rx_filter_clear_all(wl);
                if (ret)
                        goto out;

                return 0;
        }

        if (WARN_ON(wow->n_patterns > WL1271_MAX_RX_FILTERS))
                return -EINVAL;

        /* Validate all incoming patterns before clearing current FW state */
        for (i = 0; i < wow->n_patterns; i++) {
                ret = wl1271_validate_wowlan_pattern(&wow->patterns[i]);
                if (ret) {
                        wl1271_warning("Bad wowlan pattern %d", i);
                        return ret;
                }
        }

        ret = wl1271_acx_default_rx_filter_enable(wl, 0, FILTER_SIGNAL);
        if (ret)
                goto out;

        ret = wl1271_rx_filter_clear_all(wl);
        if (ret)
                goto out;

        /* Translate WoWLAN patterns into filters */
        for (i = 0; i < wow->n_patterns; i++) {
                struct cfg80211_pkt_pattern *p;
                struct wl12xx_rx_filter *filter = NULL;

                p = &wow->patterns[i];

                ret = wl1271_convert_wowlan_pattern_to_rx_filter(p, &filter);
                if (ret) {
                        wl1271_warning("Failed to create an RX filter from "
                                       "wowlan pattern %d", i);
                        goto out;
                }

                ret = wl1271_rx_filter_enable(wl, i, 1, filter);

                wl1271_rx_filter_free(filter);
                if (ret)
                        goto out;
        }

        ret = wl1271_acx_default_rx_filter_enable(wl, 1, FILTER_DROP);

out:
        return ret;
}

static int wl1271_configure_suspend_sta(struct wl1271 *wl,
                                        struct wl12xx_vif *wlvif,
                                        struct cfg80211_wowlan *wow)
{
        int ret = 0;

        if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
                goto out;

        ret = wl1271_configure_wowlan(wl, wow);
        if (ret < 0)
                goto out;

        if ((wl->conf.conn.suspend_wake_up_event ==
             wl->conf.conn.wake_up_event) &&
            (wl->conf.conn.suspend_listen_interval ==
             wl->conf.conn.listen_interval))
                goto out;

        ret = wl1271_acx_wake_up_conditions(wl, wlvif,
                                    wl->conf.conn.suspend_wake_up_event,
                                    wl->conf.conn.suspend_listen_interval);

        if (ret < 0)
                wl1271_error("suspend: set wake up conditions failed: %d", ret);
out:
        return ret;

}

static int wl1271_configure_suspend_ap(struct wl1271 *wl,
                                        struct wl12xx_vif *wlvif,
                                        struct cfg80211_wowlan *wow)
{
        int ret = 0;

        if (!test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags))
                goto out;

        ret = wl1271_acx_beacon_filter_opt(wl, wlvif, true);
        if (ret < 0)
                goto out;

        ret = wl1271_configure_wowlan(wl, wow);
        if (ret < 0)
                goto out;

out:
        return ret;

}

static int wl1271_configure_suspend(struct wl1271 *wl,
                                    struct wl12xx_vif *wlvif,
                                    struct cfg80211_wowlan *wow)
{
        if (wlvif->bss_type == BSS_TYPE_STA_BSS)
                return wl1271_configure_suspend_sta(wl, wlvif, wow);
        if (wlvif->bss_type == BSS_TYPE_AP_BSS)
                return wl1271_configure_suspend_ap(wl, wlvif, wow);
        return 0;
}

static void wl1271_configure_resume(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        int ret = 0;
        bool is_ap = wlvif->bss_type == BSS_TYPE_AP_BSS;
        bool is_sta = wlvif->bss_type == BSS_TYPE_STA_BSS;

        if ((!is_ap) && (!is_sta))
                return;

        if ((is_sta && !test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags)) ||
            (is_ap && !test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags)))
                return;

        wl1271_configure_wowlan(wl, NULL);

        if (is_sta) {
                if ((wl->conf.conn.suspend_wake_up_event ==
                     wl->conf.conn.wake_up_event) &&
                    (wl->conf.conn.suspend_listen_interval ==
                     wl->conf.conn.listen_interval))
                        return;

                ret = wl1271_acx_wake_up_conditions(wl, wlvif,
                                    wl->conf.conn.wake_up_event,
                                    wl->conf.conn.listen_interval);

                if (ret < 0)
                        wl1271_error("resume: wake up conditions failed: %d",
                                     ret);

        } else if (is_ap) {
                ret = wl1271_acx_beacon_filter_opt(wl, wlvif, false);
        }
}

static int __maybe_unused wl1271_op_suspend(struct ieee80211_hw *hw,
                                            struct cfg80211_wowlan *wow)
{
        struct wl1271 *wl = hw->priv;
        struct wl12xx_vif *wlvif;
        unsigned long flags;
        int ret;

        wl1271_debug(DEBUG_MAC80211, "mac80211 suspend wow=%d", !!wow);
        WARN_ON(!wow);

        /* we want to perform the recovery before suspending */
        if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
                wl1271_warning("postponing suspend to perform recovery");
                return -EBUSY;
        }

        wl1271_tx_flush(wl);

        mutex_lock(&wl->mutex);

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                mutex_unlock(&wl->mutex);
                return ret;
        }

        wl->wow_enabled = true;
        wl12xx_for_each_wlvif(wl, wlvif) {
                if (wlcore_is_p2p_mgmt(wlvif))
                        continue;

                ret = wl1271_configure_suspend(wl, wlvif, wow);
                if (ret < 0) {
                        goto out_sleep;
                }
        }

        /* disable fast link flow control notifications from FW */
        ret = wlcore_hw_interrupt_notify(wl, false);
        if (ret < 0)
                goto out_sleep;

        /* if filtering is enabled, configure the FW to drop all RX BA frames */
        ret = wlcore_hw_rx_ba_filter(wl,
                                     !!wl->conf.conn.suspend_rx_ba_activity);
        if (ret < 0)
                goto out_sleep;

out_sleep:
        pm_runtime_put_noidle(wl->dev);
        mutex_unlock(&wl->mutex);

        if (ret < 0) {
                wl1271_warning("couldn't prepare device to suspend");
                return ret;
        }

        /* flush any remaining work */
        wl1271_debug(DEBUG_MAC80211, "flushing remaining works");

        flush_work(&wl->tx_work);

        /*
         * Cancel the watchdog even if above tx_flush failed. We will detect
         * it on resume anyway.
         */
        cancel_delayed_work(&wl->tx_watchdog_work);

        /*
         * set suspended flag to avoid triggering a new threaded_irq
         * work.
         */
        spin_lock_irqsave(&wl->wl_lock, flags);
        set_bit(WL1271_FLAG_SUSPENDED, &wl->flags);
        spin_unlock_irqrestore(&wl->wl_lock, flags);

        return pm_runtime_force_suspend(wl->dev);
}

static int __maybe_unused wl1271_op_resume(struct ieee80211_hw *hw)
{
        struct wl1271 *wl = hw->priv;
        struct wl12xx_vif *wlvif;
        unsigned long flags;
        bool run_irq_work = false, pending_recovery;
        int ret;

        wl1271_debug(DEBUG_MAC80211, "mac80211 resume wow=%d",
                     wl->wow_enabled);
        WARN_ON(!wl->wow_enabled);

        ret = pm_runtime_force_resume(wl->dev);
        if (ret < 0) {
                wl1271_error("ELP wakeup failure!");
                goto out_sleep;
        }

        /*
         * re-enable irq_work enqueuing, and call irq_work directly if
         * there is a pending work.
         */
        spin_lock_irqsave(&wl->wl_lock, flags);
        clear_bit(WL1271_FLAG_SUSPENDED, &wl->flags);
        if (test_and_clear_bit(WL1271_FLAG_PENDING_WORK, &wl->flags))
                run_irq_work = true;
        spin_unlock_irqrestore(&wl->wl_lock, flags);

        mutex_lock(&wl->mutex);

        /* test the recovery flag before calling any SDIO functions */
        pending_recovery = test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
                                    &wl->flags);

        if (run_irq_work) {
                wl1271_debug(DEBUG_MAC80211,
                             "run postponed irq_work directly");

                /* don't talk to the HW if recovery is pending */
                if (!pending_recovery) {
                        ret = wlcore_irq_locked(wl);
                        if (ret)
                                wl12xx_queue_recovery_work(wl);
                }

                wlcore_enable_interrupts(wl);
        }

        if (pending_recovery) {
                wl1271_warning("queuing forgotten recovery on resume");
                ieee80211_queue_work(wl->hw, &wl->recovery_work);
                goto out_sleep;
        }

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        wl12xx_for_each_wlvif(wl, wlvif) {
                if (wlcore_is_p2p_mgmt(wlvif))
                        continue;

                wl1271_configure_resume(wl, wlvif);
        }

        ret = wlcore_hw_interrupt_notify(wl, true);
        if (ret < 0)
                goto out_sleep;

        /* if filtering is enabled, configure the FW to drop all RX BA frames */
        ret = wlcore_hw_rx_ba_filter(wl, false);
        if (ret < 0)
                goto out_sleep;

out_sleep:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);

out:
        wl->wow_enabled = false;

        /*
         * Set a flag to re-init the watchdog on the first Tx after resume.
         * That way we avoid possible conditions where Tx-complete interrupts
         * fail to arrive and we perform a spurious recovery.
         */
        set_bit(WL1271_FLAG_REINIT_TX_WDOG, &wl->flags);
        mutex_unlock(&wl->mutex);

        return 0;
}

static int wl1271_op_start(struct ieee80211_hw *hw)
{
        wl1271_debug(DEBUG_MAC80211, "mac80211 start");

        /*
         * We have to delay the booting of the hardware because
         * we need to know the local MAC address before downloading and
         * initializing the firmware. The MAC address cannot be changed
         * after boot, and without the proper MAC address, the firmware
         * will not function properly.
         *
         * The MAC address is first known when the corresponding interface
         * is added. That is where we will initialize the hardware.
         */

        return 0;
}

static void wlcore_op_stop_locked(struct wl1271 *wl)
{
        int i;

        if (wl->state == WLCORE_STATE_OFF) {
                if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS,
                                        &wl->flags))
                        wlcore_enable_interrupts(wl);

                return;
        }

        /*
         * this must be before the cancel_work calls below, so that the work
         * functions don't perform further work.
         */
        wl->state = WLCORE_STATE_OFF;

        /*
         * Use the nosync variant to disable interrupts, so the mutex could be
         * held while doing so without deadlocking.
         */
        wlcore_disable_interrupts_nosync(wl);

        mutex_unlock(&wl->mutex);

        wlcore_synchronize_interrupts(wl);
        if (!test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
                cancel_work_sync(&wl->recovery_work);
        wl1271_flush_deferred_work(wl);
        cancel_delayed_work_sync(&wl->scan_complete_work);
        cancel_work_sync(&wl->netstack_work);
        cancel_work_sync(&wl->tx_work);
        cancel_delayed_work_sync(&wl->tx_watchdog_work);

        /* let's notify MAC80211 about the remaining pending TX frames */
        mutex_lock(&wl->mutex);
        wl12xx_tx_reset(wl);

        wl1271_power_off(wl);
        /*
         * In case a recovery was scheduled, interrupts were disabled to avoid
         * an interrupt storm. Now that the power is down, it is safe to
         * re-enable interrupts to balance the disable depth
         */
        if (test_and_clear_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags))
                wlcore_enable_interrupts(wl);

        wl->band = NL80211_BAND_2GHZ;

        wl->rx_counter = 0;
        wl->power_level = WL1271_DEFAULT_POWER_LEVEL;
        wl->channel_type = NL80211_CHAN_NO_HT;
        wl->tx_blocks_available = 0;
        wl->tx_allocated_blocks = 0;
        wl->tx_results_count = 0;
        wl->tx_packets_count = 0;
        wl->time_offset = 0;
        wl->ap_fw_ps_map = 0;
        wl->ap_ps_map = 0;
        wl->sleep_auth = WL1271_PSM_ILLEGAL;
        memset(wl->roles_map, 0, sizeof(wl->roles_map));
        memset(wl->links_map, 0, sizeof(wl->links_map));
        memset(wl->roc_map, 0, sizeof(wl->roc_map));
        memset(wl->session_ids, 0, sizeof(wl->session_ids));
        memset(wl->rx_filter_enabled, 0, sizeof(wl->rx_filter_enabled));
        wl->active_sta_count = 0;
        wl->active_link_count = 0;

        /* The system link is always allocated */
        wl->links[WL12XX_SYSTEM_HLID].allocated_pkts = 0;
        wl->links[WL12XX_SYSTEM_HLID].prev_freed_pkts = 0;
        __set_bit(WL12XX_SYSTEM_HLID, wl->links_map);

        /*
         * this is performed after the cancel_work calls and the associated
         * mutex_lock, so that wl1271_op_add_interface does not accidentally
         * get executed before all these vars have been reset.
         */
        wl->flags = 0;

        wl->tx_blocks_freed = 0;

        for (i = 0; i < NUM_TX_QUEUES; i++) {
                wl->tx_pkts_freed[i] = 0;

                wl->tx_allocated_pkts[i] = 0;
        }

        wl1271_debugfs_reset(wl);

        kfree(wl->raw_fw_status);
        wl->raw_fw_status = NULL;
        kfree(wl->fw_status);
        wl->fw_status = NULL;
        kfree(wl->tx_res_if);
        wl->tx_res_if = NULL;
        kfree(wl->target_mem_map);
        wl->target_mem_map = NULL;

        /*
         * FW channels must be re-calibrated after recovery,
         * save current Reg-Domain channel configuration and clear it.
         */
        memcpy(wl->reg_ch_conf_pending, wl->reg_ch_conf_last,
               sizeof(wl->reg_ch_conf_pending));
        memset(wl->reg_ch_conf_last, 0, sizeof(wl->reg_ch_conf_last));
}

static void wlcore_op_stop(struct ieee80211_hw *hw)
{
        struct wl1271 *wl = hw->priv;

        wl1271_debug(DEBUG_MAC80211, "mac80211 stop");

        mutex_lock(&wl->mutex);

        wlcore_op_stop_locked(wl);

        mutex_unlock(&wl->mutex);
}

static void wlcore_channel_switch_work(struct work_struct *work)
{
        struct delayed_work *dwork;
        struct wl1271 *wl;
        struct ieee80211_vif *vif;
        struct wl12xx_vif *wlvif;
        int ret;

        dwork = to_delayed_work(work);
        wlvif = container_of(dwork, struct wl12xx_vif, channel_switch_work);
        wl = wlvif->wl;

        wl1271_info("channel switch failed (role_id: %d).", wlvif->role_id);

        mutex_lock(&wl->mutex);

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        /* check the channel switch is still ongoing */
        if (!test_and_clear_bit(WLVIF_FLAG_CS_PROGRESS, &wlvif->flags))
                goto out;

        vif = wl12xx_wlvif_to_vif(wlvif);
        ieee80211_chswitch_done(vif, false);

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        wl12xx_cmd_stop_channel_switch(wl, wlvif);

        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out:
        mutex_unlock(&wl->mutex);
}

static void wlcore_connection_loss_work(struct work_struct *work)
{
        struct delayed_work *dwork;
        struct wl1271 *wl;
        struct ieee80211_vif *vif;
        struct wl12xx_vif *wlvif;

        dwork = to_delayed_work(work);
        wlvif = container_of(dwork, struct wl12xx_vif, connection_loss_work);
        wl = wlvif->wl;

        wl1271_info("Connection loss work (role_id: %d).", wlvif->role_id);

        mutex_lock(&wl->mutex);

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        /* Call mac80211 connection loss */
        if (!test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
                goto out;

        vif = wl12xx_wlvif_to_vif(wlvif);
        ieee80211_connection_loss(vif);
out:
        mutex_unlock(&wl->mutex);
}

static void wlcore_pending_auth_complete_work(struct work_struct *work)
{
        struct delayed_work *dwork;
        struct wl1271 *wl;
        struct wl12xx_vif *wlvif;
        unsigned long time_spare;
        int ret;

        dwork = to_delayed_work(work);
        wlvif = container_of(dwork, struct wl12xx_vif,
                             pending_auth_complete_work);
        wl = wlvif->wl;

        mutex_lock(&wl->mutex);

        if (unlikely(wl->state != WLCORE_STATE_ON))
                goto out;

        /*
         * Make sure a second really passed since the last auth reply. Maybe
         * a second auth reply arrived while we were stuck on the mutex.
         * Check for a little less than the timeout to protect from scheduler
         * irregularities.
         */
        time_spare = jiffies +
                        msecs_to_jiffies(WLCORE_PEND_AUTH_ROC_TIMEOUT - 50);
        if (!time_after(time_spare, wlvif->pending_auth_reply_time))
                goto out;

        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out;
        }

        /* cancel the ROC if active */
        wlcore_update_inconn_sta(wl, wlvif, NULL, false);

        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out:
        mutex_unlock(&wl->mutex);
}

static int wl12xx_allocate_rate_policy(struct wl1271 *wl, u8 *idx)
{
        u8 policy = find_first_zero_bit(wl->rate_policies_map,
                                        WL12XX_MAX_RATE_POLICIES);
        if (policy >= WL12XX_MAX_RATE_POLICIES)
                return -EBUSY;

        __set_bit(policy, wl->rate_policies_map);
        *idx = policy;
        return 0;
}

static void wl12xx_free_rate_policy(struct wl1271 *wl, u8 *idx)
{
        if (WARN_ON(*idx >= WL12XX_MAX_RATE_POLICIES))
                return;

        __clear_bit(*idx, wl->rate_policies_map);
        *idx = WL12XX_MAX_RATE_POLICIES;
}

static int wlcore_allocate_klv_template(struct wl1271 *wl, u8 *idx)
{
        u8 policy = find_first_zero_bit(wl->klv_templates_map,
                                        WLCORE_MAX_KLV_TEMPLATES);
        if (policy >= WLCORE_MAX_KLV_TEMPLATES)
                return -EBUSY;

        __set_bit(policy, wl->klv_templates_map);
        *idx = policy;
        return 0;
}

static void wlcore_free_klv_template(struct wl1271 *wl, u8 *idx)
{
        if (WARN_ON(*idx >= WLCORE_MAX_KLV_TEMPLATES))
                return;

        __clear_bit(*idx, wl->klv_templates_map);
        *idx = WLCORE_MAX_KLV_TEMPLATES;
}

static u8 wl12xx_get_role_type(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);

        switch (wlvif->bss_type) {
        case BSS_TYPE_AP_BSS:
                if (wlvif->p2p)
                        return WL1271_ROLE_P2P_GO;
                else if (ieee80211_vif_is_mesh(vif))
                        return WL1271_ROLE_MESH_POINT;
                else
                        return WL1271_ROLE_AP;

        case BSS_TYPE_STA_BSS:
                if (wlvif->p2p)
                        return WL1271_ROLE_P2P_CL;
                else
                        return WL1271_ROLE_STA;

        case BSS_TYPE_IBSS:
                return WL1271_ROLE_IBSS;

        default:
                wl1271_error("invalid bss_type: %d", wlvif->bss_type);
        }
        return WL12XX_INVALID_ROLE_TYPE;
}

static int wl12xx_init_vif_data(struct wl1271 *wl, struct ieee80211_vif *vif)
{
        struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
        int i;

        /* clear everything but the persistent data */
        memset(wlvif, 0, offsetof(struct wl12xx_vif, persistent));

        switch (ieee80211_vif_type_p2p(vif)) {
        case NL80211_IFTYPE_P2P_CLIENT:
                wlvif->p2p = 1;
                fallthrough;
        case NL80211_IFTYPE_STATION:
        case NL80211_IFTYPE_P2P_DEVICE:
                wlvif->bss_type = BSS_TYPE_STA_BSS;
                break;
        case NL80211_IFTYPE_ADHOC:
                wlvif->bss_type = BSS_TYPE_IBSS;
                break;
        case NL80211_IFTYPE_P2P_GO:
                wlvif->p2p = 1;
                fallthrough;
        case NL80211_IFTYPE_AP:
        case NL80211_IFTYPE_MESH_POINT:
                wlvif->bss_type = BSS_TYPE_AP_BSS;
                break;
        default:
                wlvif->bss_type = MAX_BSS_TYPE;
                return -EOPNOTSUPP;
        }

        wlvif->role_id = WL12XX_INVALID_ROLE_ID;
        wlvif->dev_role_id = WL12XX_INVALID_ROLE_ID;
        wlvif->dev_hlid = WL12XX_INVALID_LINK_ID;

        if (wlvif->bss_type == BSS_TYPE_STA_BSS ||
            wlvif->bss_type == BSS_TYPE_IBSS) {
                /* init sta/ibss data */
                wlvif->sta.hlid = WL12XX_INVALID_LINK_ID;
                wl12xx_allocate_rate_policy(wl, &wlvif->sta.basic_rate_idx);
                wl12xx_allocate_rate_policy(wl, &wlvif->sta.ap_rate_idx);
                wl12xx_allocate_rate_policy(wl, &wlvif->sta.p2p_rate_idx);
                wlcore_allocate_klv_template(wl, &wlvif->sta.klv_template_id);
                wlvif->basic_rate_set = CONF_TX_RATE_MASK_BASIC;
                wlvif->basic_rate = CONF_TX_RATE_MASK_BASIC;
                wlvif->rate_set = CONF_TX_RATE_MASK_BASIC;
        } else {
                /* init ap data */
                wlvif->ap.bcast_hlid = WL12XX_INVALID_LINK_ID;
                wlvif->ap.global_hlid = WL12XX_INVALID_LINK_ID;
                wl12xx_allocate_rate_policy(wl, &wlvif->ap.mgmt_rate_idx);
                wl12xx_allocate_rate_policy(wl, &wlvif->ap.bcast_rate_idx);
                for (i = 0; i < CONF_TX_MAX_AC_COUNT; i++)
                        wl12xx_allocate_rate_policy(wl,
                                                &wlvif->ap.ucast_rate_idx[i]);
                wlvif->basic_rate_set = CONF_TX_ENABLED_RATES;
                /*
                 * TODO: check if basic_rate shouldn't be
                 * wl1271_tx_min_rate_get(wl, wlvif->basic_rate_set);
                 * instead (the same thing for STA above).
                */
                wlvif->basic_rate = CONF_TX_ENABLED_RATES;
                /* TODO: this seems to be used only for STA, check it */
                wlvif->rate_set = CONF_TX_ENABLED_RATES;
        }

        wlvif->bitrate_masks[NL80211_BAND_2GHZ] = wl->conf.tx.basic_rate;
        wlvif->bitrate_masks[NL80211_BAND_5GHZ] = wl->conf.tx.basic_rate_5;
        wlvif->beacon_int = WL1271_DEFAULT_BEACON_INT;

        /*
         * mac80211 configures some values globally, while we treat them
         * per-interface. thus, on init, we have to copy them from wl
         */
        wlvif->band = wl->band;
        wlvif->channel = wl->channel;
        wlvif->power_level = wl->power_level;
        wlvif->channel_type = wl->channel_type;

        INIT_WORK(&wlvif->rx_streaming_enable_work,
                  wl1271_rx_streaming_enable_work);
        INIT_WORK(&wlvif->rx_streaming_disable_work,
                  wl1271_rx_streaming_disable_work);
        INIT_WORK(&wlvif->rc_update_work, wlcore_rc_update_work);
        INIT_DELAYED_WORK(&wlvif->channel_switch_work,
                          wlcore_channel_switch_work);
        INIT_DELAYED_WORK(&wlvif->connection_loss_work,
                          wlcore_connection_loss_work);
        INIT_DELAYED_WORK(&wlvif->pending_auth_complete_work,
                          wlcore_pending_auth_complete_work);
        INIT_LIST_HEAD(&wlvif->list);

        timer_setup(&wlvif->rx_streaming_timer, wl1271_rx_streaming_timer, 0);
        return 0;
}

static int wl12xx_init_fw(struct wl1271 *wl)
{
        int retries = WL1271_BOOT_RETRIES;
        bool booted = false;
        struct wiphy *wiphy = wl->hw->wiphy;
        int ret;

        while (retries) {
                retries--;
                ret = wl12xx_chip_wakeup(wl, false);
                if (ret < 0)
                        goto power_off;

                ret = wl->ops->boot(wl);
                if (ret < 0)
                        goto power_off;

                ret = wl1271_hw_init(wl);
                if (ret < 0)
                        goto irq_disable;

                booted = true;
                break;

irq_disable:
                mutex_unlock(&wl->mutex);
                /* Unlocking the mutex in the middle of handling is
                   inherently unsafe. In this case we deem it safe to do,
                   because we need to let any possibly pending IRQ out of
                   the system (and while we are WLCORE_STATE_OFF the IRQ
                   work function will not do anything.) Also, any other
                   possible concurrent operations will fail due to the
                   current state, hence the wl1271 struct should be safe. */
                wlcore_disable_interrupts(wl);
                wl1271_flush_deferred_work(wl);
                cancel_work_sync(&wl->netstack_work);
                mutex_lock(&wl->mutex);
power_off:
                wl1271_power_off(wl);
        }

        if (!booted) {
                wl1271_error("firmware boot failed despite %d retries",
                             WL1271_BOOT_RETRIES);
                goto out;
        }

        wl1271_info("firmware booted (%s)", wl->chip.fw_ver_str);

        /* update hw/fw version info in wiphy struct */
        wiphy->hw_version = wl->chip.id;
        strncpy(wiphy->fw_version, wl->chip.fw_ver_str,
                sizeof(wiphy->fw_version));

        /*
         * Now we know if 11a is supported (info from the NVS), so disable
         * 11a channels if not supported
         */
        if (!wl->enable_11a)
                wiphy->bands[NL80211_BAND_5GHZ]->n_channels = 0;

        wl1271_debug(DEBUG_MAC80211, "11a is %ssupported",
                     wl->enable_11a ? "" : "not ");

        wl->state = WLCORE_STATE_ON;
out:
        return ret;
}

static bool wl12xx_dev_role_started(struct wl12xx_vif *wlvif)
{
        return wlvif->dev_hlid != WL12XX_INVALID_LINK_ID;
}

/*
 * Check whether a fw switch (i.e. moving from one loaded
 * fw to another) is needed. This function is also responsible
 * for updating wl->last_vif_count, so it must be called before
 * loading a non-plt fw (so the correct fw (single-role/multi-role)
 * will be used).
 */
static bool wl12xx_need_fw_change(struct wl1271 *wl,
                                  struct vif_counter_data vif_counter_data,
                                  bool add)
{
        enum wl12xx_fw_type current_fw = wl->fw_type;
        u8 vif_count = vif_counter_data.counter;

        if (test_bit(WL1271_FLAG_VIF_CHANGE_IN_PROGRESS, &wl->flags))
                return false;

        /* increase the vif count if this is a new vif */
        if (add && !vif_counter_data.cur_vif_running)
                vif_count++;

        wl->last_vif_count = vif_count;

        /* no need for fw change if the device is OFF */
        if (wl->state == WLCORE_STATE_OFF)
                return false;

        /* no need for fw change if a single fw is used */
        if (!wl->mr_fw_name)
                return false;

        if (vif_count > 1 && current_fw == WL12XX_FW_TYPE_NORMAL)
                return true;
        if (vif_count <= 1 && current_fw == WL12XX_FW_TYPE_MULTI)
                return true;

        return false;
}

/*
 * Enter "forced psm". Make sure the sta is in psm against the ap,
 * to make the fw switch a bit more disconnection-persistent.
 */
static void wl12xx_force_active_psm(struct wl1271 *wl)
{
        struct wl12xx_vif *wlvif;

        wl12xx_for_each_wlvif_sta(wl, wlvif) {
                wl1271_ps_set_mode(wl, wlvif, STATION_POWER_SAVE_MODE);
        }
}

struct wlcore_hw_queue_iter_data {
        unsigned long hw_queue_map[BITS_TO_LONGS(WLCORE_NUM_MAC_ADDRESSES)];
        /* current vif */
        struct ieee80211_vif *vif;
        /* is the current vif among those iterated */
        bool cur_running;
};

static void wlcore_hw_queue_iter(void *data, u8 *mac,
                                 struct ieee80211_vif *vif)
{
        struct wlcore_hw_queue_iter_data *iter_data = data;

        if (vif->type == NL80211_IFTYPE_P2P_DEVICE ||
            WARN_ON_ONCE(vif->hw_queue[0] == IEEE80211_INVAL_HW_QUEUE))
                return;

        if (iter_data->cur_running || vif == iter_data->vif) {
                iter_data->cur_running = true;
                return;
        }

        __set_bit(vif->hw_queue[0] / NUM_TX_QUEUES, iter_data->hw_queue_map);
}

static int wlcore_allocate_hw_queue_base(struct wl1271 *wl,
                                         struct wl12xx_vif *wlvif)
{
        struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
        struct wlcore_hw_queue_iter_data iter_data = {};
        int i, q_base;

        if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
                vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;
                return 0;
        }

        iter_data.vif = vif;

        /* mark all bits taken by active interfaces */
        ieee80211_iterate_active_interfaces_atomic(wl->hw,
                                        IEEE80211_IFACE_ITER_RESUME_ALL,
                                        wlcore_hw_queue_iter, &iter_data);

        /* the current vif is already running in mac80211 (resume/recovery) */
        if (iter_data.cur_running) {
                wlvif->hw_queue_base = vif->hw_queue[0];
                wl1271_debug(DEBUG_MAC80211,
                             "using pre-allocated hw queue base %d",
                             wlvif->hw_queue_base);

                /* interface type might have changed type */
                goto adjust_cab_queue;
        }

        q_base = find_first_zero_bit(iter_data.hw_queue_map,
                                     WLCORE_NUM_MAC_ADDRESSES);
        if (q_base >= WLCORE_NUM_MAC_ADDRESSES)
                return -EBUSY;

        wlvif->hw_queue_base = q_base * NUM_TX_QUEUES;
        wl1271_debug(DEBUG_MAC80211, "allocating hw queue base: %d",
                     wlvif->hw_queue_base);

        for (i = 0; i < NUM_TX_QUEUES; i++) {
                wl->queue_stop_reasons[wlvif->hw_queue_base + i] = 0;
                /* register hw queues in mac80211 */
                vif->hw_queue[i] = wlvif->hw_queue_base + i;
        }

adjust_cab_queue:
        /* the last places are reserved for cab queues per interface */
        if (wlvif->bss_type == BSS_TYPE_AP_BSS)
                vif->cab_queue = NUM_TX_QUEUES * WLCORE_NUM_MAC_ADDRESSES +
                                 wlvif->hw_queue_base / NUM_TX_QUEUES;
        else
                vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;

        return 0;
}

static int wl1271_op_add_interface(struct ieee80211_hw *hw,
                                   struct ieee80211_vif *vif)
{
        struct wl1271 *wl = hw->priv;
        struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
        struct vif_counter_data vif_count;
        int ret = 0;
        u8 role_type;

        if (wl->plt) {
                wl1271_error("Adding Interface not allowed while in PLT mode");
                return -EBUSY;
        }

        vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER |
                             IEEE80211_VIF_SUPPORTS_UAPSD |
                             IEEE80211_VIF_SUPPORTS_CQM_RSSI;

        wl1271_debug(DEBUG_MAC80211, "mac80211 add interface type %d mac %pM",
                     ieee80211_vif_type_p2p(vif), vif->addr);

        wl12xx_get_vif_count(hw, vif, &vif_count);

        mutex_lock(&wl->mutex);

        /*
         * in some very corner case HW recovery scenarios its possible to
         * get here before __wl1271_op_remove_interface is complete, so
         * opt out if that is the case.
         */
        if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags) ||
            test_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags)) {
                ret = -EBUSY;
                goto out;
        }


        ret = wl12xx_init_vif_data(wl, vif);
        if (ret < 0)
                goto out;

        wlvif->wl = wl;
        role_type = wl12xx_get_role_type(wl, wlvif);
        if (role_type == WL12XX_INVALID_ROLE_TYPE) {
                ret = -EINVAL;
                goto out;
        }

        ret = wlcore_allocate_hw_queue_base(wl, wlvif);
        if (ret < 0)
                goto out;

        /*
         * TODO: after the nvs issue will be solved, move this block
         * to start(), and make sure here the driver is ON.
         */
        if (wl->state == WLCORE_STATE_OFF) {
                /*
                 * we still need this in order to configure the fw
                 * while uploading the nvs
                 */
                memcpy(wl->addresses[0].addr, vif->addr, ETH_ALEN);

                ret = wl12xx_init_fw(wl);
                if (ret < 0)
                        goto out;
        }

        /*
         * Call runtime PM only after possible wl12xx_init_fw() above
         * is done. Otherwise we do not have interrupts enabled.
         */
        ret = pm_runtime_get_sync(wl->dev);
        if (ret < 0) {
                pm_runtime_put_noidle(wl->dev);
                goto out_unlock;
        }

        if (wl12xx_need_fw_change(wl, vif_count, true)) {
                wl12xx_force_active_psm(wl);
                set_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags);
                mutex_unlock(&wl->mutex);
                wl1271_recovery_work(&wl->recovery_work);
                return 0;
        }

        if (!wlcore_is_p2p_mgmt(wlvif)) {
                ret = wl12xx_cmd_role_enable(wl, vif->addr,
                                             role_type, &wlvif->role_id);
                if (ret < 0)
                        goto out;

                ret = wl1271_init_vif_specific(wl, vif);
                if (ret < 0)
                        goto out;

        } else {
                ret = wl12xx_cmd_role_enable(wl, vif->addr, WL1271_ROLE_DEVICE,
                                             &wlvif->dev_role_id);
                if (ret < 0)
                        goto out;

                /* needed mainly for configuring rate policies */
                ret = wl1271_sta_hw_init(wl, wlvif);
                if (ret < 0)
                        goto out;
        }

        list_add(&wlvif->list, &wl->wlvif_list);
        set_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags);

        if (wlvif->bss_type == BSS_TYPE_AP_BSS)
                wl->ap_count++;
        else
                wl->sta_count++;
out:
        pm_runtime_mark_last_busy(wl->dev);
        pm_runtime_put_autosuspend(wl->dev);
out_unlock:
        mutex_unlock(&wl->mutex);

        return ret;
}

static void __wl1271_op_remove_interface(struct wl1271 *wl,
                                         struct ieee80211_vif *vif,
                                         bool reset_tx_queues)
{
        struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
        int i, ret;
        bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);

        wl1271_debug(DEBUG_MAC80211, "mac80211 remove interface");

        if (!test_and_clear_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags))
                return;

        /* because of hardware recovery, we may get here twice */
        if (wl->state == WLCORE_STATE_OFF)
                return;

        wl1271_info("down");

        if (wl->scan.state != WL1271_SCAN_STATE_IDLE &&
            wl->scan_wlvif == wlvif) {
                struct cfg80211_scan_info info = {
                        .aborted = true,
                };

                /*
                 * Rearm the tx watchdog just before idling scan. This
                 * prevents just-finished scans from triggering the watchdog
                 */
                wl12xx_rearm_tx_watchdog_locked(wl);

                wl->scan.state = WL1271_SCAN_STATE_IDLE;
                memset(wl->scan.scanned_ch, 0, sizeof(wl->scan.scanned_ch));
                wl->scan_wlvif = NULL;
                wl->scan.req = NULL;
                ieee80211_scan_completed(wl->hw, &info);
        }

        if (wl->sched_vif == wlvif)
                wl->sched_vif = NULL;

        if (wl->roc_vif == vif) {
                wl->roc_vif = NULL;
                ieee80211_remain_on_channel_expired(wl->hw);
        }

        if (!test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags)) {
                /* disable active roles */
                ret = pm_runtime_get_sync(wl->dev);
                if (ret < 0) {
                        pm_runtime_put_noidle(wl->dev);
                        goto deinit;
                }

                if (wlvif->bss_type == BSS_TYPE_STA_BSS ||
                    wlvif->bss_type == BSS_TYPE_IBSS) {
                        if (wl12xx_dev_role_started(wlvif))
                                wl12xx_stop_dev(wl, wlvif);
                }

                if (!wlcore_is_p2p_mgmt(wlvif)) {
                        ret = wl12xx_cmd_role_disable(wl, &wlvif->role_id);
                        if (ret < 0) {
                                pm_runtime_put_noidle(wl->dev);
                                goto deinit;
                        }
                } else {
                        ret = wl12xx_cmd_role_disable(wl, &wlvif->dev_role_id);
                        if (ret < 0) {
                                pm_runtime_put_noidle(wl->dev);
                                goto deinit;
                        }
                }

                pm_runtime_mark_last_busy(wl->dev);
                pm_runtime_put_autosuspend(wl->dev);
        }
deinit:
        wl12xx_tx_reset_wlvif(wl, wlvif);

        /* clear all hlids (except system_hlid) */
        wlvif->dev_hlid = WL12XX_INVALID_LINK_ID;

        if (wlvif->bss_type == BSS_TYPE_STA_BSS ||
            wlvif->bss_type == BSS_TYPE_IBSS) {
                wlvif->sta.hlid = WL12XX_INVALID_LINK_ID;
                wl12xx_free_rate_policy(wl, &wlvif->sta.basic_rate_idx);
                wl12xx_free_rate_policy(wl, &wlvif->sta.ap_rate_idx);
                wl12xx_free_rate_policy(wl, &wlvif->sta.p2p_rate_idx);
                wlcore_free_klv_template(wl, &wlvif->sta.klv_template_id);
        } else {
                wlvif->ap.bcast_hlid = WL12XX_INVALID_LINK_ID;
                wlvif->ap.global_hlid = WL12XX_INVALID_LINK_ID;
                wl12xx_free_rate_policy(wl, &wlvif->ap.mgmt_rate_idx);
                wl12xx_free_rate_policy(wl, &wlvif->ap.bcast_rate_idx);
                for (i = 0; i < CONF_TX_MAX_AC_COUNT; i++)
                        wl12xx_free_rate_policy(wl,
                                                &wlvif->ap.ucast_rate_idx[i]);
                wl1271_free_ap_keys(wl, wlvif);
        }

        dev_kfree_skb(wlvif->probereq);
        wlvif->probereq = NULL;
        if (wl->last_wlvif == wlvif)
                wl->last_wlvif = NULL;
        list_del(&wlvif->list);
        memset(wlvif->ap.sta_hlid_map, 0, sizeof(wlvif->ap.sta_hlid_map));
        wlvif->role_id = WL12XX_INVALID_ROLE_ID;
        wlvif->dev_role_id = WL12XX_INVALID_ROLE_ID;

        if (is_ap)
                wl->ap_count--;
        else
                wl->sta_count--;

        /*
         * Last AP, have more stations. Configure sleep auth according to STA.
         * Don't do thin on unintended recovery.
         */
        if (test_bit(WL1271_FLAG_RECOVERY_IN_PROGRESS, &wl->flags) &&
            !test_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags))
                goto unlock;

        if (wl->ap_count == 0 && is_ap) {
                /* mask ap events */
                wl->event_mask &= ~wl->ap_event_mask;
                wl1271_event_unmask(wl);
        }

        if (wl->ap_count == 0 && is_ap && wl->sta_count) {
                u8 sta_auth = wl->conf.conn.sta_sleep_auth;
                /* Configure for power according to debugfs */
                if (sta_auth != WL1271_PSM_ILLEGAL)
                        wl1271_acx_sleep_auth(wl, sta_auth);
                /* Configure for ELP power saving */
                else
                        wl1271_acx_sleep_auth(wl, WL1271_PSM_ELP);
        }

unlock:
        mutex_unlock(&wl->mutex);

        del_timer_sync(&wlvif->rx_streaming_timer);
        cancel_work_sync(&wlvif->rx_streaming_enable_work);
        cancel_work_sync(&wlvif->rx_streaming_disable_work);
        cancel_work_sync(&wlvif->rc_update_work);
        cancel_delayed_work_sync(&wlvif->connection_loss_work);
        cancel_delayed_work_sync(&wlvif->channel_switch_work);
        cancel_delayed_work_sync(&wlvif->pending_auth_complete_work);

        mutex_lock(&wl->mutex);
}

static void wl1271_op_remove_interface(struct ieee80211_hw *hw,
                                       struct ieee80211_vif *vif)
{
        struct wl1271 *wl = hw->priv;
        struct wl12xx_vif *wlvif = wl12xx_vif_to_data(vif);
        struct wl12xx_vif *iter;
        struct vif_counter_data vif_count;

        wl12xx_get_vif_count(hw, vif, &vif_count);
        mutex_lock(&wl->mutex);

        if (wl->state == WLCORE_STATE_OFF ||
            !test_bit(WLVIF_FLAG_INITIALIZED, &wlvif->flags))
                goto out;

        /*
         * wl->vif can be null here if someone shuts down the interface
         * just when hardware recovery has been started.
         */
        wl12xx_for_each_wlvif(wl, iter) {
                if (iter != wlvif)
                        continue;

                __wl1271_op_remove_interface(wl, vif, true);
                break;
        }
        WARN_ON(iter != wlvif);
        if (wl12xx_need_fw_change(wl, vif_count, false)) {
                wl12xx_force_active_psm(wl);
                set_bit(WL1271_FLAG_INTENDED_FW_RECOVERY, &wl->flags);
                wl12xx_queue_recovery_work(wl);
        }
out:
        mutex_unlock(&wl->mutex);
}

static int wl12xx_op_change_interface(struct ieee80211_hw *hw,
                                      struct ieee80211_vif *vif,
                                      enum nl80211_iftype new_type, bool p2p)
{
        struct wl1271 *wl = hw->priv;
        int ret;

        set_bit(WL1271_FLAG_VIF_CHANGE_IN_PROGRESS, &wl->flags);
        wl1271_op_remove_interface(hw, vif);

        vif->type = new_type;
        vif->p2p = p2p;
        ret = wl1271_op_add_interface(hw, vif);

        clear_bit(WL1271_FLAG_VIF_CHANGE_IN_PROGRESS, &wl->flags);
        return ret;
}

static int wlcore_join(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        int ret;
        bool is_ibss = (wlvif->bss_type == BSS_TYPE_IBSS);

        /*
         * One of the side effects of the JOIN command is that is clears
         * WPA/WPA2 keys from the chipset. Performing a JOIN while associated
         * to a WPA/WPA2 access point will therefore kill the data-path.
         * Currently the only valid scenario for JOIN during association
         * is on roaming, in which case we will also be given new keys.
         * Keep the below message for now, unless it starts bothering
         * users who really like to roam a lot :)
         */
        if (test_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags))
                wl1271_info("JOIN while associated.");

        /* clear encryption type */
        wlvif->encryption_type = KEY_NONE;

        if (is_ibss)
                ret = wl12xx_cmd_role_start_ibss(wl, wlvif);
        else
                ret = wl12xx_cmd_role_start_sta(wl, wlvif);

        return ret;
}

static int wl1271_ssid_set(struct wl12xx_vif *wlvif, struct sk_buff *skb,
                            int offset)
{
        u8 ssid_len;
        const u8 *ptr = cfg80211_find_ie(WLAN_EID_SSID, skb->data + offset,
                                         skb->len - offset);

        if (!ptr) {
                wl1271_error("No SSID in IEs!");
                return -ENOENT;
        }

        ssid_len = ptr[1];
        if (ssid_len > IEEE80211_MAX_SSID_LEN) {
                wl1271_error("SSID is too long!");
                return -EINVAL;
        }

        wlvif->ssid_len = ssid_len;
        memcpy(wlvif->ssid, ptr+2, ssid_len);
        return 0;
}

static int wlcore_set_ssid(struct wl1271 *wl, struct wl12xx_vif *wlvif)
{
        struct ieee80211_vif *vif = wl12xx_wlvif_to_vif(wlvif);
        struct sk_buff *skb;
        int ieoffset;

        /* we currently only support setting the ssid from the ap probe req */
        if (wlvif->bss_type != BSS_TYPE_STA_BSS)
                return -EINVAL;

        skb = ieee80211_ap_probereq_get(wl->hw, vif);
        if (!skb)
                return -EINVAL;

        ieoffset = offsetof(struct ieee80211_mgmt,
                            u.probe_req.variable);
        wl1271_ssid_set(wlvif, skb, ieoffset);
        dev_kfree_skb(skb);

        return 0;
}

static int wlcore_set_assoc(struct wl1271 *wl, struct wl12xx_vif *wlvif,
                            struct ieee80211_bss_conf *bss_conf,
                            u32 sta_rate_set)
{
        int ieoffset;
        int ret;

        wlvif->aid = bss_conf->aid;
        wlvif->channel_type = cfg80211_get_chandef_type(&bss_conf->chandef);
        wlvif->beacon_int = bss_conf->beacon_int;
        wlvif->wmm_enabled = bss_conf->qos;

        set_bit(WLVIF_FLAG_STA_ASSOCIATED, &wlvif->flags);

        /*
         * with wl1271, we don't need to update the
         * beacon_int and dtim_period, because the firmware
         * updates it by itself when the first beacon is
         * received after a join.
         */
        ret = wl1271_cmd_build_ps_poll(wl, wlvif, wlvif->aid);
        if (ret < 0)
                return ret;

        /*
         * Get a template for hardware connection maintenance
         */
        dev_kfree_skb(wlvif->probereq);
        wlvif->probereq = wl1271_cmd_build_ap_probe_req(wl,
                                                        wlvif,
                                                        NULL);
        ieoffset = offsetof(struct ieee80211_mgmt,
                            u.probe_req.variable);
        wl1271_ssid_set(wlvif, wlvif->probereq, ieoffset);

        /* enable the connection monitoring feature */
        ret = wl1271_acx_conn_monit_params(wl, wlvif, true);
        if (ret < 0)
                return ret;

        /*
         * The join command disable the keep-alive mode, shut down its process,
         * and also clear the template config, so we need to reset it all after
         * the join. The acx_aid starts the keep-alive process, and the order
         * of the commands below is relevant.
         */
        ret = wl1271_acx_keep_alive_mode(wl, wlvif, true);
        if (ret < 0)
                return ret;

        ret = wl1271_acx_aid(wl, wlvif, wlvif->aid);
        if (ret < 0)
                return ret;

        ret = wl12xx_cmd_build_klv_null_data(wl, wlvif);
        if (ret < 0)
                return ret;

        ret = wl1271_acx_keep_alive_config(wl, wlvif,
                                           wlvif->sta.klv_template_id,
                                           ACX_KEEP_ALIVE_TPL_VALID);
        if (ret < 0)
                return ret;

        /*
         * The default fw psm configuration is AUTO, while mac80211 default
         * setting is off (ACTIVE), so sync the fw with the correct value.
         */
        ret = wl1271_ps_set_mode(wl, wlvif, STATION_ACTIVE_MODE);
        if (ret < 0)
                return ret;

        if (sta_rate_set) {
                wlvif->rate_set =
                        wl1271_tx_enabled_rates_get(wl,