drivers/net/wireless/mediatek/mt76/sdio.c

   1 // SPDX-License-Identifier: ISC
   2 /* Copyright (C) 2020 MediaTek Inc.
   3  *
   4  * This file is written based on mt76/usb.c.
   5  *
   6  * Author: Felix Fietkau <[email protected]>
   7  *         Lorenzo Bianconi <[email protected]>
   8  *         Sean Wang <[email protected]>
   9  */
  10
  11 #include <linux/iopoll.h>
  12 #include <linux/kernel.h>
  13 #include <linux/module.h>
  14 #include <linux/mmc/sdio_func.h>
  15 #include <linux/sched.h>
  16 #include <linux/kthread.h>
  17
  18 #include "mt76.h"
  19
  20 static int
  21 mt76s_alloc_rx_queue(struct mt76_dev *dev, enum mt76_rxq_id qid)
  22 {
  23         struct mt76_queue *q = &dev->q_rx[qid];
  24
  25         spin_lock_init(&q->lock);
  26         q->entry = devm_kcalloc(dev->dev,
  27                                 MT_NUM_RX_ENTRIES, sizeof(*q->entry),
  28                                 GFP_KERNEL);
  29         if (!q->entry)
  30                 return -ENOMEM;
  31
  32         q->ndesc = MT_NUM_RX_ENTRIES;
  33         q->head = q->tail = 0;
  34         q->queued = 0;
  35
  36         return 0;
  37 }
  38
  39 static struct mt76_queue *mt76s_alloc_tx_queue(struct mt76_dev *dev)
  40 {
  41         struct mt76_queue *q;
  42
  43         q = devm_kzalloc(dev->dev, sizeof(*q), GFP_KERNEL);
  44         if (!q)
  45                 return ERR_PTR(-ENOMEM);
  46
  47         spin_lock_init(&q->lock);
  48         q->entry = devm_kcalloc(dev->dev,
  49                                 MT_NUM_TX_ENTRIES, sizeof(*q->entry),
  50                                 GFP_KERNEL);
  51         if (!q->entry)
  52                 return ERR_PTR(-ENOMEM);
  53
  54         q->ndesc = MT_NUM_TX_ENTRIES;
  55
  56         return q;
  57 }
  58
  59 static int mt76s_alloc_tx(struct mt76_dev *dev)
  60 {
  61         struct mt76_queue *q;
  62         int i;
  63
  64         for (i = 0; i <= MT_TXQ_PSD; i++) {
  65                 q = mt76s_alloc_tx_queue(dev);
  66                 if (IS_ERR(q))
  67                         return PTR_ERR(q);
  68
  69                 q->qid = i;
  70                 dev->phy.q_tx[i] = q;
  71         }
  72
  73         q = mt76s_alloc_tx_queue(dev);
  74         if (IS_ERR(q))
  75                 return PTR_ERR(q);
  76
  77         q->qid = MT_MCUQ_WM;
  78         dev->q_mcu[MT_MCUQ_WM] = q;
  79
  80         return 0;
  81 }
  82
  83 int mt76s_alloc_queues(struct mt76_dev *dev)
  84 {
  85         int err;
  86
  87         err = mt76s_alloc_rx_queue(dev, MT_RXQ_MAIN);
  88         if (err < 0)
  89                 return err;
  90
  91         return mt76s_alloc_tx(dev);
  92 }
  93 EXPORT_SYMBOL_GPL(mt76s_alloc_queues);
  94
  95 static struct mt76_queue_entry *
  96 mt76s_get_next_rx_entry(struct mt76_queue *q)
  97 {
  98         struct mt76_queue_entry *e = NULL;
  99
 100         spin_lock_bh(&q->lock);
 101         if (q->queued > 0) {
 102                 e = &q->entry[q->tail];
 103                 q->tail = (q->tail + 1) % q->ndesc;
 104                 q->queued--;
 105         }
 106         spin_unlock_bh(&q->lock);
 107
 108         return e;
 109 }
 110
 111 static int
 112 mt76s_process_rx_queue(struct mt76_dev *dev, struct mt76_queue *q)
 113 {
 114         int qid = q - &dev->q_rx[MT_RXQ_MAIN];
 115         int nframes = 0;
 116
 117         while (true) {
 118                 struct mt76_queue_entry *e;
 119
 120                 if (!test_bit(MT76_STATE_INITIALIZED, &dev->phy.state))
 121                         break;
 122
 123                 e = mt76s_get_next_rx_entry(q);
 124                 if (!e || !e->skb)
 125                         break;
 126
 127                 dev->drv->rx_skb(dev, MT_RXQ_MAIN, e->skb);
 128                 e->skb = NULL;
 129                 nframes++;
 130         }
 131         if (qid == MT_RXQ_MAIN)
 132                 mt76_rx_poll_complete(dev, MT_RXQ_MAIN, NULL);
 133
 134         return nframes;
 135 }
 136
 137 static void mt76s_net_worker(struct mt76_worker *w)
 138 {
 139         struct mt76_sdio *sdio = container_of(w, struct mt76_sdio,
 140                                               net_worker);
 141         struct mt76_dev *dev = container_of(sdio, struct mt76_dev, sdio);
 142         int i, nframes;
 143
 144         do {
 145                 nframes = 0;
 146
 147                 local_bh_disable();
 148                 rcu_read_lock();
 149
 150                 mt76_for_each_q_rx(dev, i)
 151                         nframes += mt76s_process_rx_queue(dev, &dev->q_rx[i]);
 152
 153                 rcu_read_unlock();
 154                 local_bh_enable();
 155         } while (nframes > 0);
 156 }
 157
 158 static int mt76s_process_tx_queue(struct mt76_dev *dev, struct mt76_queue *q)
 159 {
 160         struct mt76_queue_entry entry;
 161         int nframes = 0;
 162         bool mcu;
 163
 164         if (!q)
 165                 return 0;
 166
 167         mcu = q == dev->q_mcu[MT_MCUQ_WM];
 168         while (q->queued > 0) {
 169                 if (!q->entry[q->tail].done)
 170                         break;
 171
 172                 entry = q->entry[q->tail];
 173                 q->entry[q->tail].done = false;
 174
 175                 if (mcu) {
 176                         dev_kfree_skb(entry.skb);
 177                         entry.skb = NULL;
 178                 }
 179
 180                 mt76_queue_tx_complete(dev, q, &entry);
 181                 nframes++;
 182         }
 183
 184         if (!q->queued)
 185                 wake_up(&dev->tx_wait);
 186
 187         if (!mcu)
 188                 mt76_txq_schedule(&dev->phy, q->qid);
 189
 190         return nframes;
 191 }
 192
 193 static void mt76s_status_worker(struct mt76_worker *w)
 194 {
 195         struct mt76_sdio *sdio = container_of(w, struct mt76_sdio,
 196                                               status_worker);
 197         struct mt76_dev *dev = container_of(sdio, struct mt76_dev, sdio);
 198         int i, nframes;
 199
 200         do {
 201                 nframes = mt76s_process_tx_queue(dev, dev->q_mcu[MT_MCUQ_WM]);
 202
 203                 for (i = 0; i <= MT_TXQ_PSD; i++)
 204                         nframes += mt76s_process_tx_queue(dev,
 205                                                           dev->phy.q_tx[i]);
 206
 207                 if (dev->drv->tx_status_data &&
 208                     !test_and_set_bit(MT76_READING_STATS, &dev->phy.state))
 209                         queue_work(dev->wq, &dev->sdio.stat_work);
 210         } while (nframes > 0);
 211 }
 212
 213 static void mt76s_tx_status_data(struct work_struct *work)
 214 {
 215         struct mt76_sdio *sdio;
 216         struct mt76_dev *dev;
 217         u8 update = 1;
 218         u16 count = 0;
 219
 220         sdio = container_of(work, struct mt76_sdio, stat_work);
 221         dev = container_of(sdio, struct mt76_dev, sdio);
 222
 223         while (true) {
 224                 if (test_bit(MT76_REMOVED, &dev->phy.state))
 225                         break;
 226
 227                 if (!dev->drv->tx_status_data(dev, &update))
 228                         break;
 229                 count++;
 230         }
 231
 232         if (count && test_bit(MT76_STATE_RUNNING, &dev->phy.state))
 233                 queue_work(dev->wq, &sdio->stat_work);
 234         else
 235                 clear_bit(MT76_READING_STATS, &dev->phy.state);
 236 }
 237
 238 static int
 239 mt76s_tx_queue_skb(struct mt76_dev *dev, struct mt76_queue *q,
 240                    struct sk_buff *skb, struct mt76_wcid *wcid,
 241                    struct ieee80211_sta *sta)
 242 {
 243         struct mt76_tx_info tx_info = {
 244                 .skb = skb,
 245         };
 246         int err, len = skb->len;
 247         u16 idx = q->head;
 248
 249         if (q->queued == q->ndesc)
 250                 return -ENOSPC;
 251
 252         skb->prev = skb->next = NULL;
 253         err = dev->drv->tx_prepare_skb(dev, NULL, q->qid, wcid, sta, &tx_info);
 254         if (err < 0)
 255                 return err;
 256
 257         q->entry[q->head].skb = tx_info.skb;
 258         q->entry[q->head].buf_sz = len;
 259
 260         smp_wmb();
 261
 262         q->head = (q->head + 1) % q->ndesc;
 263         q->queued++;
 264
 265         return idx;
 266 }
 267
 268 static int
 269 mt76s_tx_queue_skb_raw(struct mt76_dev *dev, struct mt76_queue *q,
 270                        struct sk_buff *skb, u32 tx_info)
 271 {
 272         int ret = -ENOSPC, len = skb->len, pad;
 273
 274         if (q->queued == q->ndesc)
 275                 goto error;
 276
 277         pad = round_up(skb->len, 4) - skb->len;
 278         ret = mt76_skb_adjust_pad(skb, pad);
 279         if (ret)
 280                 goto error;
 281
 282         spin_lock_bh(&q->lock);
 283
 284         q->entry[q->head].buf_sz = len;
 285         q->entry[q->head].skb = skb;
 286         q->head = (q->head + 1) % q->ndesc;
 287         q->queued++;
 288
 289         spin_unlock_bh(&q->lock);
 290
 291         return 0;
 292
 293 error:
 294         dev_kfree_skb(skb);
 295
 296         return ret;
 297 }
 298
 299 static void mt76s_tx_kick(struct mt76_dev *dev, struct mt76_queue *q)
 300 {
 301         struct mt76_sdio *sdio = &dev->sdio;
 302
 303         mt76_worker_schedule(&sdio->txrx_worker);
 304 }
 305
 306 static const struct mt76_queue_ops sdio_queue_ops = {
 307         .tx_queue_skb = mt76s_tx_queue_skb,
 308         .kick = mt76s_tx_kick,
 309         .tx_queue_skb_raw = mt76s_tx_queue_skb_raw,
 310 };
 311
 312 void mt76s_deinit(struct mt76_dev *dev)
 313 {
 314         struct mt76_sdio *sdio = &dev->sdio;
 315         int i;
 316
 317         mt76_worker_teardown(&sdio->txrx_worker);
 318         mt76_worker_teardown(&sdio->status_worker);
 319         mt76_worker_teardown(&sdio->net_worker);
 320
 321         cancel_work_sync(&sdio->stat_work);
 322         clear_bit(MT76_READING_STATS, &dev->phy.state);
 323
 324         mt76_tx_status_check(dev, NULL, true);
 325
 326         sdio_claim_host(sdio->func);
 327         sdio_release_irq(sdio->func);
 328         sdio_release_host(sdio->func);
 329
 330         mt76_for_each_q_rx(dev, i) {
 331                 struct mt76_queue *q = &dev->q_rx[i];
 332                 int j;
 333
 334                 for (j = 0; j < q->ndesc; j++) {
 335                         struct mt76_queue_entry *e = &q->entry[j];
 336
 337                         if (!e->skb)
 338                                 continue;
 339
 340                         dev_kfree_skb(e->skb);
 341                         e->skb = NULL;
 342                 }
 343         }
 344 }
 345 EXPORT_SYMBOL_GPL(mt76s_deinit);
 346
 347 int mt76s_init(struct mt76_dev *dev, struct sdio_func *func,
 348                const struct mt76_bus_ops *bus_ops)
 349 {
 350         struct mt76_sdio *sdio = &dev->sdio;
 351         int err;
 352
 353         err = mt76_worker_setup(dev->hw, &sdio->status_worker,
 354                                 mt76s_status_worker, "sdio-status");
 355         if (err)
 356                 return err;
 357
 358         err = mt76_worker_setup(dev->hw, &sdio->net_worker, mt76s_net_worker,
 359                                 "sdio-net");
 360         if (err)
 361                 return err;
 362
 363         sched_set_fifo_low(sdio->status_worker.task);
 364         sched_set_fifo_low(sdio->net_worker.task);
 365
 366         INIT_WORK(&sdio->stat_work, mt76s_tx_status_data);
 367
 368         dev->queue_ops = &sdio_queue_ops;
 369         dev->bus = bus_ops;
 370         dev->sdio.func = func;
 371
 372         return 0;
 373 }
 374 EXPORT_SYMBOL_GPL(mt76s_init);
 375
 376 MODULE_AUTHOR("Sean Wang <[email protected]>");
 377 MODULE_AUTHOR("Lorenzo Bianconi <[email protected]>");
 378 MODULE_LICENSE("Dual BSD/GPL");