developer | b11a539 | 2022-03-31 00:34:47 +0800 | [diff] [blame] | 1 | // SPDX-License-Identifier: ISC |
| 2 | /* |
| 3 | * Copyright (C) 2018 Felix Fietkau <nbd@nbd.name> |
| 4 | */ |
| 5 | #include "mt76.h" |
| 6 | |
| 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 |
| 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 |
| 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 |
| 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 |
| 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 |
| 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 |
| 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 | |
| 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 && !(status->flag & RX_FLAG_8023)) { |
| 166 | mt76_rx_aggr_check_ctl(skb, frames); |
| 167 | return; |
| 168 | } |
| 169 | |
| 170 | /* not part of a BA session */ |
| 171 | ackp = status->qos_ctl & IEEE80211_QOS_CTL_ACK_POLICY_MASK; |
| 172 | if (ackp != IEEE80211_QOS_CTL_ACK_POLICY_BLOCKACK && |
| 173 | ackp != IEEE80211_QOS_CTL_ACK_POLICY_NORMAL) |
| 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 | |
| 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 | |
| 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 | |
| 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); |