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 <nbd@nbd.name> 7 * Lorenzo Bianconi <lorenzo@kernel.org> 8 * Sean Wang <sean.wang@mediatek.com> 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 bool wake, mcu = q == dev->q_mcu[MT_MCUQ_WM]; 161 struct mt76_queue_entry entry; 162 int nframes = 0; 163 164 while (q->queued > 0) { 165 if (!q->entry[q->tail].done) 166 break; 167 168 entry = q->entry[q->tail]; 169 q->entry[q->tail].done = false; 170 171 if (mcu) { 172 dev_kfree_skb(entry.skb); 173 entry.skb = NULL; 174 } 175 176 mt76_queue_tx_complete(dev, q, &entry); 177 nframes++; 178 } 179 180 wake = q->stopped && q->queued < q->ndesc - 8; 181 if (wake) 182 q->stopped = false; 183 184 if (!q->queued) 185 wake_up(&dev->tx_wait); 186 187 if (mcu) 188 goto out; 189 190 mt76_txq_schedule(&dev->phy, q->qid); 191 192 if (wake) 193 ieee80211_wake_queue(dev->hw, q->qid); 194 out: 195 return nframes; 196 } 197 198 static void mt76s_status_worker(struct mt76_worker *w) 199 { 200 struct mt76_sdio *sdio = container_of(w, struct mt76_sdio, 201 status_worker); 202 struct mt76_dev *dev = container_of(sdio, struct mt76_dev, sdio); 203 int i, nframes; 204 205 do { 206 nframes = mt76s_process_tx_queue(dev, dev->q_mcu[MT_MCUQ_WM]); 207 208 for (i = 0; i <= MT_TXQ_PSD; i++) 209 nframes += mt76s_process_tx_queue(dev, 210 dev->phy.q_tx[i]); 211 212 if (dev->drv->tx_status_data && 213 !test_and_set_bit(MT76_READING_STATS, &dev->phy.state)) 214 queue_work(dev->wq, &dev->sdio.stat_work); 215 } while (nframes > 0); 216 } 217 218 static void mt76s_tx_status_data(struct work_struct *work) 219 { 220 struct mt76_sdio *sdio; 221 struct mt76_dev *dev; 222 u8 update = 1; 223 u16 count = 0; 224 225 sdio = container_of(work, struct mt76_sdio, stat_work); 226 dev = container_of(sdio, struct mt76_dev, sdio); 227 228 while (true) { 229 if (test_bit(MT76_REMOVED, &dev->phy.state)) 230 break; 231 232 if (!dev->drv->tx_status_data(dev, &update)) 233 break; 234 count++; 235 } 236 237 if (count && test_bit(MT76_STATE_RUNNING, &dev->phy.state)) 238 queue_work(dev->wq, &sdio->stat_work); 239 else 240 clear_bit(MT76_READING_STATS, &dev->phy.state); 241 } 242 243 static int 244 mt76s_tx_queue_skb(struct mt76_dev *dev, struct mt76_queue *q, 245 struct sk_buff *skb, struct mt76_wcid *wcid, 246 struct ieee80211_sta *sta) 247 { 248 struct mt76_tx_info tx_info = { 249 .skb = skb, 250 }; 251 int err, len = skb->len; 252 u16 idx = q->head; 253 254 if (q->queued == q->ndesc) 255 return -ENOSPC; 256 257 skb->prev = skb->next = NULL; 258 err = dev->drv->tx_prepare_skb(dev, NULL, q->qid, wcid, sta, &tx_info); 259 if (err < 0) 260 return err; 261 262 q->entry[q->head].skb = tx_info.skb; 263 q->entry[q->head].buf_sz = len; 264 q->head = (q->head + 1) % q->ndesc; 265 q->queued++; 266 267 return idx; 268 } 269 270 static int 271 mt76s_tx_queue_skb_raw(struct mt76_dev *dev, struct mt76_queue *q, 272 struct sk_buff *skb, u32 tx_info) 273 { 274 int ret = -ENOSPC, len = skb->len, pad; 275 276 if (q->queued == q->ndesc) 277 goto error; 278 279 pad = round_up(skb->len, 4) - skb->len; 280 ret = mt76_skb_adjust_pad(skb, pad); 281 if (ret) 282 goto error; 283 284 spin_lock_bh(&q->lock); 285 286 q->entry[q->head].buf_sz = len; 287 q->entry[q->head].skb = skb; 288 q->head = (q->head + 1) % q->ndesc; 289 q->queued++; 290 291 spin_unlock_bh(&q->lock); 292 293 return 0; 294 295 error: 296 dev_kfree_skb(skb); 297 298 return ret; 299 } 300 301 static void mt76s_tx_kick(struct mt76_dev *dev, struct mt76_queue *q) 302 { 303 struct mt76_sdio *sdio = &dev->sdio; 304 305 mt76_worker_schedule(&sdio->txrx_worker); 306 } 307 308 static const struct mt76_queue_ops sdio_queue_ops = { 309 .tx_queue_skb = mt76s_tx_queue_skb, 310 .kick = mt76s_tx_kick, 311 .tx_queue_skb_raw = mt76s_tx_queue_skb_raw, 312 }; 313 314 void mt76s_deinit(struct mt76_dev *dev) 315 { 316 struct mt76_sdio *sdio = &dev->sdio; 317 int i; 318 319 mt76_worker_teardown(&sdio->txrx_worker); 320 mt76_worker_teardown(&sdio->status_worker); 321 mt76_worker_teardown(&sdio->net_worker); 322 323 cancel_work_sync(&sdio->stat_work); 324 clear_bit(MT76_READING_STATS, &dev->phy.state); 325 326 mt76_tx_status_check(dev, NULL, true); 327 328 sdio_claim_host(sdio->func); 329 sdio_release_irq(sdio->func); 330 sdio_release_host(sdio->func); 331 332 mt76_for_each_q_rx(dev, i) { 333 struct mt76_queue *q = &dev->q_rx[i]; 334 int j; 335 336 for (j = 0; j < q->ndesc; j++) { 337 struct mt76_queue_entry *e = &q->entry[j]; 338 339 if (!e->skb) 340 continue; 341 342 dev_kfree_skb(e->skb); 343 e->skb = NULL; 344 } 345 } 346 } 347 EXPORT_SYMBOL_GPL(mt76s_deinit); 348 349 int mt76s_init(struct mt76_dev *dev, struct sdio_func *func, 350 const struct mt76_bus_ops *bus_ops) 351 { 352 struct mt76_sdio *sdio = &dev->sdio; 353 int err; 354 355 err = mt76_worker_setup(dev->hw, &sdio->status_worker, 356 mt76s_status_worker, "sdio-status"); 357 if (err) 358 return err; 359 360 err = mt76_worker_setup(dev->hw, &sdio->net_worker, mt76s_net_worker, 361 "sdio-net"); 362 if (err) 363 return err; 364 365 sched_set_fifo_low(sdio->status_worker.task); 366 sched_set_fifo_low(sdio->net_worker.task); 367 368 INIT_WORK(&sdio->stat_work, mt76s_tx_status_data); 369 370 dev->queue_ops = &sdio_queue_ops; 371 dev->bus = bus_ops; 372 dev->sdio.func = func; 373 374 return 0; 375 } 376 EXPORT_SYMBOL_GPL(mt76s_init); 377 378 MODULE_AUTHOR("Sean Wang <sean.wang@mediatek.com>"); 379 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo@kernel.org>"); 380 MODULE_LICENSE("Dual BSD/GPL"); 381