1 // SPDX-License-Identifier: ISC
2 /*
3  * Copyright (C) 2018 Felix Fietkau <nbd@nbd.name>
4  */
5 #include "mt76.h"
6 
mt76_aggr_tid_to_timeo(u8 tidno)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
mt76_aggr_release(struct mt76_rx_tid * tid,struct sk_buff_head * frames,int idx)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
mt76_rx_aggr_release_frames(struct mt76_rx_tid * tid,struct sk_buff_head * frames,u16 head)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
mt76_rx_aggr_release_head(struct mt76_rx_tid * tid,struct sk_buff_head * frames)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
mt76_rx_aggr_check_release(struct mt76_rx_tid * tid,struct sk_buff_head * frames)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_after32(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
mt76_rx_aggr_reorder_work(struct work_struct * work)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
mt76_rx_aggr_check_ctl(struct sk_buff * skb,struct sk_buff_head * frames)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 	u8 tidno = status->qos_ctl & IEEE80211_QOS_CTL_TID_MASK;
126 	u16 seqno;
127 
128 	if (!ieee80211_is_ctl(bar->frame_control))
129 		return;
130 
131 	if (!ieee80211_is_back_req(bar->frame_control))
132 		return;
133 
134 	status->qos_ctl = tidno = le16_to_cpu(bar->control) >> 12;
135 	seqno = IEEE80211_SEQ_TO_SN(le16_to_cpu(bar->start_seq_num));
136 	tid = rcu_dereference(wcid->aggr[tidno]);
137 	if (!tid)
138 		return;
139 
140 	spin_lock_bh(&tid->lock);
141 	if (!tid->stopped) {
142 		mt76_rx_aggr_release_frames(tid, frames, seqno);
143 		mt76_rx_aggr_release_head(tid, frames);
144 	}
145 	spin_unlock_bh(&tid->lock);
146 }
147 
mt76_rx_aggr_reorder(struct sk_buff * skb,struct sk_buff_head * frames)148 void mt76_rx_aggr_reorder(struct sk_buff *skb, struct sk_buff_head *frames)
149 {
150 	struct mt76_rx_status *status = (struct mt76_rx_status *)skb->cb;
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 tidno = status->qos_ctl & IEEE80211_QOS_CTL_TID_MASK;
157 	u8 ackp;
158 
159 	__skb_queue_tail(frames, skb);
160 
161 	sta = wcid_to_sta(wcid);
162 	if (!sta)
163 		return;
164 
165 	if (!status->aggr) {
166 		if (!(status->flag & RX_FLAG_8023))
167 			mt76_rx_aggr_check_ctl(skb, frames);
168 		return;
169 	}
170 
171 	/* not part of a BA session */
172 	ackp = status->qos_ctl & IEEE80211_QOS_CTL_ACK_POLICY_MASK;
173 	if (ackp == IEEE80211_QOS_CTL_ACK_POLICY_NOACK)
174 		return;
175 
176 	tid = rcu_dereference(wcid->aggr[tidno]);
177 	if (!tid)
178 		return;
179 
180 	status->flag |= RX_FLAG_DUP_VALIDATED;
181 	spin_lock_bh(&tid->lock);
182 
183 	if (tid->stopped)
184 		goto out;
185 
186 	head = tid->head;
187 	seqno = status->seqno;
188 	size = tid->size;
189 	sn_less = ieee80211_sn_less(seqno, head);
190 
191 	if (!tid->started) {
192 		if (sn_less)
193 			goto out;
194 
195 		tid->started = true;
196 	}
197 
198 	if (sn_less) {
199 		__skb_unlink(skb, frames);
200 		dev_kfree_skb(skb);
201 		goto out;
202 	}
203 
204 	if (seqno == head) {
205 		tid->head = ieee80211_sn_inc(head);
206 		if (tid->nframes)
207 			mt76_rx_aggr_release_head(tid, frames);
208 		goto out;
209 	}
210 
211 	__skb_unlink(skb, frames);
212 
213 	/*
214 	 * Frame sequence number exceeds buffering window, free up some space
215 	 * by releasing previous frames
216 	 */
217 	if (!ieee80211_sn_less(seqno, head + size)) {
218 		head = ieee80211_sn_inc(ieee80211_sn_sub(seqno, size));
219 		mt76_rx_aggr_release_frames(tid, frames, head);
220 	}
221 
222 	idx = seqno % size;
223 
224 	/* Discard if the current slot is already in use */
225 	if (tid->reorder_buf[idx]) {
226 		dev_kfree_skb(skb);
227 		goto out;
228 	}
229 
230 	status->reorder_time = jiffies;
231 	tid->reorder_buf[idx] = skb;
232 	tid->nframes++;
233 	mt76_rx_aggr_release_head(tid, frames);
234 
235 	ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work,
236 				     mt76_aggr_tid_to_timeo(tid->num));
237 
238 out:
239 	spin_unlock_bh(&tid->lock);
240 }
241 
mt76_rx_aggr_start(struct mt76_dev * dev,struct mt76_wcid * wcid,u8 tidno,u16 ssn,u16 size)242 int mt76_rx_aggr_start(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno,
243 		       u16 ssn, u16 size)
244 {
245 	struct mt76_rx_tid *tid;
246 
247 	mt76_rx_aggr_stop(dev, wcid, tidno);
248 
249 	tid = kzalloc(struct_size(tid, reorder_buf, size), GFP_KERNEL);
250 	if (!tid)
251 		return -ENOMEM;
252 
253 	tid->dev = dev;
254 	tid->head = ssn;
255 	tid->size = size;
256 	tid->num = tidno;
257 	INIT_DELAYED_WORK(&tid->reorder_work, mt76_rx_aggr_reorder_work);
258 	spin_lock_init(&tid->lock);
259 
260 	rcu_assign_pointer(wcid->aggr[tidno], tid);
261 
262 	return 0;
263 }
264 EXPORT_SYMBOL_GPL(mt76_rx_aggr_start);
265 
mt76_rx_aggr_shutdown(struct mt76_dev * dev,struct mt76_rx_tid * tid)266 static void mt76_rx_aggr_shutdown(struct mt76_dev *dev, struct mt76_rx_tid *tid)
267 {
268 	u16 size = tid->size;
269 	int i;
270 
271 	spin_lock_bh(&tid->lock);
272 
273 	tid->stopped = true;
274 	for (i = 0; tid->nframes && i < size; i++) {
275 		struct sk_buff *skb = tid->reorder_buf[i];
276 
277 		if (!skb)
278 			continue;
279 
280 		tid->reorder_buf[i] = NULL;
281 		tid->nframes--;
282 		dev_kfree_skb(skb);
283 	}
284 
285 	spin_unlock_bh(&tid->lock);
286 
287 	cancel_delayed_work_sync(&tid->reorder_work);
288 }
289 
mt76_rx_aggr_stop(struct mt76_dev * dev,struct mt76_wcid * wcid,u8 tidno)290 void mt76_rx_aggr_stop(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno)
291 {
292 	struct mt76_rx_tid *tid = NULL;
293 
294 	tid = rcu_replace_pointer(wcid->aggr[tidno], tid,
295 				  lockdep_is_held(&dev->mutex));
296 	if (tid) {
297 		mt76_rx_aggr_shutdown(dev, tid);
298 		kfree_rcu(tid, rcu_head);
299 	}
300 }
301 EXPORT_SYMBOL_GPL(mt76_rx_aggr_stop);
302