1 #include "wil6210.h"
2 #include "txrx.h"
3 
4 #define SEQ_MODULO 0x1000
5 #define SEQ_MASK   0xfff
6 
7 static inline int seq_less(u16 sq1, u16 sq2)
8 {
9 	return ((sq1 - sq2) & SEQ_MASK) > (SEQ_MODULO >> 1);
10 }
11 
12 static inline u16 seq_inc(u16 sq)
13 {
14 	return (sq + 1) & SEQ_MASK;
15 }
16 
17 static inline u16 seq_sub(u16 sq1, u16 sq2)
18 {
19 	return (sq1 - sq2) & SEQ_MASK;
20 }
21 
22 static inline int reorder_index(struct wil_tid_ampdu_rx *r, u16 seq)
23 {
24 	return seq_sub(seq, r->ssn) % r->buf_size;
25 }
26 
27 static void wil_release_reorder_frame(struct wil6210_priv *wil,
28 				      struct wil_tid_ampdu_rx *r,
29 				      int index)
30 {
31 	struct net_device *ndev = wil_to_ndev(wil);
32 	struct sk_buff *skb = r->reorder_buf[index];
33 
34 	if (!skb)
35 		goto no_frame;
36 
37 	/* release the frame from the reorder ring buffer */
38 	r->stored_mpdu_num--;
39 	r->reorder_buf[index] = NULL;
40 	wil_netif_rx_any(skb, ndev);
41 
42 no_frame:
43 	r->head_seq_num = seq_inc(r->head_seq_num);
44 }
45 
46 static void wil_release_reorder_frames(struct wil6210_priv *wil,
47 				       struct wil_tid_ampdu_rx *r,
48 				       u16 hseq)
49 {
50 	int index;
51 
52 	/* note: this function is never called with
53 	 * hseq preceding r->head_seq_num, i.e it is always true
54 	 * !seq_less(hseq, r->head_seq_num)
55 	 * and thus on loop exit it should be
56 	 * r->head_seq_num == hseq
57 	 */
58 	while (seq_less(r->head_seq_num, hseq) && r->stored_mpdu_num) {
59 		index = reorder_index(r, r->head_seq_num);
60 		wil_release_reorder_frame(wil, r, index);
61 	}
62 	r->head_seq_num = hseq;
63 }
64 
65 static void wil_reorder_release(struct wil6210_priv *wil,
66 				struct wil_tid_ampdu_rx *r)
67 {
68 	int index = reorder_index(r, r->head_seq_num);
69 
70 	while (r->reorder_buf[index]) {
71 		wil_release_reorder_frame(wil, r, index);
72 		index = reorder_index(r, r->head_seq_num);
73 	}
74 }
75 
76 void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
77 {
78 	struct net_device *ndev = wil_to_ndev(wil);
79 	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
80 	int tid = wil_rxdesc_tid(d);
81 	int cid = wil_rxdesc_cid(d);
82 	int mid = wil_rxdesc_mid(d);
83 	u16 seq = wil_rxdesc_seq(d);
84 	struct wil_sta_info *sta = &wil->sta[cid];
85 	struct wil_tid_ampdu_rx *r = sta->tid_rx[tid];
86 	u16 hseq;
87 	int index;
88 
89 	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x\n",
90 		     mid, cid, tid, seq);
91 
92 	if (!r) {
93 		wil_netif_rx_any(skb, ndev);
94 		return;
95 	}
96 
97 	hseq = r->head_seq_num;
98 
99 	spin_lock(&r->reorder_lock);
100 
101 	/** Due to the race between WMI events, where BACK establishment
102 	 * reported, and data Rx, few packets may be pass up before reorder
103 	 * buffer get allocated. Catch up by pretending SSN is what we
104 	 * see in the 1-st Rx packet
105 	 */
106 	if (r->first_time) {
107 		r->first_time = false;
108 		if (seq != r->head_seq_num) {
109 			wil_err(wil, "Error: 1-st frame with wrong sequence"
110 				" %d, should be %d. Fixing...\n", seq,
111 				r->head_seq_num);
112 			r->head_seq_num = seq;
113 			r->ssn = seq;
114 		}
115 	}
116 
117 	/* frame with out of date sequence number */
118 	if (seq_less(seq, r->head_seq_num)) {
119 		dev_kfree_skb(skb);
120 		goto out;
121 	}
122 
123 	/*
124 	 * If frame the sequence number exceeds our buffering window
125 	 * size release some previous frames to make room for this one.
126 	 */
127 	if (!seq_less(seq, r->head_seq_num + r->buf_size)) {
128 		hseq = seq_inc(seq_sub(seq, r->buf_size));
129 		/* release stored frames up to new head to stack */
130 		wil_release_reorder_frames(wil, r, hseq);
131 	}
132 
133 	/* Now the new frame is always in the range of the reordering buffer */
134 
135 	index = reorder_index(r, seq);
136 
137 	/* check if we already stored this frame */
138 	if (r->reorder_buf[index]) {
139 		dev_kfree_skb(skb);
140 		goto out;
141 	}
142 
143 	/*
144 	 * If the current MPDU is in the right order and nothing else
145 	 * is stored we can process it directly, no need to buffer it.
146 	 * If it is first but there's something stored, we may be able
147 	 * to release frames after this one.
148 	 */
149 	if (seq == r->head_seq_num && r->stored_mpdu_num == 0) {
150 		r->head_seq_num = seq_inc(r->head_seq_num);
151 		wil_netif_rx_any(skb, ndev);
152 		goto out;
153 	}
154 
155 	/* put the frame in the reordering buffer */
156 	r->reorder_buf[index] = skb;
157 	r->reorder_time[index] = jiffies;
158 	r->stored_mpdu_num++;
159 	wil_reorder_release(wil, r);
160 
161 out:
162 	spin_unlock(&r->reorder_lock);
163 }
164 
165 struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
166 						int size, u16 ssn)
167 {
168 	struct wil_tid_ampdu_rx *r = kzalloc(sizeof(*r), GFP_KERNEL);
169 	if (!r)
170 		return NULL;
171 
172 	r->reorder_buf =
173 		kcalloc(size, sizeof(struct sk_buff *), GFP_KERNEL);
174 	r->reorder_time =
175 		kcalloc(size, sizeof(unsigned long), GFP_KERNEL);
176 	if (!r->reorder_buf || !r->reorder_time) {
177 		kfree(r->reorder_buf);
178 		kfree(r->reorder_time);
179 		kfree(r);
180 		return NULL;
181 	}
182 
183 	spin_lock_init(&r->reorder_lock);
184 	r->ssn = ssn;
185 	r->head_seq_num = ssn;
186 	r->buf_size = size;
187 	r->stored_mpdu_num = 0;
188 	r->first_time = true;
189 	return r;
190 }
191 
192 void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
193 			   struct wil_tid_ampdu_rx *r)
194 {
195 	if (!r)
196 		return;
197 	wil_release_reorder_frames(wil, r, r->head_seq_num + r->buf_size);
198 	kfree(r->reorder_buf);
199 	kfree(r->reorder_time);
200 	kfree(r);
201 }
202