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