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