#include "cfg.h"
#include "util.h"
#include "fw.h"
#include "main.h"
#include "wmm.h"
#include "11n.h"
#define DRV_PKT_DELAY_TO_FW_MAX 512
#define WMM_QUEUED_PACKET_LOWER_LIMIT 180
#define WMM_QUEUED_PACKET_UPPER_LIMIT 200
#define IPTOS_OFFSET 5
static bool disable_tx_amsdu;
static const u8 tos_to_tid_inv[] = {
0x02,
0x00,
0x01,
0x03,
0x04,
0x05,
0x06,
0x07
};
static const u8 wmm_info_ie[] = { WLAN_EID_VENDOR_SPECIFIC, 0x07,
0x00, 0x50, 0xf2, 0x02,
0x00, 0x01, 0x00
};
static const u8 wmm_aci_to_qidx_map[] = { WMM_AC_BE,
WMM_AC_BK,
WMM_AC_VI,
WMM_AC_VO
};
static u8 tos_to_tid[] = {
0x01,
0x02,
0x00,
0x03,
0x04,
0x05,
0x06,
0x07
};
static u8 ac_to_tid[4][2] = { {1, 2}, {0, 3}, {4, 5}, {6, 7} };
static void
nxpwifi_wmm_ac_debug_print(const struct ieee80211_wmm_ac_param *ac_param)
{
static const char * const ac_str[] = { "BK", "BE", "VI", "VO" };
pr_debug("info: WMM AC_%s: ACI=%d, ACM=%d, Aifsn=%d, ",
ac_str[wmm_aci_to_qidx_map[(ac_param->aci_aifsn
& NXPWIFI_ACI) >> 5]],
(ac_param->aci_aifsn & NXPWIFI_ACI) >> 5,
(ac_param->aci_aifsn & NXPWIFI_ACM) >> 4,
ac_param->aci_aifsn & NXPWIFI_AIFSN);
pr_debug("EcwMin=%d, EcwMax=%d, TxopLimit=%d\n",
ac_param->cw & NXPWIFI_ECW_MIN,
(ac_param->cw & NXPWIFI_ECW_MAX) >> 4,
le16_to_cpu(ac_param->txop_limit));
}
static struct nxpwifi_ra_list_tbl *
nxpwifi_wmm_allocate_ralist_node(struct nxpwifi_adapter *adapter, const u8 *ra)
{
struct nxpwifi_ra_list_tbl *ra_list;
ra_list = kzalloc_obj(*ra_list, GFP_ATOMIC);
if (!ra_list)
return NULL;
INIT_LIST_HEAD(&ra_list->list);
skb_queue_head_init(&ra_list->skb_head);
memcpy(ra_list->ra, ra, ETH_ALEN);
ra_list->total_pkt_count = 0;
nxpwifi_dbg(adapter, INFO, "info: allocated ra_list %p\n", ra_list);
return ra_list;
}
static u8 nxpwifi_get_random_ba_threshold(void)
{
u64 ns;
ns = ktime_get_ns();
ns += (ns >> 32) + (ns >> 16);
return ((u8)ns % BA_SETUP_MAX_PACKET_THRESHOLD) + BA_SETUP_PACKET_OFFSET;
}
void nxpwifi_ralist_add(struct nxpwifi_private *priv, const u8 *ra)
{
int i;
struct nxpwifi_ra_list_tbl *ra_list;
struct nxpwifi_adapter *adapter = priv->adapter;
struct nxpwifi_sta_node *node;
for (i = 0; i < MAX_NUM_TID; ++i) {
ra_list = nxpwifi_wmm_allocate_ralist_node(adapter, ra);
nxpwifi_dbg(adapter, INFO,
"info: created ra_list %p\n", ra_list);
if (!ra_list)
break;
ra_list->is_11n_enabled = 0;
ra_list->ba_status = BA_SETUP_NONE;
ra_list->amsdu_in_ampdu = false;
if (!nxpwifi_queuing_ra_based(priv)) {
ra_list->is_11n_enabled = IS_11N_ENABLED(priv);
} else {
rcu_read_lock();
node = nxpwifi_get_sta_entry(priv, ra);
if (node)
ra_list->tx_paused = node->tx_pause;
ra_list->is_11n_enabled =
nxpwifi_is_sta_11n_enabled(priv, node);
if (ra_list->is_11n_enabled)
ra_list->max_amsdu = node->max_amsdu;
rcu_read_unlock();
}
nxpwifi_dbg(adapter, DATA, "data: ralist %p: is_11n_enabled=%d\n",
ra_list, ra_list->is_11n_enabled);
if (ra_list->is_11n_enabled) {
ra_list->ba_pkt_count = 0;
ra_list->ba_packet_thr =
nxpwifi_get_random_ba_threshold();
}
list_add_tail(&ra_list->list,
&priv->wmm.tid_tbl_ptr[i].ra_list);
}
}
static void nxpwifi_wmm_default_queue_priorities(struct nxpwifi_private *priv)
{
priv->wmm.queue_priority[0] = WMM_AC_VO;
priv->wmm.queue_priority[1] = WMM_AC_VI;
priv->wmm.queue_priority[2] = WMM_AC_BE;
priv->wmm.queue_priority[3] = WMM_AC_BK;
}
static void
nxpwifi_wmm_queue_priorities_tid(struct nxpwifi_private *priv)
{
struct nxpwifi_wmm_desc *wmm = &priv->wmm;
u8 *queue_priority = wmm->queue_priority;
int i;
for (i = 0; i < 4; ++i) {
tos_to_tid[7 - (i * 2)] = ac_to_tid[queue_priority[i]][1];
tos_to_tid[6 - (i * 2)] = ac_to_tid[queue_priority[i]][0];
}
for (i = 0; i < MAX_NUM_TID; ++i)
priv->tos_to_tid_inv[tos_to_tid[i]] = (u8)i;
atomic_set(&wmm->highest_queued_prio, HIGH_PRIO_TID);
}
void
nxpwifi_wmm_setup_queue_priorities(struct nxpwifi_private *priv,
struct ieee80211_wmm_param_ie *wmm_ie)
{
u16 cw_min, avg_back_off, tmp[4];
u32 i, j, num_ac;
u8 ac_idx;
if (!wmm_ie || !priv->wmm_enabled) {
nxpwifi_wmm_default_queue_priorities(priv);
return;
}
nxpwifi_dbg(priv->adapter, INFO,
"info: WMM Parameter element: version=%d,\t"
"qos_info Parameter Set Count=%d, Reserved=%#x\n",
wmm_ie->version, wmm_ie->qos_info &
IEEE80211_WMM_IE_AP_QOSINFO_PARAM_SET_CNT_MASK,
wmm_ie->reserved);
for (num_ac = 0; num_ac < ARRAY_SIZE(wmm_ie->ac); num_ac++) {
u8 ecw = wmm_ie->ac[num_ac].cw;
u8 aci_aifsn = wmm_ie->ac[num_ac].aci_aifsn;
cw_min = (1 << (ecw & NXPWIFI_ECW_MIN)) - 1;
avg_back_off = (cw_min >> 1) + (aci_aifsn & NXPWIFI_AIFSN);
ac_idx = wmm_aci_to_qidx_map[(aci_aifsn & NXPWIFI_ACI) >> 5];
priv->wmm.queue_priority[ac_idx] = ac_idx;
tmp[ac_idx] = avg_back_off;
nxpwifi_dbg(priv->adapter, INFO,
"info: WMM: CWmax=%d CWmin=%d Avg Back-off=%d\n",
(1 << ((ecw & NXPWIFI_ECW_MAX) >> 4)) - 1,
cw_min, avg_back_off);
nxpwifi_wmm_ac_debug_print(&wmm_ie->ac[num_ac]);
}
for (i = 0; i < num_ac; i++) {
for (j = 1; j < num_ac - i; j++) {
if (tmp[j - 1] > tmp[j]) {
swap(tmp[j - 1], tmp[j]);
swap(priv->wmm.queue_priority[j - 1],
priv->wmm.queue_priority[j]);
} else if (tmp[j - 1] == tmp[j]) {
if (priv->wmm.queue_priority[j - 1]
< priv->wmm.queue_priority[j])
swap(priv->wmm.queue_priority[j - 1],
priv->wmm.queue_priority[j]);
}
}
}
nxpwifi_wmm_queue_priorities_tid(priv);
}
static enum nxpwifi_wmm_ac_e
nxpwifi_wmm_eval_downgrade_ac(struct nxpwifi_private *priv,
enum nxpwifi_wmm_ac_e eval_ac)
{
int down_ac;
enum nxpwifi_wmm_ac_e ret_ac;
struct nxpwifi_wmm_ac_status *ac_status;
ac_status = &priv->wmm.ac_status[eval_ac];
if (!ac_status->disabled)
return eval_ac;
ret_ac = WMM_AC_BK;
for (down_ac = WMM_AC_BK; down_ac < eval_ac; down_ac++) {
ac_status = &priv->wmm.ac_status[down_ac];
if (!ac_status->disabled && !ac_status->flow_required)
ret_ac = (enum nxpwifi_wmm_ac_e)down_ac;
}
return ret_ac;
}
void
nxpwifi_wmm_setup_ac_downgrade(struct nxpwifi_private *priv)
{
int ac_val;
nxpwifi_dbg(priv->adapter, INFO, "info: WMM: AC Priorities:\t"
"BK(0), BE(1), VI(2), VO(3)\n");
if (!priv->wmm_enabled) {
for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++)
priv->wmm.ac_down_graded_vals[ac_val] =
(enum nxpwifi_wmm_ac_e)ac_val;
} else {
for (ac_val = WMM_AC_BK; ac_val <= WMM_AC_VO; ac_val++) {
priv->wmm.ac_down_graded_vals[ac_val] =
nxpwifi_wmm_eval_downgrade_ac
(priv, (enum nxpwifi_wmm_ac_e)ac_val);
nxpwifi_dbg(priv->adapter, INFO,
"info: WMM: AC PRIO %d maps to %d\n",
ac_val,
priv->wmm.ac_down_graded_vals[ac_val]);
}
}
}
static enum nxpwifi_wmm_ac_e
nxpwifi_wmm_convert_tos_to_ac(struct nxpwifi_adapter *adapter, u32 tos)
{
static const enum nxpwifi_wmm_ac_e tos_to_ac[] = {
WMM_AC_BE,
WMM_AC_BK,
WMM_AC_BK,
WMM_AC_BE,
WMM_AC_VI,
WMM_AC_VI,
WMM_AC_VO,
WMM_AC_VO
};
if (tos >= ARRAY_SIZE(tos_to_ac))
return WMM_AC_BE;
return tos_to_ac[tos];
}
u8 nxpwifi_wmm_downgrade_tid(struct nxpwifi_private *priv, u32 tid)
{
enum nxpwifi_wmm_ac_e ac, ac_down;
u8 new_tid;
ac = nxpwifi_wmm_convert_tos_to_ac(priv->adapter, tid);
ac_down = priv->wmm.ac_down_graded_vals[ac];
new_tid = ac_to_tid[ac_down][tid % 2];
return new_tid;
}
void
nxpwifi_wmm_init(struct nxpwifi_adapter *adapter)
{
int i, j;
struct nxpwifi_private *priv;
for (j = 0; j < adapter->priv_num; ++j) {
priv = adapter->priv[j];
for (i = 0; i < MAX_NUM_TID; ++i) {
if (!disable_tx_amsdu &&
adapter->tx_buf_size > NXPWIFI_TX_DATA_BUF_SIZE_2K)
priv->aggr_prio_tbl[i].amsdu =
priv->tos_to_tid_inv[i];
else
priv->aggr_prio_tbl[i].amsdu =
BA_STREAM_NOT_ALLOWED;
priv->aggr_prio_tbl[i].ampdu_ap =
priv->tos_to_tid_inv[i];
priv->aggr_prio_tbl[i].ampdu_user =
priv->tos_to_tid_inv[i];
}
priv->aggr_prio_tbl[6].amsdu =
priv->aggr_prio_tbl[6].ampdu_ap =
priv->aggr_prio_tbl[6].ampdu_user =
BA_STREAM_NOT_ALLOWED;
priv->aggr_prio_tbl[7].amsdu =
priv->aggr_prio_tbl[7].ampdu_ap =
priv->aggr_prio_tbl[7].ampdu_user =
BA_STREAM_NOT_ALLOWED;
nxpwifi_set_ba_params(priv);
nxpwifi_reset_11n_rx_seq_num(priv);
priv->wmm.drv_pkt_delay_max = NXPWIFI_WMM_DRV_DELAY_MAX;
atomic_set(&priv->wmm.tx_pkts_queued, 0);
atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID);
}
}
bool nxpwifi_bypass_txlist_empty(struct nxpwifi_adapter *adapter)
{
struct nxpwifi_private *priv;
int i;
for (i = 0; i < adapter->priv_num; i++) {
priv = adapter->priv[i];
if (!skb_queue_empty(&priv->bypass_txq))
return false;
}
return true;
}
bool nxpwifi_wmm_lists_empty(struct nxpwifi_adapter *adapter)
{
int i;
struct nxpwifi_private *priv;
for (i = 0; i < adapter->priv_num; ++i) {
priv = adapter->priv[i];
if (!priv->port_open)
continue;
if (atomic_read(&priv->wmm.tx_pkts_queued))
return false;
}
return true;
}
static void
nxpwifi_wmm_del_pkts_in_ralist_node(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ra_list)
{
struct nxpwifi_adapter *adapter = priv->adapter;
struct sk_buff *skb, *tmp;
skb_queue_walk_safe(&ra_list->skb_head, skb, tmp) {
skb_unlink(skb, &ra_list->skb_head);
nxpwifi_write_data_complete(adapter, skb, 0, -1);
}
}
static void
nxpwifi_wmm_del_pkts_in_ralist(struct nxpwifi_private *priv,
struct list_head *ra_list_head)
{
struct nxpwifi_ra_list_tbl *ra_list;
list_for_each_entry(ra_list, ra_list_head, list)
nxpwifi_wmm_del_pkts_in_ralist_node(priv, ra_list);
}
static void nxpwifi_wmm_cleanup_queues(struct nxpwifi_private *priv)
{
int i;
for (i = 0; i < MAX_NUM_TID; i++)
nxpwifi_wmm_del_pkts_in_ralist
(priv, &priv->wmm.tid_tbl_ptr[i].ra_list);
atomic_set(&priv->wmm.tx_pkts_queued, 0);
atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID);
}
static void nxpwifi_wmm_delete_all_ralist(struct nxpwifi_private *priv)
{
struct nxpwifi_ra_list_tbl *ra_list, *tmp_node;
int i;
for (i = 0; i < MAX_NUM_TID; ++i) {
nxpwifi_dbg(priv->adapter, INFO,
"info: ra_list: freeing buf for tid %d\n", i);
list_for_each_entry_safe(ra_list, tmp_node,
&priv->wmm.tid_tbl_ptr[i].ra_list,
list) {
list_del(&ra_list->list);
kfree(ra_list);
}
INIT_LIST_HEAD(&priv->wmm.tid_tbl_ptr[i].ra_list);
}
}
static int nxpwifi_free_ack_frame(int id, void *p, void *data)
{
pr_warn("Have pending ack frames!\n");
kfree_skb(p);
return 0;
}
void
nxpwifi_clean_txrx(struct nxpwifi_private *priv)
{
struct sk_buff *skb, *tmp;
unsigned long index;
void *entry;
nxpwifi_11n_cleanup_reorder_tbl(priv);
spin_lock_bh(&priv->wmm.ra_list_spinlock);
nxpwifi_wmm_cleanup_queues(priv);
nxpwifi_11n_delete_all_tx_ba_stream_tbl(priv);
if (priv->adapter->if_ops.cleanup_mpa_buf)
priv->adapter->if_ops.cleanup_mpa_buf(priv->adapter);
nxpwifi_wmm_delete_all_ralist(priv);
memcpy(tos_to_tid, ac_to_tid, sizeof(tos_to_tid));
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
skb_queue_walk_safe(&priv->bypass_txq, skb, tmp) {
skb_unlink(skb, &priv->bypass_txq);
nxpwifi_write_data_complete(priv->adapter, skb, 0, -1);
}
atomic_set(&priv->adapter->bypass_tx_pending, 0);
xa_for_each(&priv->ack_status_frames, index, entry) {
nxpwifi_free_ack_frame(index, entry, NULL);
xa_erase(&priv->ack_status_frames, index);
}
xa_destroy(&priv->ack_status_frames);
}
struct nxpwifi_ra_list_tbl *
nxpwifi_wmm_get_ralist_node(struct nxpwifi_private *priv, u8 tid,
const u8 *ra_addr)
{
struct nxpwifi_ra_list_tbl *ra_list;
list_for_each_entry(ra_list, &priv->wmm.tid_tbl_ptr[tid].ra_list,
list) {
if (!memcmp(ra_list->ra, ra_addr, ETH_ALEN))
return ra_list;
}
return NULL;
}
void nxpwifi_update_ralist_tx_pause(struct nxpwifi_private *priv, u8 *mac,
u8 tx_pause)
{
struct nxpwifi_ra_list_tbl *ra_list;
u32 pkt_cnt = 0, tx_pkts_queued;
int i;
spin_lock_bh(&priv->wmm.ra_list_spinlock);
for (i = 0; i < MAX_NUM_TID; ++i) {
ra_list = nxpwifi_wmm_get_ralist_node(priv, i, mac);
if (ra_list && ra_list->tx_paused != tx_pause) {
pkt_cnt += ra_list->total_pkt_count;
ra_list->tx_paused = tx_pause;
if (tx_pause)
priv->wmm.pkts_paused[i] +=
ra_list->total_pkt_count;
else
priv->wmm.pkts_paused[i] -=
ra_list->total_pkt_count;
}
}
if (pkt_cnt) {
tx_pkts_queued = atomic_read(&priv->wmm.tx_pkts_queued);
if (tx_pause)
tx_pkts_queued -= pkt_cnt;
else
tx_pkts_queued += pkt_cnt;
atomic_set(&priv->wmm.tx_pkts_queued, tx_pkts_queued);
atomic_set(&priv->wmm.highest_queued_prio, HIGH_PRIO_TID);
}
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
}
struct nxpwifi_ra_list_tbl *
nxpwifi_wmm_get_queue_raptr(struct nxpwifi_private *priv, u8 tid,
const u8 *ra_addr)
{
struct nxpwifi_ra_list_tbl *ra_list;
ra_list = nxpwifi_wmm_get_ralist_node(priv, tid, ra_addr);
if (ra_list)
return ra_list;
nxpwifi_ralist_add(priv, ra_addr);
return nxpwifi_wmm_get_ralist_node(priv, tid, ra_addr);
}
void
nxpwifi_wmm_del_peer_ra_list(struct nxpwifi_private *priv, const u8 *ra_addr)
{
struct nxpwifi_ra_list_tbl *ra_list;
int i;
spin_lock_bh(&priv->wmm.ra_list_spinlock);
for (i = 0; i < MAX_NUM_TID; ++i) {
ra_list = nxpwifi_wmm_get_ralist_node(priv, i, ra_addr);
if (!ra_list)
continue;
nxpwifi_wmm_del_pkts_in_ralist_node(priv, ra_list);
if (ra_list->tx_paused)
priv->wmm.pkts_paused[i] -= ra_list->total_pkt_count;
else
atomic_sub(ra_list->total_pkt_count,
&priv->wmm.tx_pkts_queued);
list_del(&ra_list->list);
kfree(ra_list);
}
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
}
bool nxpwifi_is_ralist_valid(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ra_list, int ptr_index)
{
struct nxpwifi_ra_list_tbl *rlist;
list_for_each_entry(rlist, &priv->wmm.tid_tbl_ptr[ptr_index].ra_list,
list) {
if (rlist == ra_list)
return true;
}
return false;
}
void
nxpwifi_wmm_add_buf_bypass_txqueue(struct nxpwifi_private *priv,
struct sk_buff *skb)
{
skb_queue_tail(&priv->bypass_txq, skb);
}
void
nxpwifi_wmm_add_buf_txqueue(struct nxpwifi_private *priv,
struct sk_buff *skb)
{
struct nxpwifi_adapter *adapter = priv->adapter;
u32 tid;
struct nxpwifi_ra_list_tbl *ra_list = NULL;
struct list_head list_head;
u8 ra[ETH_ALEN], tid_down;
struct ethhdr *eth_hdr = (struct ethhdr *)skb->data;
memcpy(ra, eth_hdr->h_dest, ETH_ALEN);
if (!priv->media_connected && !nxpwifi_is_skb_mgmt_frame(skb)) {
nxpwifi_dbg(adapter, DATA, "data: drop packet in disconnect\n");
nxpwifi_write_data_complete(adapter, skb, 0, -1);
return;
}
tid = skb->priority;
spin_lock_bh(&priv->wmm.ra_list_spinlock);
tid_down = nxpwifi_wmm_downgrade_tid(priv, tid);
if (!nxpwifi_queuing_ra_based(priv) &&
!nxpwifi_is_skb_mgmt_frame(skb)) {
list_head = priv->wmm.tid_tbl_ptr[tid_down].ra_list;
ra_list = list_first_entry_or_null(&list_head,
struct nxpwifi_ra_list_tbl,
list);
} else {
memcpy(ra, skb->data, ETH_ALEN);
if (is_multicast_ether_addr(ra) ||
nxpwifi_is_skb_mgmt_frame(skb))
eth_broadcast_addr(ra);
ra_list = nxpwifi_wmm_get_queue_raptr(priv, tid_down, ra);
}
if (!ra_list) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
nxpwifi_write_data_complete(adapter, skb, 0, -1);
return;
}
skb_queue_tail(&ra_list->skb_head, skb);
ra_list->ba_pkt_count++;
ra_list->total_pkt_count++;
if (atomic_read(&priv->wmm.highest_queued_prio) <
priv->tos_to_tid_inv[tid_down])
atomic_set(&priv->wmm.highest_queued_prio,
priv->tos_to_tid_inv[tid_down]);
if (ra_list->tx_paused)
priv->wmm.pkts_paused[tid_down]++;
else
atomic_inc(&priv->wmm.tx_pkts_queued);
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
}
int nxpwifi_ret_wmm_get_status(struct nxpwifi_private *priv,
const struct host_cmd_ds_command *resp)
{
u8 *curr;
u16 resp_len = le16_to_cpu(resp->size), tlv_len;
bool valid = true;
struct nxpwifi_ie_types_data *tlv_hdr;
struct nxpwifi_ie_types_wmm_queue_status *wmm_qs;
struct ieee80211_wmm_param_ie *wmm_param_ie = NULL;
struct nxpwifi_wmm_ac_status *ac_status;
u32 base;
nxpwifi_dbg(priv->adapter, INFO,
"info: WMM: WMM_GET_STATUS cmdresp received: %d\n",
resp_len);
base = offsetofend(struct host_cmd_ds_command, params.get_wmm_status);
if (resp_len < base)
return -EINVAL;
curr = (u8 *)&resp->params.get_wmm_status;
resp_len -= base;
while (resp_len >= sizeof(tlv_hdr->header) && valid) {
tlv_hdr = (struct nxpwifi_ie_types_data *)curr;
tlv_len = le16_to_cpu(tlv_hdr->header.len);
if (resp_len < tlv_len + sizeof(tlv_hdr->header))
break;
switch (le16_to_cpu(tlv_hdr->header.type)) {
case TLV_TYPE_WMMQSTATUS:
if (tlv_len <
sizeof(struct nxpwifi_ie_types_wmm_queue_status) -
sizeof(tlv_hdr->header))
break;
wmm_qs =
(struct nxpwifi_ie_types_wmm_queue_status *)tlv_hdr;
if (wmm_qs->queue_index >= IEEE80211_NUM_ACS)
break;
ac_status = &priv->wmm.ac_status[wmm_qs->queue_index];
ac_status->disabled = wmm_qs->disabled;
ac_status->flow_required = wmm_qs->flow_required;
ac_status->flow_created = wmm_qs->flow_created;
break;
case WLAN_EID_VENDOR_SPECIFIC:
if (tlv_len + sizeof(tlv_hdr->header) <
offsetofend(struct ieee80211_wmm_param_ie,
qos_info))
break;
wmm_param_ie =
(struct ieee80211_wmm_param_ie *)(curr + 2);
if (tlv_len + 2 > sizeof(struct ieee80211_wmm_param_ie))
break;
wmm_param_ie->len = (u8)tlv_len;
wmm_param_ie->element_id = WLAN_EID_VENDOR_SPECIFIC;
memcpy(&priv->curr_bss_params.bss_descriptor.wmm_ie,
wmm_param_ie, wmm_param_ie->len + 2);
break;
default:
valid = false;
break;
}
curr += sizeof(tlv_hdr->header) + tlv_len;
resp_len -= sizeof(tlv_hdr->header) + tlv_len;
}
nxpwifi_wmm_setup_queue_priorities(priv, wmm_param_ie);
nxpwifi_wmm_setup_ac_downgrade(priv);
return 0;
}
u32
nxpwifi_wmm_process_association_req(struct nxpwifi_private *priv,
u8 **assoc_buf,
struct ieee80211_wmm_param_ie *wmm_ie,
struct ieee80211_ht_cap *ht_cap)
{
struct nxpwifi_ie_types_wmm_param_set *wmm_tlv;
u32 ret_len = 0;
if (!assoc_buf)
return 0;
if (!(*assoc_buf))
return 0;
if (!wmm_ie)
return 0;
nxpwifi_dbg(priv->adapter, INFO,
"info: WMM: process assoc req: bss->wmm_ie=%#x\n",
wmm_ie->element_id);
if ((priv->wmm_required ||
(ht_cap && (priv->config_bands & BAND_GN ||
priv->config_bands & BAND_AN))) &&
wmm_ie->element_id == WLAN_EID_VENDOR_SPECIFIC) {
wmm_tlv = (struct nxpwifi_ie_types_wmm_param_set *)*assoc_buf;
wmm_tlv->header.type = cpu_to_le16((u16)wmm_info_ie[0]);
wmm_tlv->header.len = cpu_to_le16((u16)wmm_info_ie[1]);
memcpy(wmm_tlv->wmm_ie, &wmm_info_ie[2],
le16_to_cpu(wmm_tlv->header.len));
if (wmm_ie->qos_info & IEEE80211_WMM_IE_AP_QOSINFO_UAPSD)
memcpy((u8 *)(wmm_tlv->wmm_ie
+ le16_to_cpu(wmm_tlv->header.len)
- sizeof(priv->wmm_qosinfo)),
&priv->wmm_qosinfo, sizeof(priv->wmm_qosinfo));
ret_len = sizeof(wmm_tlv->header)
+ le16_to_cpu(wmm_tlv->header.len);
*assoc_buf += ret_len;
}
return ret_len;
}
u8
nxpwifi_wmm_compute_drv_pkt_delay(struct nxpwifi_private *priv,
const struct sk_buff *skb)
{
u32 queue_delay = ktime_to_ms(net_timedelta(skb->tstamp));
u8 ret_val;
ret_val = (u8)(min(queue_delay, priv->wmm.drv_pkt_delay_max) >> 1);
nxpwifi_dbg(priv->adapter, DATA, "data: WMM: Pkt Delay: %d ms,\t"
"%d ms sent to FW\n", queue_delay, ret_val);
return ret_val;
}
static struct nxpwifi_ra_list_tbl *
nxpwifi_wmm_get_highest_priolist_ptr(struct nxpwifi_adapter *adapter,
struct nxpwifi_private **priv, int *tid)
{
struct nxpwifi_private *priv_tmp;
struct nxpwifi_ra_list_tbl *ptr;
struct nxpwifi_tid_tbl *tid_ptr;
atomic_t *hqp;
int i, j;
u8 to_tid;
for (j = adapter->priv_num - 1; j >= 0; --j) {
list_for_each_entry(adapter->bss_prio_tbl[j].bss_prio_cur,
&adapter->bss_prio_tbl[j].bss_prio_head,
list) {
try_again:
priv_tmp = adapter->bss_prio_tbl[j].bss_prio_cur->priv;
if (!priv_tmp->port_open ||
(atomic_read(&priv_tmp->wmm.tx_pkts_queued) == 0))
continue;
hqp = &priv_tmp->wmm.highest_queued_prio;
for (i = atomic_read(hqp); i >= LOW_PRIO_TID; --i) {
spin_lock_bh(&priv_tmp->wmm.ra_list_spinlock);
to_tid = tos_to_tid[i];
tid_ptr = &(priv_tmp)->wmm.tid_tbl_ptr[to_tid];
list_for_each_entry(ptr, &tid_ptr->ra_list,
list) {
if (!ptr->tx_paused &&
!skb_queue_empty(&ptr->skb_head))
goto found;
}
spin_unlock_bh(&priv_tmp->wmm.ra_list_spinlock);
}
if (atomic_read(&priv_tmp->wmm.tx_pkts_queued) != 0) {
atomic_set(&priv_tmp->wmm.highest_queued_prio,
HIGH_PRIO_TID);
goto try_again;
} else {
atomic_set(&priv_tmp->wmm.highest_queued_prio,
NO_PKT_PRIO_TID);
}
}
}
return NULL;
found:
if (atomic_read(hqp) > i)
atomic_set(hqp, i);
spin_unlock_bh(&priv_tmp->wmm.ra_list_spinlock);
*priv = priv_tmp;
*tid = tos_to_tid[i];
return ptr;
}
void nxpwifi_rotate_priolists(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ra,
int tid)
{
struct nxpwifi_adapter *adapter = priv->adapter;
struct nxpwifi_bss_prio_tbl *tbl = adapter->bss_prio_tbl;
struct nxpwifi_tid_tbl *tid_ptr = &priv->wmm.tid_tbl_ptr[tid];
spin_lock_bh(&tbl[priv->bss_priority].bss_prio_lock);
list_move(&tbl[priv->bss_priority].bss_prio_head,
&tbl[priv->bss_priority].bss_prio_cur->list);
spin_unlock_bh(&tbl[priv->bss_priority].bss_prio_lock);
spin_lock_bh(&priv->wmm.ra_list_spinlock);
if (nxpwifi_is_ralist_valid(priv, ra, tid)) {
priv->wmm.packets_out[tid]++;
list_move(&tid_ptr->ra_list, &ra->list);
}
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
}
static bool
nxpwifi_is_11n_aggragation_possible(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ptr,
int max_buf_size)
{
int count = 0, total_size = 0;
struct sk_buff *skb, *tmp;
int max_amsdu_size;
if (priv->bss_role == NXPWIFI_BSS_ROLE_UAP && priv->ap_11n_enabled &&
ptr->is_11n_enabled)
max_amsdu_size = min_t(int, ptr->max_amsdu, max_buf_size);
else
max_amsdu_size = max_buf_size;
skb_queue_walk_safe(&ptr->skb_head, skb, tmp) {
total_size += skb->len;
if (total_size >= max_amsdu_size)
break;
if (++count >= MIN_NUM_AMSDU)
return true;
}
return false;
}
static void
nxpwifi_send_single_packet(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ptr, int ptr_index)
__releases(&priv->wmm.ra_list_spinlock)
{
struct sk_buff *skb, *skb_next;
struct nxpwifi_tx_param tx_param;
struct nxpwifi_adapter *adapter = priv->adapter;
struct nxpwifi_txinfo *tx_info;
if (skb_queue_empty(&ptr->skb_head)) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
nxpwifi_dbg(adapter, DATA, "data: nothing to send\n");
return;
}
skb = skb_dequeue(&ptr->skb_head);
tx_info = NXPWIFI_SKB_TXCB(skb);
nxpwifi_dbg(adapter, DATA,
"data: dequeuing the packet %p %p\n", ptr, skb);
ptr->total_pkt_count--;
if (!skb_queue_empty(&ptr->skb_head))
skb_next = skb_peek(&ptr->skb_head);
else
skb_next = NULL;
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
tx_param.next_pkt_len = ((skb_next) ? skb_next->len +
sizeof(struct txpd) : 0);
if (nxpwifi_process_tx(priv, skb, &tx_param) == -EBUSY) {
spin_lock_bh(&priv->wmm.ra_list_spinlock);
if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
nxpwifi_write_data_complete(adapter, skb, 0, -1);
return;
}
skb_queue_tail(&ptr->skb_head, skb);
ptr->total_pkt_count++;
ptr->ba_pkt_count++;
tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT;
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
} else {
nxpwifi_rotate_priolists(priv, ptr, ptr_index);
atomic_dec(&priv->wmm.tx_pkts_queued);
}
}
static bool
nxpwifi_is_ptr_processed(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ptr)
{
struct sk_buff *skb;
struct nxpwifi_txinfo *tx_info;
if (skb_queue_empty(&ptr->skb_head))
return false;
skb = skb_peek(&ptr->skb_head);
tx_info = NXPWIFI_SKB_TXCB(skb);
if (tx_info->flags & NXPWIFI_BUF_FLAG_REQUEUED_PKT)
return true;
return false;
}
static void
nxpwifi_send_processed_packet(struct nxpwifi_private *priv,
struct nxpwifi_ra_list_tbl *ptr, int ptr_index)
__releases(&priv->wmm.ra_list_spinlock)
{
struct nxpwifi_tx_param tx_param;
struct nxpwifi_adapter *adapter = priv->adapter;
int ret;
struct sk_buff *skb, *skb_next;
struct nxpwifi_txinfo *tx_info;
if (skb_queue_empty(&ptr->skb_head)) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
return;
}
skb = skb_dequeue(&ptr->skb_head);
if (adapter->data_sent || adapter->tx_lock_flag) {
ptr->total_pkt_count--;
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
skb_queue_tail(&adapter->tx_data_q, skb);
atomic_dec(&priv->wmm.tx_pkts_queued);
atomic_inc(&adapter->tx_queued);
return;
}
if (!skb_queue_empty(&ptr->skb_head))
skb_next = skb_peek(&ptr->skb_head);
else
skb_next = NULL;
tx_info = NXPWIFI_SKB_TXCB(skb);
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
tx_param.next_pkt_len =
((skb_next) ? skb_next->len +
sizeof(struct txpd) : 0);
ret = adapter->if_ops.host_to_card(adapter, NXPWIFI_TYPE_DATA,
skb, &tx_param);
switch (ret) {
case -EBUSY:
nxpwifi_dbg(adapter, ERROR, "data: -EBUSY is returned\n");
spin_lock_bh(&priv->wmm.ra_list_spinlock);
if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
nxpwifi_write_data_complete(adapter, skb, 0, -1);
return;
}
skb_queue_tail(&ptr->skb_head, skb);
tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT;
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
break;
case -EINPROGRESS:
break;
case 0:
nxpwifi_write_data_complete(adapter, skb, 0, ret);
break;
default:
nxpwifi_dbg(adapter, ERROR, "host_to_card failed: %#x\n", ret);
adapter->dbg.num_tx_host_to_card_failure++;
nxpwifi_write_data_complete(adapter, skb, 0, ret);
break;
}
if (ret != -EBUSY) {
nxpwifi_rotate_priolists(priv, ptr, ptr_index);
atomic_dec(&priv->wmm.tx_pkts_queued);
spin_lock_bh(&priv->wmm.ra_list_spinlock);
ptr->total_pkt_count--;
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
}
}
static int
nxpwifi_dequeue_tx_packet(struct nxpwifi_adapter *adapter)
{
struct nxpwifi_ra_list_tbl *ptr;
struct nxpwifi_private *priv = NULL;
int ptr_index = 0;
u8 ra[ETH_ALEN];
int tid_del = 0, tid = 0;
ptr = nxpwifi_wmm_get_highest_priolist_ptr(adapter, &priv, &ptr_index);
if (!ptr)
return -ENOENT;
tid = nxpwifi_get_tid(ptr);
nxpwifi_dbg(adapter, DATA, "data: tid=%d\n", tid);
spin_lock_bh(&priv->wmm.ra_list_spinlock);
if (!nxpwifi_is_ralist_valid(priv, ptr, ptr_index)) {
spin_unlock_bh(&priv->wmm.ra_list_spinlock);
return -EINVAL;
}
if (nxpwifi_is_ptr_processed(priv, ptr)) {
nxpwifi_send_processed_packet(priv, ptr, ptr_index);
return 0;
}
if (!ptr->is_11n_enabled ||
ptr->ba_status ||
priv->wps.session_enable) {
if (ptr->is_11n_enabled &&
ptr->ba_status &&
ptr->amsdu_in_ampdu &&
nxpwifi_is_amsdu_allowed(priv, tid) &&
nxpwifi_is_11n_aggragation_possible(priv, ptr,
adapter->tx_buf_size))
nxpwifi_11n_aggregate_pkt(priv, ptr, ptr_index);
else
nxpwifi_send_single_packet(priv, ptr, ptr_index);
} else {
if (nxpwifi_is_ampdu_allowed(priv, ptr, tid) &&
ptr->ba_pkt_count > ptr->ba_packet_thr) {
if (nxpwifi_space_avail_for_new_ba_stream(adapter)) {
nxpwifi_create_ba_tbl(priv, ptr->ra, tid,
BA_SETUP_INPROGRESS);
nxpwifi_send_addba(priv, tid, ptr->ra);
} else if (nxpwifi_find_stream_to_delete
(priv, tid, &tid_del, ra)) {
nxpwifi_create_ba_tbl(priv, ptr->ra, tid,
BA_SETUP_INPROGRESS);
nxpwifi_send_delba(priv, tid_del, ra, 1);
}
}
if (nxpwifi_is_amsdu_allowed(priv, tid) &&
nxpwifi_is_11n_aggragation_possible(priv, ptr,
adapter->tx_buf_size))
nxpwifi_11n_aggregate_pkt(priv, ptr, ptr_index);
else
nxpwifi_send_single_packet(priv, ptr, ptr_index);
}
return 0;
}
void nxpwifi_process_bypass_tx(struct nxpwifi_adapter *adapter)
{
struct nxpwifi_tx_param tx_param;
struct sk_buff *skb;
struct nxpwifi_txinfo *tx_info;
struct nxpwifi_private *priv;
int i;
if (adapter->data_sent || adapter->tx_lock_flag)
return;
for (i = 0; i < adapter->priv_num; ++i) {
priv = adapter->priv[i];
if (skb_queue_empty(&priv->bypass_txq))
continue;
skb = skb_dequeue(&priv->bypass_txq);
tx_info = NXPWIFI_SKB_TXCB(skb);
tx_param.next_pkt_len = 0;
if (nxpwifi_process_tx(priv, skb, &tx_param) == -EBUSY) {
skb_queue_head(&priv->bypass_txq, skb);
tx_info->flags |= NXPWIFI_BUF_FLAG_REQUEUED_PKT;
} else {
atomic_dec(&adapter->bypass_tx_pending);
}
}
}
void
nxpwifi_wmm_process_tx(struct nxpwifi_adapter *adapter)
{
do {
if (nxpwifi_dequeue_tx_packet(adapter))
break;
if (adapter->iface_type != NXPWIFI_SDIO) {
if (adapter->data_sent ||
adapter->tx_lock_flag)
break;
} else {
if (atomic_read(&adapter->tx_queued) >=
NXPWIFI_MAX_PKTS_TXQ)
break;
}
} while (!nxpwifi_wmm_lists_empty(adapter));
}
void nxpwifi_wmm_init_tos_to_tid_inv(struct nxpwifi_private *priv)
{
memcpy(priv->tos_to_tid_inv, tos_to_tid_inv, sizeof(priv->tos_to_tid_inv));
}