drivers/net/wireless/mediatek/mt76-6e-usb/agg-rx.c

   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) {
 166                 if (!(status->flag & RX_FLAG_8023))
 167                         mt76_rx_aggr_check_ctl(skb, frames);
 168                 return;
 169         }
 170
 171         /* not part of a BA session */
 172         ackp = status->qos_ctl & IEEE80211_QOS_CTL_ACK_POLICY_MASK;
 173         if (ackp != IEEE80211_QOS_CTL_ACK_POLICY_BLOCKACK &&
 174             ackp != IEEE80211_QOS_CTL_ACK_POLICY_NORMAL)
 175                 return;
 176
 177         tid = rcu_dereference(wcid->aggr[tidno]);
 178         if (!tid)
 179                 return;
 180
 181         status->flag |= RX_FLAG_DUP_VALIDATED;
 182         spin_lock_bh(&tid->lock);
 183
 184         if (tid->stopped)
 185                 goto out;
 186
 187         head = tid->head;
 188         seqno = status->seqno;
 189         size = tid->size;
 190         sn_less = ieee80211_sn_less(seqno, head);
 191
 192         if (!tid->started) {
 193                 if (sn_less)
 194                         goto out;
 195
 196                 tid->started = true;
 197         }
 198
 199         if (sn_less) {
 200                 __skb_unlink(skb, frames);
 201                 dev_kfree_skb(skb);
 202                 goto out;
 203         }
 204
 205         if (seqno == head) {
 206                 tid->head = ieee80211_sn_inc(head);
 207                 if (tid->nframes)
 208                         mt76_rx_aggr_release_head(tid, frames);
 209                 goto out;
 210         }
 211
 212         __skb_unlink(skb, frames);
 213
 214         /*
 215          * Frame sequence number exceeds buffering window, free up some space
 216          * by releasing previous frames
 217          */
 218         if (!ieee80211_sn_less(seqno, head + size)) {
 219                 head = ieee80211_sn_inc(ieee80211_sn_sub(seqno, size));
 220                 mt76_rx_aggr_release_frames(tid, frames, head);
 221         }
 222
 223         idx = seqno % size;
 224
 225         /* Discard if the current slot is already in use */
 226         if (tid->reorder_buf[idx]) {
 227                 dev_kfree_skb(skb);
 228                 goto out;
 229         }
 230
 231         status->reorder_time = jiffies;
 232         tid->reorder_buf[idx] = skb;
 233         tid->nframes++;
 234         mt76_rx_aggr_release_head(tid, frames);
 235
 236         ieee80211_queue_delayed_work(tid->dev->hw, &tid->reorder_work,
 237                                      mt76_aggr_tid_to_timeo(tid->num));
 238
 239 out:
 240         spin_unlock_bh(&tid->lock);
 241 }
 242
 243 int mt76_rx_aggr_start(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno,
 244                        u16 ssn, u16 size)
 245 {
 246         struct mt76_rx_tid *tid;
 247
 248         mt76_rx_aggr_stop(dev, wcid, tidno);
 249
 250         tid = kzalloc(struct_size(tid, reorder_buf, size), GFP_KERNEL);
 251         if (!tid)
 252                 return -ENOMEM;
 253
 254         tid->dev = dev;
 255         tid->head = ssn;
 256         tid->size = size;
 257         tid->num = tidno;
 258         INIT_DELAYED_WORK(&tid->reorder_work, mt76_rx_aggr_reorder_work);
 259         spin_lock_init(&tid->lock);
 260
 261         rcu_assign_pointer(wcid->aggr[tidno], tid);
 262
 263         return 0;
 264 }
 265 EXPORT_SYMBOL_GPL(mt76_rx_aggr_start);
 266
 267 static void mt76_rx_aggr_shutdown(struct mt76_dev *dev, struct mt76_rx_tid *tid)
 268 {
 269         u16 size = tid->size;
 270         int i;
 271
 272         spin_lock_bh(&tid->lock);
 273
 274         tid->stopped = true;
 275         for (i = 0; tid->nframes && i < size; i++) {
 276                 struct sk_buff *skb = tid->reorder_buf[i];
 277
 278                 if (!skb)
 279                         continue;
 280
 281                 tid->reorder_buf[i] = NULL;
 282                 tid->nframes--;
 283                 dev_kfree_skb(skb);
 284         }
 285
 286         spin_unlock_bh(&tid->lock);
 287
 288         cancel_delayed_work_sync(&tid->reorder_work);
 289 }
 290
 291 void mt76_rx_aggr_stop(struct mt76_dev *dev, struct mt76_wcid *wcid, u8 tidno)
 292 {
 293         struct mt76_rx_tid *tid = NULL;
 294
 295         tid = rcu_replace_pointer(wcid->aggr[tidno], tid,
 296                                   lockdep_is_held(&dev->mutex));
 297         if (tid) {
 298                 mt76_rx_aggr_shutdown(dev, tid);
 299                 kfree_rcu(tid, rcu_head);
 300         }
 301 }
 302 EXPORT_SYMBOL_GPL(mt76_rx_aggr_stop);