tx.c

   1// SPDX-License-Identifier: GPL-2.0-only
   2/*
   3 * This file is part of wl1271
   4 *
   5 * Copyright (C) 2009 Nokia Corporation
   6 *
   7 * Contact: Luciano Coelho <luciano.coelho@nokia.com>
   8 */
   9
  10#include <linux/kernel.h>
  11#include <linux/module.h>
  12#include <linux/etherdevice.h>
  13#include <linux/pm_runtime.h>
  14#include <linux/spinlock.h>
  15
  16#include "wlcore.h"
  17#include "debug.h"
  18#include "io.h"
  19#include "ps.h"
  20#include "tx.h"
  21#include "event.h"
  22#include "hw_ops.h"
  23
  24/*
  25 * TODO: this is here just for now, it must be removed when the data
  26 * operations are in place.
  27 */
  28#include "../wl12xx/reg.h"
  29
  30static int wl1271_set_default_wep_key(struct wl1271 *wl,
  31				      struct wl12xx_vif *wlvif, u8 id)
  32{
  33	int ret;
  34	bool is_ap = (wlvif->bss_type == BSS_TYPE_AP_BSS);
  35
  36	if (is_ap)
  37		ret = wl12xx_cmd_set_default_wep_key(wl, id,
  38						     wlvif->ap.bcast_hlid);
  39	else
  40		ret = wl12xx_cmd_set_default_wep_key(wl, id, wlvif->sta.hlid);
  41
  42	if (ret < 0)
  43		return ret;
  44
  45	wl1271_debug(DEBUG_CRYPT, "default wep key idx: %d", (int)id);
  46	return 0;
  47}
  48
  49static int wl1271_alloc_tx_id(struct wl1271 *wl, struct sk_buff *skb)
  50{
  51	int id;
  52
  53	id = find_first_zero_bit(wl->tx_frames_map, wl->num_tx_desc);
  54	if (id >= wl->num_tx_desc)
  55		return -EBUSY;
  56
  57	__set_bit(id, wl->tx_frames_map);
  58	wl->tx_frames[id] = skb;
  59	wl->tx_frames_cnt++;
  60	return id;
  61}
  62
  63void wl1271_free_tx_id(struct wl1271 *wl, int id)
  64{
  65	if (__test_and_clear_bit(id, wl->tx_frames_map)) {
  66		if (unlikely(wl->tx_frames_cnt == wl->num_tx_desc))
  67			clear_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);
  68
  69		wl->tx_frames[id] = NULL;
  70		wl->tx_frames_cnt--;
  71	}
  72}
  73EXPORT_SYMBOL(wl1271_free_tx_id);
  74
  75static void wl1271_tx_ap_update_inconnection_sta(struct wl1271 *wl,
  76						 struct wl12xx_vif *wlvif,
  77						 struct sk_buff *skb)
  78{
  79	struct ieee80211_hdr *hdr;
  80
  81	hdr = (struct ieee80211_hdr *)(skb->data +
  82				       sizeof(struct wl1271_tx_hw_descr));
  83	if (!ieee80211_is_auth(hdr->frame_control))
  84		return;
  85
  86	/*
  87	 * add the station to the known list before transmitting the
  88	 * authentication response. this way it won't get de-authed by FW
  89	 * when transmitting too soon.
  90	 */
  91	wl1271_acx_set_inconnection_sta(wl, wlvif, hdr->addr1);
  92
  93	/*
  94	 * ROC for 1 second on the AP channel for completing the connection.
  95	 * Note the ROC will be continued by the update_sta_state callbacks
  96	 * once the station reaches the associated state.
  97	 */
  98	wlcore_update_inconn_sta(wl, wlvif, NULL, true);
  99	wlvif->pending_auth_reply_time = jiffies;
 100	cancel_delayed_work(&wlvif->pending_auth_complete_work);
 101	ieee80211_queue_delayed_work(wl->hw,
 102				&wlvif->pending_auth_complete_work,
 103				msecs_to_jiffies(WLCORE_PEND_AUTH_ROC_TIMEOUT));
 104}
 105
 106static void wl1271_tx_regulate_link(struct wl1271 *wl,
 107				    struct wl12xx_vif *wlvif,
 108				    u8 hlid)
 109{
 110	bool fw_ps;
 111	u8 tx_pkts;
 112
 113	if (WARN_ON(!test_bit(hlid, wlvif->links_map)))
 114		return;
 115
 116	fw_ps = test_bit(hlid, &wl->ap_fw_ps_map);
 117	tx_pkts = wl->links[hlid].allocated_pkts;
 118
 119	/*
 120	 * if in FW PS and there is enough data in FW we can put the link
 121	 * into high-level PS and clean out its TX queues.
 122	 * Make an exception if this is the only connected link. In this
 123	 * case FW-memory congestion is less of a problem.
 124	 * Note that a single connected STA means 2*ap_count + 1 active links,
 125	 * since we must account for the global and broadcast AP links
 126	 * for each AP. The "fw_ps" check assures us the other link is a STA
 127	 * connected to the AP. Otherwise the FW would not set the PSM bit.
 128	 */
 129	if (wl->active_link_count > (wl->ap_count*2 + 1) && fw_ps &&
 130	    tx_pkts >= WL1271_PS_STA_MAX_PACKETS)
 131		wl12xx_ps_link_start(wl, wlvif, hlid, true);
 132}
 133
 134bool wl12xx_is_dummy_packet(struct wl1271 *wl, struct sk_buff *skb)
 135{
 136	return wl->dummy_packet == skb;
 137}
 138EXPORT_SYMBOL(wl12xx_is_dummy_packet);
 139
 140static u8 wl12xx_tx_get_hlid_ap(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 141				struct sk_buff *skb, struct ieee80211_sta *sta)
 142{
 143	if (sta && wl1271_station(sta)->fw_added) {
 144		return wl1271_station(sta)->hlid;
 145	} else {
 146		struct ieee80211_hdr *hdr;
 147
 148		if (!test_bit(WLVIF_FLAG_AP_STARTED, &wlvif->flags))
 149			return wl->system_hlid;
 150
 151		hdr = (struct ieee80211_hdr *)skb->data;
 152		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
 153			return wlvif->ap.bcast_hlid;
 154		else
 155			return wlvif->ap.global_hlid;
 156	}
 157}
 158
 159u8 wl12xx_tx_get_hlid(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 160		      struct sk_buff *skb, struct ieee80211_sta *sta)
 161{
 162	struct ieee80211_tx_info *control;
 163
 164	if (wlvif->bss_type == BSS_TYPE_AP_BSS)
 165		return wl12xx_tx_get_hlid_ap(wl, wlvif, skb, sta);
 166
 167	control = IEEE80211_SKB_CB(skb);
 168	if (control->flags & IEEE80211_TX_CTL_TX_OFFCHAN) {
 169		wl1271_debug(DEBUG_TX, "tx offchannel");
 170		return wlvif->dev_hlid;
 171	}
 172
 173	return wlvif->sta.hlid;
 174}
 175
 176unsigned int wlcore_calc_packet_alignment(struct wl1271 *wl,
 177					  unsigned int packet_length)
 178{
 179	if ((wl->quirks & WLCORE_QUIRK_TX_PAD_LAST_FRAME) ||
 180	    !(wl->quirks & WLCORE_QUIRK_TX_BLOCKSIZE_ALIGN))
 181		return ALIGN(packet_length, WL1271_TX_ALIGN_TO);
 182	else
 183		return ALIGN(packet_length, WL12XX_BUS_BLOCK_SIZE);
 184}
 185EXPORT_SYMBOL(wlcore_calc_packet_alignment);
 186
 187static int wl1271_tx_allocate(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 188			      struct sk_buff *skb, u32 extra, u32 buf_offset,
 189			      u8 hlid, bool is_gem)
 190{
 191	struct wl1271_tx_hw_descr *desc;
 192	u32 total_len = skb->len + sizeof(struct wl1271_tx_hw_descr) + extra;
 193	u32 total_blocks;
 194	int id, ret = -EBUSY, ac;
 195	u32 spare_blocks;
 196
 197	if (buf_offset + total_len > wl->aggr_buf_size)
 198		return -EAGAIN;
 199
 200	spare_blocks = wlcore_hw_get_spare_blocks(wl, is_gem);
 201
 202	/* allocate free identifier for the packet */
 203	id = wl1271_alloc_tx_id(wl, skb);
 204	if (id < 0)
 205		return id;
 206
 207	total_blocks = wlcore_hw_calc_tx_blocks(wl, total_len, spare_blocks);
 208
 209	if (total_blocks <= wl->tx_blocks_available) {
 210		if (skb_headroom(skb) < (total_len - skb->len) &&
 211		    pskb_expand_head(skb, (total_len - skb->len), 0, GFP_ATOMIC)) {
 212			wl1271_free_tx_id(wl, id);
 213			return -ENOMEM;
 214		}
 215		desc = skb_push(skb, total_len - skb->len);
 216
 217		wlcore_hw_set_tx_desc_blocks(wl, desc, total_blocks,
 218					     spare_blocks);
 219
 220		desc->id = id;
 221
 222		wl->tx_blocks_available -= total_blocks;
 223		wl->tx_allocated_blocks += total_blocks;
 224
 225		/*
 226		 * If the FW was empty before, arm the Tx watchdog. Also do
 227		 * this on the first Tx after resume, as we always cancel the
 228		 * watchdog on suspend.
 229		 */
 230		if (wl->tx_allocated_blocks == total_blocks ||
 231		    test_and_clear_bit(WL1271_FLAG_REINIT_TX_WDOG, &wl->flags))
 232			wl12xx_rearm_tx_watchdog_locked(wl);
 233
 234		ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 235		wl->tx_allocated_pkts[ac]++;
 236
 237		if (test_bit(hlid, wl->links_map))
 238			wl->links[hlid].allocated_pkts++;
 239
 240		ret = 0;
 241
 242		wl1271_debug(DEBUG_TX,
 243			     "tx_allocate: size: %d, blocks: %d, id: %d",
 244			     total_len, total_blocks, id);
 245	} else {
 246		wl1271_free_tx_id(wl, id);
 247	}
 248
 249	return ret;
 250}
 251
 252static void wl1271_tx_fill_hdr(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 253			       struct sk_buff *skb, u32 extra,
 254			       struct ieee80211_tx_info *control, u8 hlid)
 255{
 256	struct wl1271_tx_hw_descr *desc;
 257	int ac, rate_idx;
 258	s64 hosttime;
 259	u16 tx_attr = 0;
 260	__le16 frame_control;
 261	struct ieee80211_hdr *hdr;
 262	u8 *frame_start;
 263	bool is_dummy;
 264
 265	desc = (struct wl1271_tx_hw_descr *) skb->data;
 266	frame_start = (u8 *)(desc + 1);
 267	hdr = (struct ieee80211_hdr *)(frame_start + extra);
 268	frame_control = hdr->frame_control;
 269
 270	/* relocate space for security header */
 271	if (extra) {
 272		int hdrlen = ieee80211_hdrlen(frame_control);
 273		memmove(frame_start, hdr, hdrlen);
 274		skb_set_network_header(skb, skb_network_offset(skb) + extra);
 275	}
 276
 277	/* configure packet life time */
 278	hosttime = (ktime_get_boottime_ns() >> 10);
 279	desc->start_time = cpu_to_le32(hosttime - wl->time_offset);
 280
 281	is_dummy = wl12xx_is_dummy_packet(wl, skb);
 282	if (is_dummy || !wlvif || wlvif->bss_type != BSS_TYPE_AP_BSS)
 283		desc->life_time = cpu_to_le16(TX_HW_MGMT_PKT_LIFETIME_TU);
 284	else
 285		desc->life_time = cpu_to_le16(TX_HW_AP_MODE_PKT_LIFETIME_TU);
 286
 287	/* queue */
 288	ac = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 289	desc->tid = skb->priority;
 290
 291	if (is_dummy) {
 292		/*
 293		 * FW expects the dummy packet to have an invalid session id -
 294		 * any session id that is different than the one set in the join
 295		 */
 296		tx_attr = (SESSION_COUNTER_INVALID <<
 297			   TX_HW_ATTR_OFST_SESSION_COUNTER) &
 298			   TX_HW_ATTR_SESSION_COUNTER;
 299
 300		tx_attr |= TX_HW_ATTR_TX_DUMMY_REQ;
 301	} else if (wlvif) {
 302		u8 session_id = wl->session_ids[hlid];
 303
 304		if ((wl->quirks & WLCORE_QUIRK_AP_ZERO_SESSION_ID) &&
 305		    (wlvif->bss_type == BSS_TYPE_AP_BSS))
 306			session_id = 0;
 307
 308		/* configure the tx attributes */
 309		tx_attr = session_id << TX_HW_ATTR_OFST_SESSION_COUNTER;
 310	}
 311
 312	desc->hlid = hlid;
 313	if (is_dummy || !wlvif)
 314		rate_idx = 0;
 315	else if (wlvif->bss_type != BSS_TYPE_AP_BSS) {
 316		/*
 317		 * if the packets are data packets
 318		 * send them with AP rate policies (EAPOLs are an exception),
 319		 * otherwise use default basic rates
 320		 */
 321		if (skb->protocol == cpu_to_be16(ETH_P_PAE))
 322			rate_idx = wlvif->sta.basic_rate_idx;
 323		else if (control->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
 324			rate_idx = wlvif->sta.p2p_rate_idx;
 325		else if (ieee80211_is_data(frame_control))
 326			rate_idx = wlvif->sta.ap_rate_idx;
 327		else
 328			rate_idx = wlvif->sta.basic_rate_idx;
 329	} else {
 330		if (hlid == wlvif->ap.global_hlid)
 331			rate_idx = wlvif->ap.mgmt_rate_idx;
 332		else if (hlid == wlvif->ap.bcast_hlid ||
 333			 skb->protocol == cpu_to_be16(ETH_P_PAE) ||
 334			 !ieee80211_is_data(frame_control))
 335			/*
 336			 * send non-data, bcast and EAPOLs using the
 337			 * min basic rate
 338			 */
 339			rate_idx = wlvif->ap.bcast_rate_idx;
 340		else
 341			rate_idx = wlvif->ap.ucast_rate_idx[ac];
 342	}
 343
 344	tx_attr |= rate_idx << TX_HW_ATTR_OFST_RATE_POLICY;
 345
 346	/* for WEP shared auth - no fw encryption is needed */
 347	if (ieee80211_is_auth(frame_control) &&
 348	    ieee80211_has_protected(frame_control))
 349		tx_attr |= TX_HW_ATTR_HOST_ENCRYPT;
 350
 351	/* send EAPOL frames as voice */
 352	if (control->control.flags & IEEE80211_TX_CTRL_PORT_CTRL_PROTO)
 353		tx_attr |= TX_HW_ATTR_EAPOL_FRAME;
 354
 355	desc->tx_attr = cpu_to_le16(tx_attr);
 356
 357	wlcore_hw_set_tx_desc_csum(wl, desc, skb);
 358	wlcore_hw_set_tx_desc_data_len(wl, desc, skb);
 359}
 360
 361/* caller must hold wl->mutex */
 362static int wl1271_prepare_tx_frame(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 363				   struct sk_buff *skb, u32 buf_offset, u8 hlid)
 364{
 365	struct ieee80211_tx_info *info;
 366	u32 extra = 0;
 367	int ret = 0;
 368	u32 total_len;
 369	bool is_dummy;
 370	bool is_gem = false;
 371
 372	if (!skb) {
 373		wl1271_error("discarding null skb");
 374		return -EINVAL;
 375	}
 376
 377	if (hlid == WL12XX_INVALID_LINK_ID) {
 378		wl1271_error("invalid hlid. dropping skb 0x%p", skb);
 379		return -EINVAL;
 380	}
 381
 382	info = IEEE80211_SKB_CB(skb);
 383
 384	is_dummy = wl12xx_is_dummy_packet(wl, skb);
 385
 386	if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
 387	    info->control.hw_key &&
 388	    info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
 389		extra = WL1271_EXTRA_SPACE_TKIP;
 390
 391	if (info->control.hw_key) {
 392		bool is_wep;
 393		u8 idx = info->control.hw_key->hw_key_idx;
 394		u32 cipher = info->control.hw_key->cipher;
 395
 396		is_wep = (cipher == WLAN_CIPHER_SUITE_WEP40) ||
 397			 (cipher == WLAN_CIPHER_SUITE_WEP104);
 398
 399		if (WARN_ON(is_wep && wlvif && wlvif->default_key != idx)) {
 400			ret = wl1271_set_default_wep_key(wl, wlvif, idx);
 401			if (ret < 0)
 402				return ret;
 403			wlvif->default_key = idx;
 404		}
 405
 406		is_gem = (cipher == WL1271_CIPHER_SUITE_GEM);
 407	}
 408
 409	ret = wl1271_tx_allocate(wl, wlvif, skb, extra, buf_offset, hlid,
 410				 is_gem);
 411	if (ret < 0)
 412		return ret;
 413
 414	wl1271_tx_fill_hdr(wl, wlvif, skb, extra, info, hlid);
 415
 416	if (!is_dummy && wlvif && wlvif->bss_type == BSS_TYPE_AP_BSS) {
 417		wl1271_tx_ap_update_inconnection_sta(wl, wlvif, skb);
 418		wl1271_tx_regulate_link(wl, wlvif, hlid);
 419	}
 420
 421	/*
 422	 * The length of each packet is stored in terms of
 423	 * words. Thus, we must pad the skb data to make sure its
 424	 * length is aligned.  The number of padding bytes is computed
 425	 * and set in wl1271_tx_fill_hdr.
 426	 * In special cases, we want to align to a specific block size
 427	 * (eg. for wl128x with SDIO we align to 256).
 428	 */
 429	total_len = wlcore_calc_packet_alignment(wl, skb->len);
 430
 431	memcpy(wl->aggr_buf + buf_offset, skb->data, skb->len);
 432	memset(wl->aggr_buf + buf_offset + skb->len, 0, total_len - skb->len);
 433
 434	/* Revert side effects in the dummy packet skb, so it can be reused */
 435	if (is_dummy)
 436		skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
 437
 438	return total_len;
 439}
 440
 441u32 wl1271_tx_enabled_rates_get(struct wl1271 *wl, u32 rate_set,
 442				enum nl80211_band rate_band)
 443{
 444	struct ieee80211_supported_band *band;
 445	u32 enabled_rates = 0;
 446	int bit;
 447
 448	band = wl->hw->wiphy->bands[rate_band];
 449	for (bit = 0; bit < band->n_bitrates; bit++) {
 450		if (rate_set & 0x1)
 451			enabled_rates |= band->bitrates[bit].hw_value;
 452		rate_set >>= 1;
 453	}
 454
 455	/* MCS rates indication are on bits 16 - 31 */
 456	rate_set >>= HW_HT_RATES_OFFSET - band->n_bitrates;
 457
 458	for (bit = 0; bit < 16; bit++) {
 459		if (rate_set & 0x1)
 460			enabled_rates |= (CONF_HW_BIT_RATE_MCS_0 << bit);
 461		rate_set >>= 1;
 462	}
 463
 464	return enabled_rates;
 465}
 466
 467void wl1271_handle_tx_low_watermark(struct wl1271 *wl)
 468{
 469	int i;
 470	struct wl12xx_vif *wlvif;
 471
 472	wl12xx_for_each_wlvif(wl, wlvif) {
 473		for (i = 0; i < NUM_TX_QUEUES; i++) {
 474			if (wlcore_is_queue_stopped_by_reason(wl, wlvif, i,
 475					WLCORE_QUEUE_STOP_REASON_WATERMARK) &&
 476			    wlvif->tx_queue_count[i] <=
 477					WL1271_TX_QUEUE_LOW_WATERMARK)
 478				/* firmware buffer has space, restart queues */
 479				wlcore_wake_queue(wl, wlvif, i,
 480					WLCORE_QUEUE_STOP_REASON_WATERMARK);
 481		}
 482	}
 483}
 484
 485static int wlcore_select_ac(struct wl1271 *wl)
 486{
 487	int i, q = -1, ac;
 488	u32 min_pkts = 0xffffffff;
 489
 490	/*
 491	 * Find a non-empty ac where:
 492	 * 1. There are packets to transmit
 493	 * 2. The FW has the least allocated blocks
 494	 *
 495	 * We prioritize the ACs according to VO>VI>BE>BK
 496	 */
 497	for (i = 0; i < NUM_TX_QUEUES; i++) {
 498		ac = wl1271_tx_get_queue(i);
 499		if (wl->tx_queue_count[ac] &&
 500		    wl->tx_allocated_pkts[ac] < min_pkts) {
 501			q = ac;
 502			min_pkts = wl->tx_allocated_pkts[q];
 503		}
 504	}
 505
 506	return q;
 507}
 508
 509static struct sk_buff *wlcore_lnk_dequeue(struct wl1271 *wl,
 510					  struct wl1271_link *lnk, u8 q)
 511{
 512	struct sk_buff *skb;
 513	unsigned long flags;
 514
 515	skb = skb_dequeue(&lnk->tx_queue[q]);
 516	if (skb) {
 517		spin_lock_irqsave(&wl->wl_lock, flags);
 518		WARN_ON_ONCE(wl->tx_queue_count[q] <= 0);
 519		wl->tx_queue_count[q]--;
 520		if (lnk->wlvif) {
 521			WARN_ON_ONCE(lnk->wlvif->tx_queue_count[q] <= 0);
 522			lnk->wlvif->tx_queue_count[q]--;
 523		}
 524		spin_unlock_irqrestore(&wl->wl_lock, flags);
 525	}
 526
 527	return skb;
 528}
 529
 530static struct sk_buff *wlcore_lnk_dequeue_high_prio(struct wl1271 *wl,
 531						    u8 hlid, u8 ac,
 532						    u8 *low_prio_hlid)
 533{
 534	struct wl1271_link *lnk = &wl->links[hlid];
 535
 536	if (!wlcore_hw_lnk_high_prio(wl, hlid, lnk)) {
 537		if (*low_prio_hlid == WL12XX_INVALID_LINK_ID &&
 538		    !skb_queue_empty(&lnk->tx_queue[ac]) &&
 539		    wlcore_hw_lnk_low_prio(wl, hlid, lnk))
 540			/* we found the first non-empty low priority queue */
 541			*low_prio_hlid = hlid;
 542
 543		return NULL;
 544	}
 545
 546	return wlcore_lnk_dequeue(wl, lnk, ac);
 547}
 548
 549static struct sk_buff *wlcore_vif_dequeue_high_prio(struct wl1271 *wl,
 550						    struct wl12xx_vif *wlvif,
 551						    u8 ac, u8 *hlid,
 552						    u8 *low_prio_hlid)
 553{
 554	struct sk_buff *skb = NULL;
 555	int i, h, start_hlid;
 556
 557	/* start from the link after the last one */
 558	start_hlid = (wlvif->last_tx_hlid + 1) % wl->num_links;
 559
 560	/* dequeue according to AC, round robin on each link */
 561	for (i = 0; i < wl->num_links; i++) {
 562		h = (start_hlid + i) % wl->num_links;
 563
 564		/* only consider connected stations */
 565		if (!test_bit(h, wlvif->links_map))
 566			continue;
 567
 568		skb = wlcore_lnk_dequeue_high_prio(wl, h, ac,
 569						   low_prio_hlid);
 570		if (!skb)
 571			continue;
 572
 573		wlvif->last_tx_hlid = h;
 574		break;
 575	}
 576
 577	if (!skb)
 578		wlvif->last_tx_hlid = 0;
 579
 580	*hlid = wlvif->last_tx_hlid;
 581	return skb;
 582}
 583
 584static struct sk_buff *wl1271_skb_dequeue(struct wl1271 *wl, u8 *hlid)
 585{
 586	unsigned long flags;
 587	struct wl12xx_vif *wlvif = wl->last_wlvif;
 588	struct sk_buff *skb = NULL;
 589	int ac;
 590	u8 low_prio_hlid = WL12XX_INVALID_LINK_ID;
 591
 592	ac = wlcore_select_ac(wl);
 593	if (ac < 0)
 594		goto out;
 595
 596	/* continue from last wlvif (round robin) */
 597	if (wlvif) {
 598		wl12xx_for_each_wlvif_continue(wl, wlvif) {
 599			if (!wlvif->tx_queue_count[ac])
 600				continue;
 601
 602			skb = wlcore_vif_dequeue_high_prio(wl, wlvif, ac, hlid,
 603							   &low_prio_hlid);
 604			if (!skb)
 605				continue;
 606
 607			wl->last_wlvif = wlvif;
 608			break;
 609		}
 610	}
 611
 612	/* dequeue from the system HLID before the restarting wlvif list */
 613	if (!skb) {
 614		skb = wlcore_lnk_dequeue_high_prio(wl, wl->system_hlid,
 615						   ac, &low_prio_hlid);
 616		if (skb) {
 617			*hlid = wl->system_hlid;
 618			wl->last_wlvif = NULL;
 619		}
 620	}
 621
 622	/* Do a new pass over the wlvif list. But no need to continue
 623	 * after last_wlvif. The previous pass should have found it. */
 624	if (!skb) {
 625		wl12xx_for_each_wlvif(wl, wlvif) {
 626			if (!wlvif->tx_queue_count[ac])
 627				goto next;
 628
 629			skb = wlcore_vif_dequeue_high_prio(wl, wlvif, ac, hlid,
 630							   &low_prio_hlid);
 631			if (skb) {
 632				wl->last_wlvif = wlvif;
 633				break;
 634			}
 635
 636next:
 637			if (wlvif == wl->last_wlvif)
 638				break;
 639		}
 640	}
 641
 642	/* no high priority skbs found - but maybe a low priority one? */
 643	if (!skb && low_prio_hlid != WL12XX_INVALID_LINK_ID) {
 644		struct wl1271_link *lnk = &wl->links[low_prio_hlid];
 645		skb = wlcore_lnk_dequeue(wl, lnk, ac);
 646
 647		WARN_ON(!skb); /* we checked this before */
 648		*hlid = low_prio_hlid;
 649
 650		/* ensure proper round robin in the vif/link levels */
 651		wl->last_wlvif = lnk->wlvif;
 652		if (lnk->wlvif)
 653			lnk->wlvif->last_tx_hlid = low_prio_hlid;
 654
 655	}
 656
 657out:
 658	if (!skb &&
 659	    test_and_clear_bit(WL1271_FLAG_DUMMY_PACKET_PENDING, &wl->flags)) {
 660		int q;
 661
 662		skb = wl->dummy_packet;
 663		*hlid = wl->system_hlid;
 664		q = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 665		spin_lock_irqsave(&wl->wl_lock, flags);
 666		WARN_ON_ONCE(wl->tx_queue_count[q] <= 0);
 667		wl->tx_queue_count[q]--;
 668		spin_unlock_irqrestore(&wl->wl_lock, flags);
 669	}
 670
 671	return skb;
 672}
 673
 674static void wl1271_skb_queue_head(struct wl1271 *wl, struct wl12xx_vif *wlvif,
 675				  struct sk_buff *skb, u8 hlid)
 676{
 677	unsigned long flags;
 678	int q = wl1271_tx_get_queue(skb_get_queue_mapping(skb));
 679
 680	if (wl12xx_is_dummy_packet(wl, skb)) {
 681		set_bit(WL1271_FLAG_DUMMY_PACKET_PENDING, &wl->flags);
 682	} else {
 683		skb_queue_head(&wl->links[hlid].tx_queue[q], skb);
 684
 685		/* make sure we dequeue the same packet next time */
 686		wlvif->last_tx_hlid = (hlid + wl->num_links - 1) %
 687				      wl->num_links;
 688	}
 689
 690	spin_lock_irqsave(&wl->wl_lock, flags);
 691	wl->tx_queue_count[q]++;
 692	if (wlvif)
 693		wlvif->tx_queue_count[q]++;
 694	spin_unlock_irqrestore(&wl->wl_lock, flags);
 695}
 696
 697static bool wl1271_tx_is_data_present(struct sk_buff *skb)
 698{
 699	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 700
 701	return ieee80211_is_data_present(hdr->frame_control);
 702}
 703
 704void wl12xx_rearm_rx_streaming(struct wl1271 *wl, unsigned long *active_hlids)
 705{
 706	struct wl12xx_vif *wlvif;
 707	u32 timeout;
 708	u8 hlid;
 709
 710	if (!wl->conf.rx_streaming.interval)
 711		return;
 712
 713	if (!wl->conf.rx_streaming.always &&
 714	    !test_bit(WL1271_FLAG_SOFT_GEMINI, &wl->flags))
 715		return;
 716
 717	timeout = wl->conf.rx_streaming.duration;
 718	wl12xx_for_each_wlvif_sta(wl, wlvif) {
 719		bool found = false;
 720		for_each_set_bit(hlid, active_hlids, wl->num_links) {
 721			if (test_bit(hlid, wlvif->links_map)) {
 722				found  = true;
 723				break;
 724			}
 725		}
 726
 727		if (!found)
 728			continue;
 729
 730		/* enable rx streaming */
 731		if (!test_bit(WLVIF_FLAG_RX_STREAMING_STARTED, &wlvif->flags))
 732			ieee80211_queue_work(wl->hw,
 733					     &wlvif->rx_streaming_enable_work);
 734
 735		mod_timer(&wlvif->rx_streaming_timer,
 736			  jiffies + msecs_to_jiffies(timeout));
 737	}
 738}
 739
 740/*
 741 * Returns failure values only in case of failed bus ops within this function.
 742 * wl1271_prepare_tx_frame retvals won't be returned in order to avoid
 743 * triggering recovery by higher layers when not necessary.
 744 * In case a FW command fails within wl1271_prepare_tx_frame fails a recovery
 745 * will be queued in wl1271_cmd_send. -EAGAIN/-EBUSY from prepare_tx_frame
 746 * can occur and are legitimate so don't propagate. -EINVAL will emit a WARNING
 747 * within prepare_tx_frame code but there's nothing we should do about those
 748 * as well.
 749 */
 750int wlcore_tx_work_locked(struct wl1271 *wl)
 751{
 752	struct wl12xx_vif *wlvif;
 753	struct sk_buff *skb;
 754	struct wl1271_tx_hw_descr *desc;
 755	u32 buf_offset = 0, last_len = 0;
 756	bool sent_packets = false;
 757	unsigned long active_hlids[BITS_TO_LONGS(WLCORE_MAX_LINKS)] = {0};
 758	int ret = 0;
 759	int bus_ret = 0;
 760	u8 hlid;
 761
 762	if (unlikely(wl->state != WLCORE_STATE_ON))
 763		return 0;
 764
 765	while ((skb = wl1271_skb_dequeue(wl, &hlid))) {
 766		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 767		bool has_data = false;
 768
 769		wlvif = NULL;
 770		if (!wl12xx_is_dummy_packet(wl, skb))
 771			wlvif = wl12xx_vif_to_data(info->control.vif);
 772		else
 773			hlid = wl->system_hlid;
 774
 775		has_data = wlvif && wl1271_tx_is_data_present(skb);
 776		ret = wl1271_prepare_tx_frame(wl, wlvif, skb, buf_offset,
 777					      hlid);
 778		if (ret == -EAGAIN) {
 779			/*
 780			 * Aggregation buffer is full.
 781			 * Flush buffer and try again.
 782			 */
 783			wl1271_skb_queue_head(wl, wlvif, skb, hlid);
 784
 785			buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset,
 786							    last_len);
 787			bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA,
 788					     wl->aggr_buf, buf_offset, true);
 789			if (bus_ret < 0)
 790				goto out;
 791
 792			sent_packets = true;
 793			buf_offset = 0;
 794			continue;
 795		} else if (ret == -EBUSY) {
 796			/*
 797			 * Firmware buffer is full.
 798			 * Queue back last skb, and stop aggregating.
 799			 */
 800			wl1271_skb_queue_head(wl, wlvif, skb, hlid);
 801			/* No work left, avoid scheduling redundant tx work */
 802			set_bit(WL1271_FLAG_FW_TX_BUSY, &wl->flags);
 803			goto out_ack;
 804		} else if (ret < 0) {
 805			if (wl12xx_is_dummy_packet(wl, skb))
 806				/*
 807				 * fw still expects dummy packet,
 808				 * so re-enqueue it
 809				 */
 810				wl1271_skb_queue_head(wl, wlvif, skb, hlid);
 811			else
 812				ieee80211_free_txskb(wl->hw, skb);
 813			goto out_ack;
 814		}
 815		last_len = ret;
 816		buf_offset += last_len;
 817		wl->tx_packets_count++;
 818		if (has_data) {
 819			desc = (struct wl1271_tx_hw_descr *) skb->data;
 820			__set_bit(desc->hlid, active_hlids);
 821		}
 822	}
 823
 824out_ack:
 825	if (buf_offset) {
 826		buf_offset = wlcore_hw_pre_pkt_send(wl, buf_offset, last_len);
 827		bus_ret = wlcore_write_data(wl, REG_SLV_MEM_DATA, wl->aggr_buf,
 828					     buf_offset, true);
 829		if (bus_ret < 0)
 830			goto out;
 831
 832		sent_packets = true;
 833	}
 834	if (sent_packets) {
 835		/*
 836		 * Interrupt the firmware with the new packets. This is only
 837		 * required for older hardware revisions
 838		 */
 839		if (wl->quirks & WLCORE_QUIRK_END_OF_TRANSACTION) {
 840			bus_ret = wlcore_write32(wl, WL12XX_HOST_WR_ACCESS,
 841					     wl->tx_packets_count);
 842			if (bus_ret < 0)
 843				goto out;
 844		}
 845
 846		wl1271_handle_tx_low_watermark(wl);
 847	}
 848	wl12xx_rearm_rx_streaming(wl, active_hlids);
 849
 850out:
 851	return bus_ret;
 852}
 853
 854void wl1271_tx_work(struct work_struct *work)
 855{
 856	struct wl1271 *wl = container_of(work, struct wl1271, tx_work);
 857	int ret;
 858
 859	mutex_lock(&wl->mutex);
 860	ret = pm_runtime_resume_and_get(wl->dev);
 861	if (ret < 0)
 862		goto out;
 863
 864	ret = wlcore_tx_work_locked(wl);
 865	if (ret < 0) {
 866		pm_runtime_put_noidle(wl->dev);
 867		wl12xx_queue_recovery_work(wl);
 868		goto out;
 869	}
 870
 871	pm_runtime_put_autosuspend(wl->dev);
 872out:
 873	mutex_unlock(&wl->mutex);
 874}
 875
 876static u8 wl1271_tx_get_rate_flags(u8 rate_class_index)
 877{
 878	u8 flags = 0;
 879
 880	/*
 881	 * TODO: use wl12xx constants when this code is moved to wl12xx, as
 882	 * only it uses Tx-completion.
 883	 */
 884	if (rate_class_index <= 8)
 885		flags |= IEEE80211_TX_RC_MCS;
 886
 887	/*
 888	 * TODO: use wl12xx constants when this code is moved to wl12xx, as
 889	 * only it uses Tx-completion.
 890	 */
 891	if (rate_class_index == 0)
 892		flags |= IEEE80211_TX_RC_SHORT_GI;
 893
 894	return flags;
 895}
 896
 897static void wl1271_tx_complete_packet(struct wl1271 *wl,
 898				      struct wl1271_tx_hw_res_descr *result)
 899{
 900	struct ieee80211_tx_info *info;
 901	struct ieee80211_vif *vif;
 902	struct wl12xx_vif *wlvif;
 903	struct sk_buff *skb;
 904	int id = result->id;
 905	int rate = -1;
 906	u8 rate_flags = 0;
 907	u8 retries = 0;
 908
 909	/* check for id legality */
 910	if (unlikely(id >= wl->num_tx_desc || wl->tx_frames[id] == NULL)) {
 911		wl1271_warning("TX result illegal id: %d", id);
 912		return;
 913	}
 914
 915	skb = wl->tx_frames[id];
 916	info = IEEE80211_SKB_CB(skb);
 917
 918	if (wl12xx_is_dummy_packet(wl, skb)) {
 919		wl1271_free_tx_id(wl, id);
 920		return;
 921	}
 922
 923	/* info->control is valid as long as we don't update info->status */
 924	vif = info->control.vif;
 925	wlvif = wl12xx_vif_to_data(vif);
 926
 927	/* update the TX status info */
 928	if (result->status == TX_SUCCESS) {
 929		if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 930			info->flags |= IEEE80211_TX_STAT_ACK;
 931		rate = wlcore_rate_to_idx(wl, result->rate_class_index,
 932					  wlvif->band);
 933		rate_flags = wl1271_tx_get_rate_flags(result->rate_class_index);
 934		retries = result->ack_failures;
 935	} else if (result->status == TX_RETRY_EXCEEDED) {
 936		wl->stats.excessive_retries++;
 937		retries = result->ack_failures;
 938	}
 939
 940	info->status.rates[0].idx = rate;
 941	info->status.rates[0].count = retries;
 942	info->status.rates[0].flags = rate_flags;
 943	info->status.ack_signal = -1;
 944
 945	wl->stats.retry_count += result->ack_failures;
 946
 947	/* remove private header from packet */
 948	skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
 949
 950	/* remove TKIP header space if present */
 951	if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
 952	    info->control.hw_key &&
 953	    info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP) {
 954		int hdrlen = ieee80211_get_hdrlen_from_skb(skb);
 955		memmove(skb->data + WL1271_EXTRA_SPACE_TKIP, skb->data,
 956			hdrlen);
 957		skb_pull(skb, WL1271_EXTRA_SPACE_TKIP);
 958	}
 959
 960	wl1271_debug(DEBUG_TX, "tx status id %u skb 0x%p failures %u rate 0x%x"
 961		     " status 0x%x",
 962		     result->id, skb, result->ack_failures,
 963		     result->rate_class_index, result->status);
 964
 965	/* return the packet to the stack */
 966	skb_queue_tail(&wl->deferred_tx_queue, skb);
 967	queue_work(wl->freezable_wq, &wl->netstack_work);
 968	wl1271_free_tx_id(wl, result->id);
 969}
 970
 971/* Called upon reception of a TX complete interrupt */
 972int wlcore_tx_complete(struct wl1271 *wl)
 973{
 974	struct wl1271_acx_mem_map *memmap = wl->target_mem_map;
 975	u32 count, fw_counter;
 976	u32 i;
 977	int ret;
 978
 979	/* read the tx results from the chipset */
 980	ret = wlcore_read(wl, le32_to_cpu(memmap->tx_result),
 981			  wl->tx_res_if, sizeof(*wl->tx_res_if), false);
 982	if (ret < 0)
 983		goto out;
 984
 985	fw_counter = le32_to_cpu(wl->tx_res_if->tx_result_fw_counter);
 986
 987	/* write host counter to chipset (to ack) */
 988	ret = wlcore_write32(wl, le32_to_cpu(memmap->tx_result) +
 989			     offsetof(struct wl1271_tx_hw_res_if,
 990				      tx_result_host_counter), fw_counter);
 991	if (ret < 0)
 992		goto out;
 993
 994	count = fw_counter - wl->tx_results_count;
 995	wl1271_debug(DEBUG_TX, "tx_complete received, packets: %d", count);
 996
 997	/* verify that the result buffer is not getting overrun */
 998	if (unlikely(count > TX_HW_RESULT_QUEUE_LEN))
 999		wl1271_warning("TX result overflow from chipset: %d", count);
1000
1001	/* process the results */
1002	for (i = 0; i < count; i++) {
1003		struct wl1271_tx_hw_res_descr *result;
1004		u8 offset = wl->tx_results_count & TX_HW_RESULT_QUEUE_LEN_MASK;
1005
1006		/* process the packet */
1007		result =  &(wl->tx_res_if->tx_results_queue[offset]);
1008		wl1271_tx_complete_packet(wl, result);
1009
1010		wl->tx_results_count++;
1011	}
1012
1013out:
1014	return ret;
1015}
1016EXPORT_SYMBOL(wlcore_tx_complete);
1017
1018void wl1271_tx_reset_link_queues(struct wl1271 *wl, u8 hlid)
1019{
1020	struct sk_buff *skb;
1021	int i;
1022	unsigned long flags;
1023	struct ieee80211_tx_info *info;
1024	int total[NUM_TX_QUEUES];
1025	struct wl1271_link *lnk = &wl->links[hlid];
1026
1027	for (i = 0; i < NUM_TX_QUEUES; i++) {
1028		total[i] = 0;
1029		while ((skb = skb_dequeue(&lnk->tx_queue[i]))) {
1030			wl1271_debug(DEBUG_TX, "link freeing skb 0x%p", skb);
1031
1032			if (!wl12xx_is_dummy_packet(wl, skb)) {
1033				info = IEEE80211_SKB_CB(skb);
1034				info->status.rates[0].idx = -1;
1035				info->status.rates[0].count = 0;
1036				ieee80211_tx_status_ni(wl->hw, skb);
1037			}
1038
1039			total[i]++;
1040		}
1041	}
1042
1043	spin_lock_irqsave(&wl->wl_lock, flags);
1044	for (i = 0; i < NUM_TX_QUEUES; i++) {
1045		wl->tx_queue_count[i] -= total[i];
1046		if (lnk->wlvif)
1047			lnk->wlvif->tx_queue_count[i] -= total[i];
1048	}
1049	spin_unlock_irqrestore(&wl->wl_lock, flags);
1050
1051	wl1271_handle_tx_low_watermark(wl);
1052}
1053
1054/* caller must hold wl->mutex and TX must be stopped */
1055void wl12xx_tx_reset_wlvif(struct wl1271 *wl, struct wl12xx_vif *wlvif)
1056{
1057	int i;
1058
1059	/* TX failure */
1060	for_each_set_bit(i, wlvif->links_map, wl->num_links) {
1061		if (wlvif->bss_type == BSS_TYPE_AP_BSS &&
1062		    i != wlvif->ap.bcast_hlid && i != wlvif->ap.global_hlid) {
1063			/* this calls wl12xx_free_link */
1064			wl1271_free_sta(wl, wlvif, i);
1065		} else {
1066			u8 hlid = i;
1067			wl12xx_free_link(wl, wlvif, &hlid);
1068		}
1069	}
1070	wlvif->last_tx_hlid = 0;
1071
1072	for (i = 0; i < NUM_TX_QUEUES; i++)
1073		wlvif->tx_queue_count[i] = 0;
1074}
1075/* caller must hold wl->mutex and TX must be stopped */
1076void wl12xx_tx_reset(struct wl1271 *wl)
1077{
1078	int i;
1079	struct sk_buff *skb;
1080	struct ieee80211_tx_info *info;
1081
1082	/* only reset the queues if something bad happened */
1083	if (wl1271_tx_total_queue_count(wl) != 0) {
1084		for (i = 0; i < wl->num_links; i++)
1085			wl1271_tx_reset_link_queues(wl, i);
1086
1087		for (i = 0; i < NUM_TX_QUEUES; i++)
1088			wl->tx_queue_count[i] = 0;
1089	}
1090
1091	/*
1092	 * Make sure the driver is at a consistent state, in case this
1093	 * function is called from a context other than interface removal.
1094	 * This call will always wake the TX queues.
1095	 */
1096	wl1271_handle_tx_low_watermark(wl);
1097
1098	for (i = 0; i < wl->num_tx_desc; i++) {
1099		if (wl->tx_frames[i] == NULL)
1100			continue;
1101
1102		skb = wl->tx_frames[i];
1103		wl1271_free_tx_id(wl, i);
1104		wl1271_debug(DEBUG_TX, "freeing skb 0x%p", skb);
1105
1106		if (!wl12xx_is_dummy_packet(wl, skb)) {
1107			/*
1108			 * Remove private headers before passing the skb to
1109			 * mac80211
1110			 */
1111			info = IEEE80211_SKB_CB(skb);
1112			skb_pull(skb, sizeof(struct wl1271_tx_hw_descr));
1113			if ((wl->quirks & WLCORE_QUIRK_TKIP_HEADER_SPACE) &&
1114			    info->control.hw_key &&
1115			    info->control.hw_key->cipher ==
1116			    WLAN_CIPHER_SUITE_TKIP) {
1117				int hdrlen = ieee80211_get_hdrlen_from_skb(skb);
1118				memmove(skb->data + WL1271_EXTRA_SPACE_TKIP,
1119					skb->data, hdrlen);
1120				skb_pull(skb, WL1271_EXTRA_SPACE_TKIP);
1121			}
1122
1123			info->status.rates[0].idx = -1;
1124			info->status.rates[0].count = 0;
1125
1126			ieee80211_tx_status_ni(wl->hw, skb);
1127		}
1128	}
1129}
1130
1131#define WL1271_TX_FLUSH_TIMEOUT 500000
1132
1133/* caller must *NOT* hold wl->mutex */
1134void wl1271_tx_flush(struct wl1271 *wl)
1135{
1136	unsigned long timeout, start_time;
1137	int i;
1138	start_time = jiffies;
1139	timeout = start_time + usecs_to_jiffies(WL1271_TX_FLUSH_TIMEOUT);
1140
1141	/* only one flush should be in progress, for consistent queue state */
1142	mutex_lock(&wl->flush_mutex);
1143
1144	mutex_lock(&wl->mutex);
1145	if (wl->tx_frames_cnt == 0 && wl1271_tx_total_queue_count(wl) == 0) {
1146		mutex_unlock(&wl->mutex);
1147		goto out;
1148	}
1149
1150	wlcore_stop_queues(wl, WLCORE_QUEUE_STOP_REASON_FLUSH);
1151
1152	while (!time_after(jiffies, timeout)) {
1153		wl1271_debug(DEBUG_MAC80211, "flushing tx buffer: %d %d",
1154			     wl->tx_frames_cnt,
1155			     wl1271_tx_total_queue_count(wl));
1156
1157		/* force Tx and give the driver some time to flush data */
1158		mutex_unlock(&wl->mutex);
1159		if (wl1271_tx_total_queue_count(wl))
1160			wl1271_tx_work(&wl->tx_work);
1161		msleep(20);
1162		mutex_lock(&wl->mutex);
1163
1164		if ((wl->tx_frames_cnt == 0) &&
1165		    (wl1271_tx_total_queue_count(wl) == 0)) {
1166			wl1271_debug(DEBUG_MAC80211, "tx flush took %d ms",
1167				     jiffies_to_msecs(jiffies - start_time));
1168			goto out_wake;
1169		}
1170	}
1171
1172	wl1271_warning("Unable to flush all TX buffers, "
1173		       "timed out (timeout %d ms",
1174		       WL1271_TX_FLUSH_TIMEOUT / 1000);
1175
1176	/* forcibly flush all Tx buffers on our queues */
1177	for (i = 0; i < wl->num_links; i++)
1178		wl1271_tx_reset_link_queues(wl, i);
1179
1180out_wake:
1181	wlcore_wake_queues(wl, WLCORE_QUEUE_STOP_REASON_FLUSH);
1182	mutex_unlock(&wl->mutex);
1183out:
1184	mutex_unlock(&wl->flush_mutex);
1185}
1186EXPORT_SYMBOL_GPL(wl1271_tx_flush);
1187
1188u32 wl1271_tx_min_rate_get(struct wl1271 *wl, u32 rate_set)
1189{
1190	if (WARN_ON(!rate_set))
1191		return 0;
1192
1193	return BIT(__ffs(rate_set));
1194}
1195EXPORT_SYMBOL_GPL(wl1271_tx_min_rate_get);
1196
1197void wlcore_stop_queue_locked(struct wl1271 *wl, struct wl12xx_vif *wlvif,
1198			      u8 queue, enum wlcore_queue_stop_reason reason)
1199{
1200	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
1201	bool stopped = !!wl->queue_stop_reasons[hwq];
1202
1203	/* queue should not be stopped for this reason */
1204	WARN_ON_ONCE(test_and_set_bit(reason, &wl->queue_stop_reasons[hwq]));
1205
1206	if (stopped)
1207		return;
1208
1209	ieee80211_stop_queue(wl->hw, hwq);
1210}
1211
1212void wlcore_stop_queue(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 queue,
1213		       enum wlcore_queue_stop_reason reason)
1214{
1215	unsigned long flags;
1216
1217	spin_lock_irqsave(&wl->wl_lock, flags);
1218	wlcore_stop_queue_locked(wl, wlvif, queue, reason);
1219	spin_unlock_irqrestore(&wl->wl_lock, flags);
1220}
1221
1222void wlcore_wake_queue(struct wl1271 *wl, struct wl12xx_vif *wlvif, u8 queue,
1223		       enum wlcore_queue_stop_reason reason)
1224{
1225	unsigned long flags;
1226	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
1227
1228	spin_lock_irqsave(&wl->wl_lock, flags);
1229
1230	/* queue should not be clear for this reason */
1231	WARN_ON_ONCE(!test_and_clear_bit(reason, &wl->queue_stop_reasons[hwq]));
1232
1233	if (wl->queue_stop_reasons[hwq])
1234		goto out;
1235
1236	ieee80211_wake_queue(wl->hw, hwq);
1237
1238out:
1239	spin_unlock_irqrestore(&wl->wl_lock, flags);
1240}
1241
1242void wlcore_stop_queues(struct wl1271 *wl,
1243			enum wlcore_queue_stop_reason reason)
1244{
1245	int i;
1246	unsigned long flags;
1247
1248	spin_lock_irqsave(&wl->wl_lock, flags);
1249
1250	/* mark all possible queues as stopped */
1251        for (i = 0; i < WLCORE_NUM_MAC_ADDRESSES * NUM_TX_QUEUES; i++)
1252                WARN_ON_ONCE(test_and_set_bit(reason,
1253					      &wl->queue_stop_reasons[i]));
1254
1255	/* use the global version to make sure all vifs in mac80211 we don't
1256	 * know are stopped.
1257	 */
1258	ieee80211_stop_queues(wl->hw);
1259
1260	spin_unlock_irqrestore(&wl->wl_lock, flags);
1261}
1262
1263void wlcore_wake_queues(struct wl1271 *wl,
1264			enum wlcore_queue_stop_reason reason)
1265{
1266	int i;
1267	unsigned long flags;
1268
1269	spin_lock_irqsave(&wl->wl_lock, flags);
1270
1271	/* mark all possible queues as awake */
1272        for (i = 0; i < WLCORE_NUM_MAC_ADDRESSES * NUM_TX_QUEUES; i++)
1273		WARN_ON_ONCE(!test_and_clear_bit(reason,
1274						 &wl->queue_stop_reasons[i]));
1275
1276	/* use the global version to make sure all vifs in mac80211 we don't
1277	 * know are woken up.
1278	 */
1279	ieee80211_wake_queues(wl->hw);
1280
1281	spin_unlock_irqrestore(&wl->wl_lock, flags);
1282}
1283
1284bool wlcore_is_queue_stopped_by_reason(struct wl1271 *wl,
1285				       struct wl12xx_vif *wlvif, u8 queue,
1286				       enum wlcore_queue_stop_reason reason)
1287{
1288	unsigned long flags;
1289	bool stopped;
1290
1291	spin_lock_irqsave(&wl->wl_lock, flags);
1292	stopped = wlcore_is_queue_stopped_by_reason_locked(wl, wlvif, queue,
1293							   reason);
1294	spin_unlock_irqrestore(&wl->wl_lock, flags);
1295
1296	return stopped;
1297}
1298
1299bool wlcore_is_queue_stopped_by_reason_locked(struct wl1271 *wl,
1300				       struct wl12xx_vif *wlvif, u8 queue,
1301				       enum wlcore_queue_stop_reason reason)
1302{
1303	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
1304
1305	assert_spin_locked(&wl->wl_lock);
1306	return test_bit(reason, &wl->queue_stop_reasons[hwq]);
1307}
1308
1309bool wlcore_is_queue_stopped_locked(struct wl1271 *wl, struct wl12xx_vif *wlvif,
1310				    u8 queue)
1311{
1312	int hwq = wlcore_tx_get_mac80211_queue(wlvif, queue);
1313
1314	assert_spin_locked(&wl->wl_lock);
1315	return !!wl->queue_stop_reasons[hwq];
1316}