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 q->head = (q->head + 1) % q->ndesc; 260 q->queued++; 261 262 return idx; 263 } 264 265 static int 266 mt76s_tx_queue_skb_raw(struct mt76_dev *dev, struct mt76_queue *q, 267 struct sk_buff *skb, u32 tx_info) 268 { 269 int ret = -ENOSPC, len = skb->len, pad; 270 271 if (q->queued == q->ndesc) 272 goto error; 273 274 pad = round_up(skb->len, 4) - skb->len; 275 ret = mt76_skb_adjust_pad(skb, pad); 276 if (ret) 277 goto error; 278 279 spin_lock_bh(&q->lock); 280 281 q->entry[q->head].buf_sz = len; 282 q->entry[q->head].skb = skb; 283 q->head = (q->head + 1) % q->ndesc; 284 q->queued++; 285 286 spin_unlock_bh(&q->lock); 287 288 return 0; 289 290 error: 291 dev_kfree_skb(skb); 292 293 return ret; 294 } 295 296 static void mt76s_tx_kick(struct mt76_dev *dev, struct mt76_queue *q) 297 { 298 struct mt76_sdio *sdio = &dev->sdio; 299 300 mt76_worker_schedule(&sdio->txrx_worker); 301 } 302 303 static const struct mt76_queue_ops sdio_queue_ops = { 304 .tx_queue_skb = mt76s_tx_queue_skb, 305 .kick = mt76s_tx_kick, 306 .tx_queue_skb_raw = mt76s_tx_queue_skb_raw, 307 }; 308 309 void mt76s_deinit(struct mt76_dev *dev) 310 { 311 struct mt76_sdio *sdio = &dev->sdio; 312 int i; 313 314 mt76_worker_teardown(&sdio->txrx_worker); 315 mt76_worker_teardown(&sdio->status_worker); 316 mt76_worker_teardown(&sdio->net_worker); 317 318 cancel_work_sync(&sdio->stat_work); 319 clear_bit(MT76_READING_STATS, &dev->phy.state); 320 321 mt76_tx_status_check(dev, NULL, true); 322 323 sdio_claim_host(sdio->func); 324 sdio_release_irq(sdio->func); 325 sdio_release_host(sdio->func); 326 327 mt76_for_each_q_rx(dev, i) { 328 struct mt76_queue *q = &dev->q_rx[i]; 329 int j; 330 331 for (j = 0; j < q->ndesc; j++) { 332 struct mt76_queue_entry *e = &q->entry[j]; 333 334 if (!e->skb) 335 continue; 336 337 dev_kfree_skb(e->skb); 338 e->skb = NULL; 339 } 340 } 341 } 342 EXPORT_SYMBOL_GPL(mt76s_deinit); 343 344 int mt76s_init(struct mt76_dev *dev, struct sdio_func *func, 345 const struct mt76_bus_ops *bus_ops) 346 { 347 struct mt76_sdio *sdio = &dev->sdio; 348 int err; 349 350 err = mt76_worker_setup(dev->hw, &sdio->status_worker, 351 mt76s_status_worker, "sdio-status"); 352 if (err) 353 return err; 354 355 err = mt76_worker_setup(dev->hw, &sdio->net_worker, mt76s_net_worker, 356 "sdio-net"); 357 if (err) 358 return err; 359 360 sched_set_fifo_low(sdio->status_worker.task); 361 sched_set_fifo_low(sdio->net_worker.task); 362 363 INIT_WORK(&sdio->stat_work, mt76s_tx_status_data); 364 365 dev->queue_ops = &sdio_queue_ops; 366 dev->bus = bus_ops; 367 dev->sdio.func = func; 368 369 return 0; 370 } 371 EXPORT_SYMBOL_GPL(mt76s_init); 372 373 MODULE_AUTHOR("Sean Wang <sean.wang@mediatek.com>"); 374 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo@kernel.org>"); 375 MODULE_LICENSE("Dual BSD/GPL"); 376