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 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 <sean.wang@mediatek.com>"); 377 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo@kernel.org>"); 378 MODULE_LICENSE("Dual BSD/GPL"); 379