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 	while (seq_less(r->head_seq_num, hseq)) {
53 		index = reorder_index(r, r->head_seq_num);
54 		wil_release_reorder_frame(wil, r, index);
55 	}
56 }
57 
58 static void wil_reorder_release(struct wil6210_priv *wil,
59 				struct wil_tid_ampdu_rx *r)
60 {
61 	int index = reorder_index(r, r->head_seq_num);
62 
63 	while (r->reorder_buf[index]) {
64 		wil_release_reorder_frame(wil, r, index);
65 		index = reorder_index(r, r->head_seq_num);
66 	}
67 }
68 
69 void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
70 {
71 	struct net_device *ndev = wil_to_ndev(wil);
72 	struct vring_rx_desc *d = wil_skb_rxdesc(skb);
73 	int tid = wil_rxdesc_tid(d);
74 	int cid = wil_rxdesc_cid(d);
75 	int mid = wil_rxdesc_mid(d);
76 	u16 seq = wil_rxdesc_seq(d);
77 	struct wil_sta_info *sta = &wil->sta[cid];
78 	struct wil_tid_ampdu_rx *r = sta->tid_rx[tid];
79 	u16 hseq;
80 	int index;
81 
82 	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x\n",
83 		     mid, cid, tid, seq);
84 
85 	if (!r) {
86 		wil_netif_rx_any(skb, ndev);
87 		return;
88 	}
89 
90 	hseq = r->head_seq_num;
91 
92 	spin_lock(&r->reorder_lock);
93 
94 	/* frame with out of date sequence number */
95 	if (seq_less(seq, r->head_seq_num)) {
96 		dev_kfree_skb(skb);
97 		goto out;
98 	}
99 
100 	/*
101 	 * If frame the sequence number exceeds our buffering window
102 	 * size release some previous frames to make room for this one.
103 	 */
104 	if (!seq_less(seq, r->head_seq_num + r->buf_size)) {
105 		hseq = seq_inc(seq_sub(seq, r->buf_size));
106 		/* release stored frames up to new head to stack */
107 		wil_release_reorder_frames(wil, r, hseq);
108 	}
109 
110 	/* Now the new frame is always in the range of the reordering buffer */
111 
112 	index = reorder_index(r, seq);
113 
114 	/* check if we already stored this frame */
115 	if (r->reorder_buf[index]) {
116 		dev_kfree_skb(skb);
117 		goto out;
118 	}
119 
120 	/*
121 	 * If the current MPDU is in the right order and nothing else
122 	 * is stored we can process it directly, no need to buffer it.
123 	 * If it is first but there's something stored, we may be able
124 	 * to release frames after this one.
125 	 */
126 	if (seq == r->head_seq_num && r->stored_mpdu_num == 0) {
127 		r->head_seq_num = seq_inc(r->head_seq_num);
128 		wil_netif_rx_any(skb, ndev);
129 		goto out;
130 	}
131 
132 	/* put the frame in the reordering buffer */
133 	r->reorder_buf[index] = skb;
134 	r->reorder_time[index] = jiffies;
135 	r->stored_mpdu_num++;
136 	wil_reorder_release(wil, r);
137 
138 out:
139 	spin_unlock(&r->reorder_lock);
140 }
141 
142 struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
143 						int size, u16 ssn)
144 {
145 	struct wil_tid_ampdu_rx *r = kzalloc(sizeof(*r), GFP_KERNEL);
146 	if (!r)
147 		return NULL;
148 
149 	r->reorder_buf =
150 		kcalloc(size, sizeof(struct sk_buff *), GFP_KERNEL);
151 	r->reorder_time =
152 		kcalloc(size, sizeof(unsigned long), GFP_KERNEL);
153 	if (!r->reorder_buf || !r->reorder_time) {
154 		kfree(r->reorder_buf);
155 		kfree(r->reorder_time);
156 		kfree(r);
157 		return NULL;
158 	}
159 
160 	spin_lock_init(&r->reorder_lock);
161 	r->ssn = ssn;
162 	r->head_seq_num = ssn;
163 	r->buf_size = size;
164 	r->stored_mpdu_num = 0;
165 	return r;
166 }
167 
168 void wil_tid_ampdu_rx_free(struct wil6210_priv *wil,
169 			   struct wil_tid_ampdu_rx *r)
170 {
171 	if (!r)
172 		return;
173 	wil_release_reorder_frames(wil, r, r->head_seq_num + r->buf_size);
174 	kfree(r->reorder_buf);
175 	kfree(r->reorder_time);
176 	kfree(r);
177 }
178