1// SPDX-License-Identifier: ISC
2/*
3 * Copyright (C) 2018 Felix Fietkau <nbd@nbd.name>
4 */
5#include "mt76.h"
6
7static 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
15static void
16mt76_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
31static void
32mt76_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
44static void
45mt76_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
55static void
56mt76_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
90static void
91mt76_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
118static void
119mt76_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
147void 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
237out:
238	spin_unlock_bh(&tid->lock);
239}
240
241int 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}
263EXPORT_SYMBOL_GPL(mt76_rx_aggr_start);
264
265static 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
289void 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}
300EXPORT_SYMBOL_GPL(mt76_rx_aggr_stop);
301