1 // SPDX-License-Identifier: ISC 2 /* 3 * Copyright (C) 2018 Felix Fietkau <nbd@nbd.name> 4 */ 5 #include "mt76.h" 6 7 static unsigned long mt76_aggr_tid_to_timeo(u8 tidno) 8 { 9 /* Currently voice traffic (AC_VO) always runs without aggregation, 10 * no special handling is needed. AC_BE/AC_BK use tids 0-3. Just check 11 * for non AC_BK/AC_BE and set smaller timeout for it. */ 12 return HZ / (tidno >= 4 ? 25 : 10); 13 } 14 15 static void 16 mt76_aggr_release(struct mt76_rx_tid *tid, struct sk_buff_head *frames, int idx) 17 { 18 struct sk_buff *skb; 19 20 tid->head = ieee80211_sn_inc(tid->head); 21 22 skb = tid->reorder_buf[idx]; 23 if (!skb) 24 return; 25 26 tid->reorder_buf[idx] = NULL; 27 tid->nframes--; 28 __skb_queue_tail(frames, skb); 29 } 30 31 static void 32 mt76_rx_aggr_release_frames(struct mt76_rx_tid *tid, 33 struct sk_buff_head *frames, 34 u16 head) 35 { 36 int idx; 37 38 while (ieee80211_sn_less(tid->head, head)) { 39 idx = tid->head % tid->size; 40 mt76_aggr_release(tid, frames, idx); 41 } 42 } 43 44 static void 45 mt76_rx_aggr_release_head(struct mt76_rx_tid *tid, struct sk_buff_head *frames) 46 { 47 int idx = tid->head % tid->size; 48 49 while (tid->reorder_buf[idx]) { 50 mt76_aggr_release(tid, frames, idx); 51 idx = tid->head % tid->size; 52 } 53 } 54 55 static void 56 mt76_rx_aggr_check_release(struct mt76_rx_tid *tid, struct sk_buff_head *frames) 57 { 58 struct mt76_rx_status *status; 59 struct sk_buff *skb; 60 int start, idx, nframes; 61 62 if (!tid->nframes) 63 return; 64 65 mt76_rx_aggr_release_head(tid, frames); 66 67 start = tid->head % tid->size; 68 nframes = tid->nframes; 69 70 for (idx = (tid->head + 1) % tid->size; 71 idx != start && nframes; 72 idx = (idx + 1) % tid->size) { 73 skb = tid->reorder_buf[idx]; 74 if (!skb) 75 continue; 76 77 nframes--; 78 status = (struct mt76_rx_status *)skb->cb; 79 if (!time_after(jiffies, 80 status->reorder_time + 81 mt76_aggr_tid_to_timeo(tid->num))) 82 continue; 83 84 mt76_rx_aggr_release_frames(tid, frames, status->seqno); 85 } 86 87 mt76_rx_aggr_release_head(tid, frames); 88 } 89 90 static void 91 mt76_rx_aggr_reorder_work(struct work_struct *work) 92 { 93 struct mt76_rx_tid *tid = container_of(work, struct mt76_rx_tid, 94 reorder_work.work); 95 struct mt76_dev *dev = tid->dev; 96 struct sk_buff_head frames; 97 int nframes; 98 99 __skb_queue_head_init(&frames); 100 101 local_bh_disable(); 102 rcu_read_lock(); 103 104 spin_lock(&tid->lock); 105 mt76_rx_aggr_check_release(tid, &frames); 106 nframes = tid->nframes; 107 spin_unlock(&tid->lock); 108 109 if (nframes) 110 ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work, 111 mt76_aggr_tid_to_timeo(tid->num)); 112 mt76_rx_complete(dev, &frames, NULL); 113 114 rcu_read_unlock(); 115 local_bh_enable(); 116 } 117 118 static void 119 mt76_rx_aggr_check_ctl(struct sk_buff *skb, struct sk_buff_head *frames) 120 { 121 struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb; 122 struct ieee80211_bar *bar = mt76_skb_get_hdr(skb); 123 struct mt76_wcid *wcid = status->wcid; 124 struct mt76_rx_tid *tid; 125 u16 seqno; 126 127 if (!ieee80211_is_ctl(bar->frame_control)) 128 return; 129 130 if (!ieee80211_is_back_req(bar->frame_control)) 131 return; 132 133 status->tid = le16_to_cpu(bar->control) >> 12; 134 seqno = IEEE80211_SEQ_TO_SN(le16_to_cpu(bar->start_seq_num)); 135 tid = rcu_dereference(wcid->aggr[status->tid]); 136 if (!tid) 137 return; 138 139 spin_lock_bh(&tid->lock); 140 if (!tid->stopped) { 141 mt76_rx_aggr_release_frames(tid, frames, seqno); 142 mt76_rx_aggr_release_head(tid, frames); 143 } 144 spin_unlock_bh(&tid->lock); 145 } 146 147 void mt76_rx_aggr_reorder(struct sk_buff *skb, struct sk_buff_head *frames) 148 { 149 struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb; 150 struct ieee80211_hdr *hdr = mt76_skb_get_hdr(skb); 151 struct mt76_wcid *wcid = status->wcid; 152 struct ieee80211_sta *sta; 153 struct mt76_rx_tid *tid; 154 bool sn_less; 155 u16 seqno, head, size, idx; 156 u8 ackp; 157 158 __skb_queue_tail(frames, skb); 159 160 sta = wcid_to_sta(wcid); 161 if (!sta) 162 return; 163 164 if (!status->aggr) { 165 mt76_rx_aggr_check_ctl(skb, frames); 166 return; 167 } 168 169 /* not part of a BA session */ 170 ackp = *ieee80211_get_qos_ctl(hdr) & IEEE80211_QOS_CTL_ACK_POLICY_MASK; 171 if (ackp != IEEE80211_QOS_CTL_ACK_POLICY_BLOCKACK && 172 ackp != IEEE80211_QOS_CTL_ACK_POLICY_NORMAL) 173 return; 174 175 tid = rcu_dereference(wcid->aggr[status->tid]); 176 if (!tid) 177 return; 178 179 status->flag |= RX_FLAG_DUP_VALIDATED; 180 spin_lock_bh(&tid->lock); 181 182 if (tid->stopped) 183 goto out; 184 185 head = tid->head; 186 seqno = status->seqno; 187 size = tid->size; 188 sn_less = ieee80211_sn_less(seqno, head); 189 190 if (!tid->started) { 191 if (sn_less) 192 goto out; 193 194 tid->started = true; 195 } 196 197 if (sn_less) { 198 __skb_unlink(skb, frames); 199 dev_kfree_skb(skb); 200 goto out; 201 } 202 203 if (seqno == head) { 204 tid->head = ieee80211_sn_inc(head); 205 if (tid->nframes) 206 mt76_rx_aggr_release_head(tid, frames); 207 goto out; 208 } 209 210 __skb_unlink(skb, frames); 211 212 /* 213 * Frame sequence number exceeds buffering window, free up some space 214 * by releasing previous frames 215 */ 216 if (!ieee80211_sn_less(seqno, head + size)) { 217 head = ieee80211_sn_inc(ieee80211_sn_sub(seqno, size)); 218 mt76_rx_aggr_release_frames(tid, frames, head); 219 } 220 221 idx = seqno % size; 222 223 /* Discard if the current slot is already in use */ 224 if (tid->reorder_buf[idx]) { 225 dev_kfree_skb(skb); 226 goto out; 227 } 228 229 status->reorder_time = jiffies; 230 tid->reorder_buf[idx] = skb; 231 tid->nframes++; 232 mt76_rx_aggr_release_head(tid, frames); 233 234 ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work, 235 mt76_aggr_tid_to_timeo(tid->num)); 236 237 out: 238 spin_unlock_bh(&tid->lock); 239 } 240 241 int mt76_rx_aggr_start(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno, 242 u16 ssn, u16 size) 243 { 244 struct mt76_rx_tid *tid; 245 246 mt76_rx_aggr_stop(dev, wcid, tidno); 247 248 tid = kzalloc(struct_size(tid, reorder_buf, size), GFP_KERNEL); 249 if (!tid) 250 return -ENOMEM; 251 252 tid->dev = dev; 253 tid->head = ssn; 254 tid->size = size; 255 tid->num = tidno; 256 INIT_DELAYED_WORK(&tid->reorder_work, mt76_rx_aggr_reorder_work); 257 spin_lock_init(&tid->lock); 258 259 rcu_assign_pointer(wcid->aggr[tidno], tid); 260 261 return 0; 262 } 263 EXPORT_SYMBOL_GPL(mt76_rx_aggr_start); 264 265 static void mt76_rx_aggr_shutdown(struct mt76_dev *dev, struct mt76_rx_tid *tid) 266 { 267 u16 size = tid->size; 268 int i; 269 270 spin_lock_bh(&tid->lock); 271 272 tid->stopped = true; 273 for (i = 0; tid->nframes && i < size; i++) { 274 struct sk_buff *skb = tid->reorder_buf[i]; 275 276 if (!skb) 277 continue; 278 279 tid->reorder_buf[i] = NULL; 280 tid->nframes--; 281 dev_kfree_skb(skb); 282 } 283 284 spin_unlock_bh(&tid->lock); 285 286 cancel_delayed_work_sync(&tid->reorder_work); 287 } 288 289 void mt76_rx_aggr_stop(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno) 290 { 291 struct mt76_rx_tid *tid = NULL; 292 293 tid = rcu_replace_pointer(wcid->aggr[tidno], tid, 294 lockdep_is_held(&dev->mutex)); 295 if (tid) { 296 mt76_rx_aggr_shutdown(dev, tid); 297 kfree_rcu(tid, rcu_head); 298 } 299 } 300 EXPORT_SYMBOL_GPL(mt76_rx_aggr_stop); 301