10e3d6777SRyder Lee // SPDX-License-Identifier: ISC 2aee5b8cfSFelix Fietkau /* 3aee5b8cfSFelix Fietkau * Copyright (C) 2018 Felix Fietkau <nbd@nbd.name> 4aee5b8cfSFelix Fietkau */ 5aee5b8cfSFelix Fietkau #include "mt76.h" 6aee5b8cfSFelix Fietkau 7e7ec563eSMarkus Theil static unsigned long mt76_aggr_tid_to_timeo(u8 tidno) 8e7ec563eSMarkus Theil { 9e7ec563eSMarkus Theil /* Currently voice traffic (AC_VO) always runs without aggregation, 10e7ec563eSMarkus Theil * no special handling is needed. AC_BE/AC_BK use tids 0-3. Just check 11e7ec563eSMarkus Theil * for non AC_BK/AC_BE and set smaller timeout for it. */ 12e7ec563eSMarkus Theil return HZ / (tidno >= 4 ? 25 : 10); 13e7ec563eSMarkus Theil } 14aee5b8cfSFelix Fietkau 15aee5b8cfSFelix Fietkau static void 16aee5b8cfSFelix Fietkau mt76_aggr_release(struct mt76_rx_tid *tid, struct sk_buff_head *frames, int idx) 17aee5b8cfSFelix Fietkau { 18aee5b8cfSFelix Fietkau struct sk_buff *skb; 19aee5b8cfSFelix Fietkau 20aee5b8cfSFelix Fietkau tid->head = ieee80211_sn_inc(tid->head); 21aee5b8cfSFelix Fietkau 22aee5b8cfSFelix Fietkau skb = tid->reorder_buf[idx]; 23aee5b8cfSFelix Fietkau if (!skb) 24aee5b8cfSFelix Fietkau return; 25aee5b8cfSFelix Fietkau 26aee5b8cfSFelix Fietkau tid->reorder_buf[idx] = NULL; 27aee5b8cfSFelix Fietkau tid->nframes--; 28aee5b8cfSFelix Fietkau __skb_queue_tail(frames, skb); 29aee5b8cfSFelix Fietkau } 30aee5b8cfSFelix Fietkau 31aee5b8cfSFelix Fietkau static void 3213381dcdSRyder Lee mt76_rx_aggr_release_frames(struct mt76_rx_tid *tid, 3313381dcdSRyder Lee struct sk_buff_head *frames, 34aee5b8cfSFelix Fietkau u16 head) 35aee5b8cfSFelix Fietkau { 36aee5b8cfSFelix Fietkau int idx; 37aee5b8cfSFelix Fietkau 38aee5b8cfSFelix Fietkau while (ieee80211_sn_less(tid->head, head)) { 39aee5b8cfSFelix Fietkau idx = tid->head % tid->size; 40aee5b8cfSFelix Fietkau mt76_aggr_release(tid, frames, idx); 41aee5b8cfSFelix Fietkau } 42aee5b8cfSFelix Fietkau } 43aee5b8cfSFelix Fietkau 44aee5b8cfSFelix Fietkau static void 45aee5b8cfSFelix Fietkau mt76_rx_aggr_release_head(struct mt76_rx_tid *tid, struct sk_buff_head *frames) 46aee5b8cfSFelix Fietkau { 47aee5b8cfSFelix Fietkau int idx = tid->head % tid->size; 48aee5b8cfSFelix Fietkau 49aee5b8cfSFelix Fietkau while (tid->reorder_buf[idx]) { 50aee5b8cfSFelix Fietkau mt76_aggr_release(tid, frames, idx); 51aee5b8cfSFelix Fietkau idx = tid->head % tid->size; 52aee5b8cfSFelix Fietkau } 53aee5b8cfSFelix Fietkau } 54aee5b8cfSFelix Fietkau 55aee5b8cfSFelix Fietkau static void 56aee5b8cfSFelix Fietkau mt76_rx_aggr_check_release(struct mt76_rx_tid *tid, struct sk_buff_head *frames) 57aee5b8cfSFelix Fietkau { 58aee5b8cfSFelix Fietkau struct mt76_rx_status *status; 59aee5b8cfSFelix Fietkau struct sk_buff *skb; 60aee5b8cfSFelix Fietkau int start, idx, nframes; 61aee5b8cfSFelix Fietkau 62aee5b8cfSFelix Fietkau if (!tid->nframes) 63aee5b8cfSFelix Fietkau return; 64aee5b8cfSFelix Fietkau 65aee5b8cfSFelix Fietkau mt76_rx_aggr_release_head(tid, frames); 66aee5b8cfSFelix Fietkau 67aee5b8cfSFelix Fietkau start = tid->head % tid->size; 68aee5b8cfSFelix Fietkau nframes = tid->nframes; 69aee5b8cfSFelix Fietkau 70aee5b8cfSFelix Fietkau for (idx = (tid->head + 1) % tid->size; 71aee5b8cfSFelix Fietkau idx != start && nframes; 72aee5b8cfSFelix Fietkau idx = (idx + 1) % tid->size) { 73aee5b8cfSFelix Fietkau skb = tid->reorder_buf[idx]; 74aee5b8cfSFelix Fietkau if (!skb) 75aee5b8cfSFelix Fietkau continue; 76aee5b8cfSFelix Fietkau 77aee5b8cfSFelix Fietkau nframes--; 78aee5b8cfSFelix Fietkau status = (struct mt76_rx_status *)skb->cb; 7913381dcdSRyder Lee if (!time_after(jiffies, 80e7ec563eSMarkus Theil status->reorder_time + 81e7ec563eSMarkus Theil mt76_aggr_tid_to_timeo(tid->num))) 82aee5b8cfSFelix Fietkau continue; 83aee5b8cfSFelix Fietkau 84aee5b8cfSFelix Fietkau mt76_rx_aggr_release_frames(tid, frames, status->seqno); 85aee5b8cfSFelix Fietkau } 86aee5b8cfSFelix Fietkau 87aee5b8cfSFelix Fietkau mt76_rx_aggr_release_head(tid, frames); 88aee5b8cfSFelix Fietkau } 89aee5b8cfSFelix Fietkau 90aee5b8cfSFelix Fietkau static void 91aee5b8cfSFelix Fietkau mt76_rx_aggr_reorder_work(struct work_struct *work) 92aee5b8cfSFelix Fietkau { 93aee5b8cfSFelix Fietkau struct mt76_rx_tid *tid = container_of(work, struct mt76_rx_tid, 94aee5b8cfSFelix Fietkau reorder_work.work); 95aee5b8cfSFelix Fietkau struct mt76_dev *dev = tid->dev; 96aee5b8cfSFelix Fietkau struct sk_buff_head frames; 97fb208dc7SFelix Fietkau int nframes; 98aee5b8cfSFelix Fietkau 99aee5b8cfSFelix Fietkau __skb_queue_head_init(&frames); 100aee5b8cfSFelix Fietkau 101aee5b8cfSFelix Fietkau local_bh_disable(); 1029febfa67SFelix Fietkau rcu_read_lock(); 103aee5b8cfSFelix Fietkau 104aee5b8cfSFelix Fietkau spin_lock(&tid->lock); 105aee5b8cfSFelix Fietkau mt76_rx_aggr_check_release(tid, &frames); 106fb208dc7SFelix Fietkau nframes = tid->nframes; 107aee5b8cfSFelix Fietkau spin_unlock(&tid->lock); 108aee5b8cfSFelix Fietkau 109fb208dc7SFelix Fietkau if (nframes) 110fb208dc7SFelix Fietkau ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work, 111e7ec563eSMarkus Theil mt76_aggr_tid_to_timeo(tid->num)); 11281e850efSLorenzo Bianconi mt76_rx_complete(dev, &frames, NULL); 113aee5b8cfSFelix Fietkau 1149febfa67SFelix Fietkau rcu_read_unlock(); 115aee5b8cfSFelix Fietkau local_bh_enable(); 116aee5b8cfSFelix Fietkau } 117aee5b8cfSFelix Fietkau 11817cf68b7SFelix Fietkau static void 11917cf68b7SFelix Fietkau mt76_rx_aggr_check_ctl(struct sk_buff *skb, struct sk_buff_head *frames) 12017cf68b7SFelix Fietkau { 12117cf68b7SFelix Fietkau struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb; 12277ae1d5eSRyder Lee struct ieee80211_bar *bar = mt76_skb_get_hdr(skb); 12317cf68b7SFelix Fietkau struct mt76_wcid *wcid = status->wcid; 12417cf68b7SFelix Fietkau struct mt76_rx_tid *tid; 125*e195dad1SFelix Fietkau u8 tidno = status->qos_ctl & IEEE80211_QOS_CTL_TID_MASK; 12617cf68b7SFelix Fietkau u16 seqno; 12717cf68b7SFelix Fietkau 12817cf68b7SFelix Fietkau if (!ieee80211_is_ctl(bar->frame_control)) 12917cf68b7SFelix Fietkau return; 13017cf68b7SFelix Fietkau 13117cf68b7SFelix Fietkau if (!ieee80211_is_back_req(bar->frame_control)) 13217cf68b7SFelix Fietkau return; 13317cf68b7SFelix Fietkau 134*e195dad1SFelix Fietkau status->qos_ctl = tidno = le16_to_cpu(bar->control) >> 12; 135b183878aSRyder Lee seqno = IEEE80211_SEQ_TO_SN(le16_to_cpu(bar->start_seq_num)); 136*e195dad1SFelix Fietkau tid = rcu_dereference(wcid->aggr[tidno]); 13717cf68b7SFelix Fietkau if (!tid) 13817cf68b7SFelix Fietkau return; 13917cf68b7SFelix Fietkau 14017cf68b7SFelix Fietkau spin_lock_bh(&tid->lock); 141e7aaa72fSFelix Fietkau if (!tid->stopped) { 14217cf68b7SFelix Fietkau mt76_rx_aggr_release_frames(tid, frames, seqno); 14317cf68b7SFelix Fietkau mt76_rx_aggr_release_head(tid, frames); 144e7aaa72fSFelix Fietkau } 14517cf68b7SFelix Fietkau spin_unlock_bh(&tid->lock); 14617cf68b7SFelix Fietkau } 14717cf68b7SFelix Fietkau 148aee5b8cfSFelix Fietkau void mt76_rx_aggr_reorder(struct sk_buff *skb, struct sk_buff_head *frames) 149aee5b8cfSFelix Fietkau { 150aee5b8cfSFelix Fietkau struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb; 151aee5b8cfSFelix Fietkau struct mt76_wcid *wcid = status->wcid; 152aee5b8cfSFelix Fietkau struct ieee80211_sta *sta; 153aee5b8cfSFelix Fietkau struct mt76_rx_tid *tid; 154aee5b8cfSFelix Fietkau bool sn_less; 1557c4f744dSRyder Lee u16 seqno, head, size, idx; 156*e195dad1SFelix Fietkau u8 tidno = status->qos_ctl & IEEE80211_QOS_CTL_TID_MASK; 1577c4f744dSRyder Lee u8 ackp; 158aee5b8cfSFelix Fietkau 159aee5b8cfSFelix Fietkau __skb_queue_tail(frames, skb); 160aee5b8cfSFelix Fietkau 161aee5b8cfSFelix Fietkau sta = wcid_to_sta(wcid); 16217cf68b7SFelix Fietkau if (!sta) 163aee5b8cfSFelix Fietkau return; 164aee5b8cfSFelix Fietkau 165*e195dad1SFelix Fietkau if (!status->aggr && !(status->flag & RX_FLAG_8023)) { 16617cf68b7SFelix Fietkau mt76_rx_aggr_check_ctl(skb, frames); 16717cf68b7SFelix Fietkau return; 16817cf68b7SFelix Fietkau } 16917cf68b7SFelix Fietkau 1701af83148SFelix Fietkau /* not part of a BA session */ 171*e195dad1SFelix Fietkau ackp = status->qos_ctl & IEEE80211_QOS_CTL_ACK_POLICY_MASK; 1721af83148SFelix Fietkau if (ackp != IEEE80211_QOS_CTL_ACK_POLICY_BLOCKACK && 1731af83148SFelix Fietkau ackp != IEEE80211_QOS_CTL_ACK_POLICY_NORMAL) 1741af83148SFelix Fietkau return; 1751af83148SFelix Fietkau 176*e195dad1SFelix Fietkau tid = rcu_dereference(wcid->aggr[tidno]); 177aee5b8cfSFelix Fietkau if (!tid) 178aee5b8cfSFelix Fietkau return; 179aee5b8cfSFelix Fietkau 18018efed59SFelix Fietkau status->flag |= RX_FLAG_DUP_VALIDATED; 181aee5b8cfSFelix Fietkau spin_lock_bh(&tid->lock); 182aee5b8cfSFelix Fietkau 183aee5b8cfSFelix Fietkau if (tid->stopped) 184aee5b8cfSFelix Fietkau goto out; 185aee5b8cfSFelix Fietkau 186aee5b8cfSFelix Fietkau head = tid->head; 187aee5b8cfSFelix Fietkau seqno = status->seqno; 188aee5b8cfSFelix Fietkau size = tid->size; 189aee5b8cfSFelix Fietkau sn_less = ieee80211_sn_less(seqno, head); 190aee5b8cfSFelix Fietkau 191aee5b8cfSFelix Fietkau if (!tid->started) { 192aee5b8cfSFelix Fietkau if (sn_less) 193aee5b8cfSFelix Fietkau goto out; 194aee5b8cfSFelix Fietkau 195aee5b8cfSFelix Fietkau tid->started = true; 196aee5b8cfSFelix Fietkau } 197aee5b8cfSFelix Fietkau 198aee5b8cfSFelix Fietkau if (sn_less) { 199aee5b8cfSFelix Fietkau __skb_unlink(skb, frames); 200aee5b8cfSFelix Fietkau dev_kfree_skb(skb); 201aee5b8cfSFelix Fietkau goto out; 202aee5b8cfSFelix Fietkau } 203aee5b8cfSFelix Fietkau 204aee5b8cfSFelix Fietkau if (seqno == head) { 205aee5b8cfSFelix Fietkau tid->head = ieee80211_sn_inc(head); 206aee5b8cfSFelix Fietkau if (tid->nframes) 207aee5b8cfSFelix Fietkau mt76_rx_aggr_release_head(tid, frames); 208aee5b8cfSFelix Fietkau goto out; 209aee5b8cfSFelix Fietkau } 210aee5b8cfSFelix Fietkau 211aee5b8cfSFelix Fietkau __skb_unlink(skb, frames); 212aee5b8cfSFelix Fietkau 213aee5b8cfSFelix Fietkau /* 214aee5b8cfSFelix Fietkau * Frame sequence number exceeds buffering window, free up some space 215aee5b8cfSFelix Fietkau * by releasing previous frames 216aee5b8cfSFelix Fietkau */ 217aee5b8cfSFelix Fietkau if (!ieee80211_sn_less(seqno, head + size)) { 218aee5b8cfSFelix Fietkau head = ieee80211_sn_inc(ieee80211_sn_sub(seqno, size)); 219aee5b8cfSFelix Fietkau mt76_rx_aggr_release_frames(tid, frames, head); 220aee5b8cfSFelix Fietkau } 221aee5b8cfSFelix Fietkau 222aee5b8cfSFelix Fietkau idx = seqno % size; 223aee5b8cfSFelix Fietkau 224aee5b8cfSFelix Fietkau /* Discard if the current slot is already in use */ 225aee5b8cfSFelix Fietkau if (tid->reorder_buf[idx]) { 226aee5b8cfSFelix Fietkau dev_kfree_skb(skb); 227aee5b8cfSFelix Fietkau goto out; 228aee5b8cfSFelix Fietkau } 229aee5b8cfSFelix Fietkau 230aee5b8cfSFelix Fietkau status->reorder_time = jiffies; 231aee5b8cfSFelix Fietkau tid->reorder_buf[idx] = skb; 232aee5b8cfSFelix Fietkau tid->nframes++; 233aee5b8cfSFelix Fietkau mt76_rx_aggr_release_head(tid, frames); 234aee5b8cfSFelix Fietkau 23513381dcdSRyder Lee ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work, 236e7ec563eSMarkus Theil mt76_aggr_tid_to_timeo(tid->num)); 237aee5b8cfSFelix Fietkau 238aee5b8cfSFelix Fietkau out: 239aee5b8cfSFelix Fietkau spin_unlock_bh(&tid->lock); 240aee5b8cfSFelix Fietkau } 241aee5b8cfSFelix Fietkau 242aee5b8cfSFelix Fietkau int mt76_rx_aggr_start(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno, 2437c4f744dSRyder Lee u16 ssn, u16 size) 244aee5b8cfSFelix Fietkau { 245aee5b8cfSFelix Fietkau struct mt76_rx_tid *tid; 246aee5b8cfSFelix Fietkau 247aee5b8cfSFelix Fietkau mt76_rx_aggr_stop(dev, wcid, tidno); 248aee5b8cfSFelix Fietkau 249acafe7e3SKees Cook tid = kzalloc(struct_size(tid, reorder_buf, size), GFP_KERNEL); 250aee5b8cfSFelix Fietkau if (!tid) 251aee5b8cfSFelix Fietkau return -ENOMEM; 252aee5b8cfSFelix Fietkau 253aee5b8cfSFelix Fietkau tid->dev = dev; 254aee5b8cfSFelix Fietkau tid->head = ssn; 255aee5b8cfSFelix Fietkau tid->size = size; 256e7ec563eSMarkus Theil tid->num = tidno; 257aee5b8cfSFelix Fietkau INIT_DELAYED_WORK(&tid->reorder_work, mt76_rx_aggr_reorder_work); 258aee5b8cfSFelix Fietkau spin_lock_init(&tid->lock); 259aee5b8cfSFelix Fietkau 260aee5b8cfSFelix Fietkau rcu_assign_pointer(wcid->aggr[tidno], tid); 261aee5b8cfSFelix Fietkau 262aee5b8cfSFelix Fietkau return 0; 263aee5b8cfSFelix Fietkau } 264aee5b8cfSFelix Fietkau EXPORT_SYMBOL_GPL(mt76_rx_aggr_start); 265aee5b8cfSFelix Fietkau 266aee5b8cfSFelix Fietkau static void mt76_rx_aggr_shutdown(struct mt76_dev *dev, struct mt76_rx_tid *tid) 267aee5b8cfSFelix Fietkau { 2687c4f744dSRyder Lee u16 size = tid->size; 269aee5b8cfSFelix Fietkau int i; 270aee5b8cfSFelix Fietkau 271aee5b8cfSFelix Fietkau spin_lock_bh(&tid->lock); 272aee5b8cfSFelix Fietkau 273aee5b8cfSFelix Fietkau tid->stopped = true; 274aee5b8cfSFelix Fietkau for (i = 0; tid->nframes && i < size; i++) { 275aee5b8cfSFelix Fietkau struct sk_buff *skb = tid->reorder_buf[i]; 276aee5b8cfSFelix Fietkau 277aee5b8cfSFelix Fietkau if (!skb) 278aee5b8cfSFelix Fietkau continue; 279aee5b8cfSFelix Fietkau 2809379df2fSFelix Fietkau tid->reorder_buf[i] = NULL; 281aee5b8cfSFelix Fietkau tid->nframes--; 282aee5b8cfSFelix Fietkau dev_kfree_skb(skb); 283aee5b8cfSFelix Fietkau } 284aee5b8cfSFelix Fietkau 285aee5b8cfSFelix Fietkau spin_unlock_bh(&tid->lock); 286e7aaa72fSFelix Fietkau 287e7aaa72fSFelix Fietkau cancel_delayed_work_sync(&tid->reorder_work); 288aee5b8cfSFelix Fietkau } 289aee5b8cfSFelix Fietkau 290aee5b8cfSFelix Fietkau void mt76_rx_aggr_stop(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno) 291aee5b8cfSFelix Fietkau { 292fb7d95c6SFelix Fietkau struct mt76_rx_tid *tid = NULL; 293aee5b8cfSFelix Fietkau 294a191c9e9SPaul E. McKenney tid = rcu_replace_pointer(wcid->aggr[tidno], tid, 295fb7d95c6SFelix Fietkau lockdep_is_held(&dev->mutex)); 296aee5b8cfSFelix Fietkau if (tid) { 297aee5b8cfSFelix Fietkau mt76_rx_aggr_shutdown(dev, tid); 298aee5b8cfSFelix Fietkau kfree_rcu(tid, rcu_head); 299aee5b8cfSFelix Fietkau } 300aee5b8cfSFelix Fietkau } 301aee5b8cfSFelix Fietkau EXPORT_SYMBOL_GPL(mt76_rx_aggr_stop); 302