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