1 // SPDX-License-Identifier: ISC
2 /*
3  * Copyright (C) 2018 Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
4  */
5 
6 #include <linux/module.h>
7 #include "mt76.h"
8 #include "usb_trace.h"
9 #include "dma.h"
10 
11 #define MT_VEND_REQ_MAX_RETRY	10
12 #define MT_VEND_REQ_TOUT_MS	300
13 
14 static bool disable_usb_sg;
15 module_param_named(disable_usb_sg, disable_usb_sg, bool, 0644);
16 MODULE_PARM_DESC(disable_usb_sg, "Disable usb scatter-gather support");
17 
18 int __mt76u_vendor_request(struct mt76_dev *dev, u8 req, u8 req_type,
19 			   u16 val, u16 offset, void *buf, size_t len)
20 {
21 	struct usb_interface *uintf = to_usb_interface(dev->dev);
22 	struct usb_device *udev = interface_to_usbdev(uintf);
23 	unsigned int pipe;
24 	int i, ret;
25 
26 	lockdep_assert_held(&dev->usb.usb_ctrl_mtx);
27 
28 	pipe = (req_type & USB_DIR_IN) ? usb_rcvctrlpipe(udev, 0)
29 				       : usb_sndctrlpipe(udev, 0);
30 	for (i = 0; i < MT_VEND_REQ_MAX_RETRY; i++) {
31 		if (test_bit(MT76_REMOVED, &dev->phy.state))
32 			return -EIO;
33 
34 		ret = usb_control_msg(udev, pipe, req, req_type, val,
35 				      offset, buf, len, MT_VEND_REQ_TOUT_MS);
36 		if (ret == -ENODEV)
37 			set_bit(MT76_REMOVED, &dev->phy.state);
38 		if (ret >= 0 || ret == -ENODEV)
39 			return ret;
40 		usleep_range(5000, 10000);
41 	}
42 
43 	dev_err(dev->dev, "vendor request req:%02x off:%04x failed:%d\n",
44 		req, offset, ret);
45 	return ret;
46 }
47 EXPORT_SYMBOL_GPL(__mt76u_vendor_request);
48 
49 int mt76u_vendor_request(struct mt76_dev *dev, u8 req,
50 			 u8 req_type, u16 val, u16 offset,
51 			 void *buf, size_t len)
52 {
53 	int ret;
54 
55 	mutex_lock(&dev->usb.usb_ctrl_mtx);
56 	ret = __mt76u_vendor_request(dev, req, req_type,
57 				     val, offset, buf, len);
58 	trace_usb_reg_wr(dev, offset, val);
59 	mutex_unlock(&dev->usb.usb_ctrl_mtx);
60 
61 	return ret;
62 }
63 EXPORT_SYMBOL_GPL(mt76u_vendor_request);
64 
65 u32 ___mt76u_rr(struct mt76_dev *dev, u8 req, u8 req_type, u32 addr)
66 {
67 	struct mt76_usb *usb = &dev->usb;
68 	u32 data = ~0;
69 	int ret;
70 
71 	ret = __mt76u_vendor_request(dev, req, req_type, addr >> 16,
72 				     addr, usb->data, sizeof(__le32));
73 	if (ret == sizeof(__le32))
74 		data = get_unaligned_le32(usb->data);
75 	trace_usb_reg_rr(dev, addr, data);
76 
77 	return data;
78 }
79 EXPORT_SYMBOL_GPL(___mt76u_rr);
80 
81 static u32 __mt76u_rr(struct mt76_dev *dev, u32 addr)
82 {
83 	u8 req;
84 
85 	switch (addr & MT_VEND_TYPE_MASK) {
86 	case MT_VEND_TYPE_EEPROM:
87 		req = MT_VEND_READ_EEPROM;
88 		break;
89 	case MT_VEND_TYPE_CFG:
90 		req = MT_VEND_READ_CFG;
91 		break;
92 	default:
93 		req = MT_VEND_MULTI_READ;
94 		break;
95 	}
96 
97 	return ___mt76u_rr(dev, req, USB_DIR_IN | USB_TYPE_VENDOR,
98 			   addr & ~MT_VEND_TYPE_MASK);
99 }
100 
101 static u32 mt76u_rr(struct mt76_dev *dev, u32 addr)
102 {
103 	u32 ret;
104 
105 	mutex_lock(&dev->usb.usb_ctrl_mtx);
106 	ret = __mt76u_rr(dev, addr);
107 	mutex_unlock(&dev->usb.usb_ctrl_mtx);
108 
109 	return ret;
110 }
111 
112 void ___mt76u_wr(struct mt76_dev *dev, u8 req, u8 req_type,
113 		 u32 addr, u32 val)
114 {
115 	struct mt76_usb *usb = &dev->usb;
116 
117 	put_unaligned_le32(val, usb->data);
118 	__mt76u_vendor_request(dev, req, req_type, addr >> 16,
119 			       addr, usb->data, sizeof(__le32));
120 	trace_usb_reg_wr(dev, addr, val);
121 }
122 EXPORT_SYMBOL_GPL(___mt76u_wr);
123 
124 static void __mt76u_wr(struct mt76_dev *dev, u32 addr, u32 val)
125 {
126 	u8 req;
127 
128 	switch (addr & MT_VEND_TYPE_MASK) {
129 	case MT_VEND_TYPE_CFG:
130 		req = MT_VEND_WRITE_CFG;
131 		break;
132 	default:
133 		req = MT_VEND_MULTI_WRITE;
134 		break;
135 	}
136 	___mt76u_wr(dev, req, USB_DIR_OUT | USB_TYPE_VENDOR,
137 		    addr & ~MT_VEND_TYPE_MASK, val);
138 }
139 
140 static void mt76u_wr(struct mt76_dev *dev, u32 addr, u32 val)
141 {
142 	mutex_lock(&dev->usb.usb_ctrl_mtx);
143 	__mt76u_wr(dev, addr, val);
144 	mutex_unlock(&dev->usb.usb_ctrl_mtx);
145 }
146 
147 static u32 mt76u_rmw(struct mt76_dev *dev, u32 addr,
148 		     u32 mask, u32 val)
149 {
150 	mutex_lock(&dev->usb.usb_ctrl_mtx);
151 	val |= __mt76u_rr(dev, addr) & ~mask;
152 	__mt76u_wr(dev, addr, val);
153 	mutex_unlock(&dev->usb.usb_ctrl_mtx);
154 
155 	return val;
156 }
157 
158 static void mt76u_copy(struct mt76_dev *dev, u32 offset,
159 		       const void *data, int len)
160 {
161 	struct mt76_usb *usb = &dev->usb;
162 	const u8 *val = data;
163 	int ret;
164 	int current_batch_size;
165 	int i = 0;
166 
167 	/* Assure that always a multiple of 4 bytes are copied,
168 	 * otherwise beacons can be corrupted.
169 	 * See: "mt76: round up length on mt76_wr_copy"
170 	 * Commit 850e8f6fbd5d0003b0
171 	 */
172 	len = round_up(len, 4);
173 
174 	mutex_lock(&usb->usb_ctrl_mtx);
175 	while (i < len) {
176 		current_batch_size = min_t(int, usb->data_len, len - i);
177 		memcpy(usb->data, val + i, current_batch_size);
178 		ret = __mt76u_vendor_request(dev, MT_VEND_MULTI_WRITE,
179 					     USB_DIR_OUT | USB_TYPE_VENDOR,
180 					     0, offset + i, usb->data,
181 					     current_batch_size);
182 		if (ret < 0)
183 			break;
184 
185 		i += current_batch_size;
186 	}
187 	mutex_unlock(&usb->usb_ctrl_mtx);
188 }
189 
190 void mt76u_read_copy(struct mt76_dev *dev, u32 offset,
191 		     void *data, int len)
192 {
193 	struct mt76_usb *usb = &dev->usb;
194 	int i = 0, batch_len, ret;
195 	u8 *val = data;
196 
197 	len = round_up(len, 4);
198 	mutex_lock(&usb->usb_ctrl_mtx);
199 	while (i < len) {
200 		batch_len = min_t(int, usb->data_len, len - i);
201 		ret = __mt76u_vendor_request(dev, MT_VEND_READ_EXT,
202 					     USB_DIR_IN | USB_TYPE_VENDOR,
203 					     (offset + i) >> 16, offset + i,
204 					     usb->data, batch_len);
205 		if (ret < 0)
206 			break;
207 
208 		memcpy(val + i, usb->data, batch_len);
209 		i += batch_len;
210 	}
211 	mutex_unlock(&usb->usb_ctrl_mtx);
212 }
213 EXPORT_SYMBOL_GPL(mt76u_read_copy);
214 
215 void mt76u_single_wr(struct mt76_dev *dev, const u8 req,
216 		     const u16 offset, const u32 val)
217 {
218 	mutex_lock(&dev->usb.usb_ctrl_mtx);
219 	__mt76u_vendor_request(dev, req,
220 			       USB_DIR_OUT | USB_TYPE_VENDOR,
221 			       val & 0xffff, offset, NULL, 0);
222 	__mt76u_vendor_request(dev, req,
223 			       USB_DIR_OUT | USB_TYPE_VENDOR,
224 			       val >> 16, offset + 2, NULL, 0);
225 	mutex_unlock(&dev->usb.usb_ctrl_mtx);
226 }
227 EXPORT_SYMBOL_GPL(mt76u_single_wr);
228 
229 static int
230 mt76u_req_wr_rp(struct mt76_dev *dev, u32 base,
231 		const struct mt76_reg_pair *data, int len)
232 {
233 	struct mt76_usb *usb = &dev->usb;
234 
235 	mutex_lock(&usb->usb_ctrl_mtx);
236 	while (len > 0) {
237 		__mt76u_wr(dev, base + data->reg, data->value);
238 		len--;
239 		data++;
240 	}
241 	mutex_unlock(&usb->usb_ctrl_mtx);
242 
243 	return 0;
244 }
245 
246 static int
247 mt76u_wr_rp(struct mt76_dev *dev, u32 base,
248 	    const struct mt76_reg_pair *data, int n)
249 {
250 	if (test_bit(MT76_STATE_MCU_RUNNING, &dev->phy.state))
251 		return dev->mcu_ops->mcu_wr_rp(dev, base, data, n);
252 	else
253 		return mt76u_req_wr_rp(dev, base, data, n);
254 }
255 
256 static int
257 mt76u_req_rd_rp(struct mt76_dev *dev, u32 base, struct mt76_reg_pair *data,
258 		int len)
259 {
260 	struct mt76_usb *usb = &dev->usb;
261 
262 	mutex_lock(&usb->usb_ctrl_mtx);
263 	while (len > 0) {
264 		data->value = __mt76u_rr(dev, base + data->reg);
265 		len--;
266 		data++;
267 	}
268 	mutex_unlock(&usb->usb_ctrl_mtx);
269 
270 	return 0;
271 }
272 
273 static int
274 mt76u_rd_rp(struct mt76_dev *dev, u32 base,
275 	    struct mt76_reg_pair *data, int n)
276 {
277 	if (test_bit(MT76_STATE_MCU_RUNNING, &dev->phy.state))
278 		return dev->mcu_ops->mcu_rd_rp(dev, base, data, n);
279 	else
280 		return mt76u_req_rd_rp(dev, base, data, n);
281 }
282 
283 static bool mt76u_check_sg(struct mt76_dev *dev)
284 {
285 	struct usb_interface *uintf = to_usb_interface(dev->dev);
286 	struct usb_device *udev = interface_to_usbdev(uintf);
287 
288 	return (!disable_usb_sg && udev->bus->sg_tablesize > 0 &&
289 		(udev->bus->no_sg_constraint ||
290 		 udev->speed == USB_SPEED_WIRELESS));
291 }
292 
293 static int
294 mt76u_set_endpoints(struct usb_interface *intf,
295 		    struct mt76_usb *usb)
296 {
297 	struct usb_host_interface *intf_desc = intf->cur_altsetting;
298 	struct usb_endpoint_descriptor *ep_desc;
299 	int i, in_ep = 0, out_ep = 0;
300 
301 	for (i = 0; i < intf_desc->desc.bNumEndpoints; i++) {
302 		ep_desc = &intf_desc->endpoint[i].desc;
303 
304 		if (usb_endpoint_is_bulk_in(ep_desc) &&
305 		    in_ep < __MT_EP_IN_MAX) {
306 			usb->in_ep[in_ep] = usb_endpoint_num(ep_desc);
307 			in_ep++;
308 		} else if (usb_endpoint_is_bulk_out(ep_desc) &&
309 			   out_ep < __MT_EP_OUT_MAX) {
310 			usb->out_ep[out_ep] = usb_endpoint_num(ep_desc);
311 			out_ep++;
312 		}
313 	}
314 
315 	if (in_ep != __MT_EP_IN_MAX || out_ep != __MT_EP_OUT_MAX)
316 		return -EINVAL;
317 	return 0;
318 }
319 
320 static int
321 mt76u_fill_rx_sg(struct mt76_dev *dev, struct mt76_queue *q, struct urb *urb,
322 		 int nsgs)
323 {
324 	int i;
325 
326 	for (i = 0; i < nsgs; i++) {
327 		void *data;
328 		int offset;
329 
330 		data = mt76_get_page_pool_buf(q, &offset, q->buf_size);
331 		if (!data)
332 			break;
333 
334 		sg_set_page(&urb->sg[i], virt_to_head_page(data), q->buf_size,
335 			    offset);
336 	}
337 
338 	if (i < nsgs) {
339 		int j;
340 
341 		for (j = nsgs; j < urb->num_sgs; j++)
342 			mt76_put_page_pool_buf(sg_virt(&urb->sg[j]), false);
343 		urb->num_sgs = i;
344 	}
345 
346 	urb->num_sgs = max_t(int, i, urb->num_sgs);
347 	urb->transfer_buffer_length = urb->num_sgs * q->buf_size;
348 	sg_init_marker(urb->sg, urb->num_sgs);
349 
350 	return i ? : -ENOMEM;
351 }
352 
353 static int
354 mt76u_refill_rx(struct mt76_dev *dev, struct mt76_queue *q,
355 		struct urb *urb, int nsgs)
356 {
357 	enum mt76_rxq_id qid = q - &dev->q_rx[MT_RXQ_MAIN];
358 	int offset;
359 
360 	if (qid == MT_RXQ_MAIN && dev->usb.sg_en)
361 		return mt76u_fill_rx_sg(dev, q, urb, nsgs);
362 
363 	urb->transfer_buffer_length = q->buf_size;
364 	urb->transfer_buffer = mt76_get_page_pool_buf(q, &offset, q->buf_size);
365 
366 	return urb->transfer_buffer ? 0 : -ENOMEM;
367 }
368 
369 static int
370 mt76u_urb_alloc(struct mt76_dev *dev, struct mt76_queue_entry *e,
371 		int sg_max_size)
372 {
373 	unsigned int size = sizeof(struct urb);
374 
375 	if (dev->usb.sg_en)
376 		size += sg_max_size * sizeof(struct scatterlist);
377 
378 	e->urb = kzalloc(size, GFP_KERNEL);
379 	if (!e->urb)
380 		return -ENOMEM;
381 
382 	usb_init_urb(e->urb);
383 
384 	if (dev->usb.sg_en && sg_max_size > 0)
385 		e->urb->sg = (struct scatterlist *)(e->urb + 1);
386 
387 	return 0;
388 }
389 
390 static int
391 mt76u_rx_urb_alloc(struct mt76_dev *dev, struct mt76_queue *q,
392 		   struct mt76_queue_entry *e)
393 {
394 	enum mt76_rxq_id qid = q - &dev->q_rx[MT_RXQ_MAIN];
395 	int err, sg_size;
396 
397 	sg_size = qid == MT_RXQ_MAIN ? MT_RX_SG_MAX_SIZE : 0;
398 	err = mt76u_urb_alloc(dev, e, sg_size);
399 	if (err)
400 		return err;
401 
402 	return mt76u_refill_rx(dev, q, e->urb, sg_size);
403 }
404 
405 static void mt76u_urb_free(struct urb *urb)
406 {
407 	int i;
408 
409 	for (i = 0; i < urb->num_sgs; i++)
410 		mt76_put_page_pool_buf(sg_virt(&urb->sg[i]), false);
411 
412 	if (urb->transfer_buffer)
413 		mt76_put_page_pool_buf(urb->transfer_buffer, false);
414 
415 	usb_free_urb(urb);
416 }
417 
418 static void
419 mt76u_fill_bulk_urb(struct mt76_dev *dev, int dir, int index,
420 		    struct urb *urb, usb_complete_t complete_fn,
421 		    void *context)
422 {
423 	struct usb_interface *uintf = to_usb_interface(dev->dev);
424 	struct usb_device *udev = interface_to_usbdev(uintf);
425 	unsigned int pipe;
426 
427 	if (dir == USB_DIR_IN)
428 		pipe = usb_rcvbulkpipe(udev, dev->usb.in_ep[index]);
429 	else
430 		pipe = usb_sndbulkpipe(udev, dev->usb.out_ep[index]);
431 
432 	urb->dev = udev;
433 	urb->pipe = pipe;
434 	urb->complete = complete_fn;
435 	urb->context = context;
436 }
437 
438 static struct urb *
439 mt76u_get_next_rx_entry(struct mt76_queue *q)
440 {
441 	struct urb *urb = NULL;
442 	unsigned long flags;
443 
444 	spin_lock_irqsave(&q->lock, flags);
445 	if (q->queued > 0) {
446 		urb = q->entry[q->tail].urb;
447 		q->tail = (q->tail + 1) % q->ndesc;
448 		q->queued--;
449 	}
450 	spin_unlock_irqrestore(&q->lock, flags);
451 
452 	return urb;
453 }
454 
455 static int
456 mt76u_get_rx_entry_len(struct mt76_dev *dev, u8 *data,
457 		       u32 data_len)
458 {
459 	u16 dma_len, min_len;
460 
461 	dma_len = get_unaligned_le16(data);
462 	if (dev->drv->drv_flags & MT_DRV_RX_DMA_HDR)
463 		return dma_len;
464 
465 	min_len = MT_DMA_HDR_LEN + MT_RX_RXWI_LEN + MT_FCE_INFO_LEN;
466 	if (data_len < min_len || !dma_len ||
467 	    dma_len + MT_DMA_HDR_LEN > data_len ||
468 	    (dma_len & 0x3))
469 		return -EINVAL;
470 	return dma_len;
471 }
472 
473 static struct sk_buff *
474 mt76u_build_rx_skb(struct mt76_dev *dev, void *data,
475 		   int len, int buf_size)
476 {
477 	int head_room, drv_flags = dev->drv->drv_flags;
478 	struct sk_buff *skb;
479 
480 	head_room = drv_flags & MT_DRV_RX_DMA_HDR ? 0 : MT_DMA_HDR_LEN;
481 	if (SKB_WITH_OVERHEAD(buf_size) < head_room + len) {
482 		struct page *page;
483 
484 		/* slow path, not enough space for data and
485 		 * skb_shared_info
486 		 */
487 		skb = alloc_skb(MT_SKB_HEAD_LEN, GFP_ATOMIC);
488 		if (!skb)
489 			return NULL;
490 
491 		skb_put_data(skb, data + head_room, MT_SKB_HEAD_LEN);
492 		data += head_room + MT_SKB_HEAD_LEN;
493 		page = virt_to_head_page(data);
494 		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
495 				page, data - page_address(page),
496 				len - MT_SKB_HEAD_LEN, buf_size);
497 
498 		return skb;
499 	}
500 
501 	/* fast path */
502 	skb = build_skb(data, buf_size);
503 	if (!skb)
504 		return NULL;
505 
506 	skb_reserve(skb, head_room);
507 	__skb_put(skb, len);
508 
509 	return skb;
510 }
511 
512 static int
513 mt76u_process_rx_entry(struct mt76_dev *dev, struct urb *urb,
514 		       int buf_size)
515 {
516 	u8 *data = urb->num_sgs ? sg_virt(&urb->sg[0]) : urb->transfer_buffer;
517 	int data_len = urb->num_sgs ? urb->sg[0].length : urb->actual_length;
518 	int len, nsgs = 1, head_room, drv_flags = dev->drv->drv_flags;
519 	struct sk_buff *skb;
520 
521 	if (!test_bit(MT76_STATE_INITIALIZED, &dev->phy.state))
522 		return 0;
523 
524 	len = mt76u_get_rx_entry_len(dev, data, urb->actual_length);
525 	if (len < 0)
526 		return 0;
527 
528 	head_room = drv_flags & MT_DRV_RX_DMA_HDR ? 0 : MT_DMA_HDR_LEN;
529 	data_len = min_t(int, len, data_len - head_room);
530 
531 	if (len == data_len &&
532 	    dev->drv->rx_check && !dev->drv->rx_check(dev, data, data_len))
533 		return 0;
534 
535 	skb = mt76u_build_rx_skb(dev, data, data_len, buf_size);
536 	if (!skb)
537 		return 0;
538 
539 	len -= data_len;
540 	while (len > 0 && nsgs < urb->num_sgs) {
541 		data_len = min_t(int, len, urb->sg[nsgs].length);
542 		skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags,
543 				sg_page(&urb->sg[nsgs]),
544 				urb->sg[nsgs].offset, data_len,
545 				buf_size);
546 		len -= data_len;
547 		nsgs++;
548 	}
549 
550 	skb_mark_for_recycle(skb);
551 	dev->drv->rx_skb(dev, MT_RXQ_MAIN, skb, NULL);
552 
553 	return nsgs;
554 }
555 
556 static void mt76u_complete_rx(struct urb *urb)
557 {
558 	struct mt76_dev *dev = dev_get_drvdata(&urb->dev->dev);
559 	struct mt76_queue *q = urb->context;
560 	unsigned long flags;
561 
562 	trace_rx_urb(dev, urb);
563 
564 	switch (urb->status) {
565 	case -ECONNRESET:
566 	case -ESHUTDOWN:
567 	case -ENOENT:
568 	case -EPROTO:
569 		return;
570 	default:
571 		dev_err_ratelimited(dev->dev, "rx urb failed: %d\n",
572 				    urb->status);
573 		fallthrough;
574 	case 0:
575 		break;
576 	}
577 
578 	spin_lock_irqsave(&q->lock, flags);
579 	if (WARN_ONCE(q->entry[q->head].urb != urb, "rx urb mismatch"))
580 		goto out;
581 
582 	q->head = (q->head + 1) % q->ndesc;
583 	q->queued++;
584 	mt76_worker_schedule(&dev->usb.rx_worker);
585 out:
586 	spin_unlock_irqrestore(&q->lock, flags);
587 }
588 
589 static int
590 mt76u_submit_rx_buf(struct mt76_dev *dev, enum mt76_rxq_id qid,
591 		    struct urb *urb)
592 {
593 	int ep = qid == MT_RXQ_MAIN ? MT_EP_IN_PKT_RX : MT_EP_IN_CMD_RESP;
594 
595 	mt76u_fill_bulk_urb(dev, USB_DIR_IN, ep, urb,
596 			    mt76u_complete_rx, &dev->q_rx[qid]);
597 	trace_submit_urb(dev, urb);
598 
599 	return usb_submit_urb(urb, GFP_ATOMIC);
600 }
601 
602 static void
603 mt76u_process_rx_queue(struct mt76_dev *dev, struct mt76_queue *q)
604 {
605 	int qid = q - &dev->q_rx[MT_RXQ_MAIN];
606 	struct urb *urb;
607 	int err, count;
608 
609 	while (true) {
610 		urb = mt76u_get_next_rx_entry(q);
611 		if (!urb)
612 			break;
613 
614 		count = mt76u_process_rx_entry(dev, urb, q->buf_size);
615 		if (count > 0) {
616 			err = mt76u_refill_rx(dev, q, urb, count);
617 			if (err < 0)
618 				break;
619 		}
620 		mt76u_submit_rx_buf(dev, qid, urb);
621 	}
622 	if (qid == MT_RXQ_MAIN) {
623 		local_bh_disable();
624 		mt76_rx_poll_complete(dev, MT_RXQ_MAIN, NULL);
625 		local_bh_enable();
626 	}
627 }
628 
629 static void mt76u_rx_worker(struct mt76_worker *w)
630 {
631 	struct mt76_usb *usb = container_of(w, struct mt76_usb, rx_worker);
632 	struct mt76_dev *dev = container_of(usb, struct mt76_dev, usb);
633 	int i;
634 
635 	rcu_read_lock();
636 	mt76_for_each_q_rx(dev, i)
637 		mt76u_process_rx_queue(dev, &dev->q_rx[i]);
638 	rcu_read_unlock();
639 }
640 
641 static int
642 mt76u_submit_rx_buffers(struct mt76_dev *dev, enum mt76_rxq_id qid)
643 {
644 	struct mt76_queue *q = &dev->q_rx[qid];
645 	unsigned long flags;
646 	int i, err = 0;
647 
648 	spin_lock_irqsave(&q->lock, flags);
649 	for (i = 0; i < q->ndesc; i++) {
650 		err = mt76u_submit_rx_buf(dev, qid, q->entry[i].urb);
651 		if (err < 0)
652 			break;
653 	}
654 	q->head = q->tail = 0;
655 	q->queued = 0;
656 	spin_unlock_irqrestore(&q->lock, flags);
657 
658 	return err;
659 }
660 
661 static int
662 mt76u_alloc_rx_queue(struct mt76_dev *dev, enum mt76_rxq_id qid)
663 {
664 	struct mt76_queue *q = &dev->q_rx[qid];
665 	int i, err;
666 
667 	err = mt76_create_page_pool(dev, q);
668 	if (err)
669 		return err;
670 
671 	spin_lock_init(&q->lock);
672 	q->entry = devm_kcalloc(dev->dev,
673 				MT_NUM_RX_ENTRIES, sizeof(*q->entry),
674 				GFP_KERNEL);
675 	if (!q->entry)
676 		return -ENOMEM;
677 
678 	q->ndesc = MT_NUM_RX_ENTRIES;
679 	q->buf_size = PAGE_SIZE;
680 
681 	for (i = 0; i < q->ndesc; i++) {
682 		err = mt76u_rx_urb_alloc(dev, q, &q->entry[i]);
683 		if (err < 0)
684 			return err;
685 	}
686 
687 	return mt76u_submit_rx_buffers(dev, qid);
688 }
689 
690 int mt76u_alloc_mcu_queue(struct mt76_dev *dev)
691 {
692 	return mt76u_alloc_rx_queue(dev, MT_RXQ_MCU);
693 }
694 EXPORT_SYMBOL_GPL(mt76u_alloc_mcu_queue);
695 
696 static void
697 mt76u_free_rx_queue(struct mt76_dev *dev, struct mt76_queue *q)
698 {
699 	int i;
700 
701 	for (i = 0; i < q->ndesc; i++) {
702 		if (!q->entry[i].urb)
703 			continue;
704 
705 		mt76u_urb_free(q->entry[i].urb);
706 		q->entry[i].urb = NULL;
707 	}
708 	page_pool_destroy(q->page_pool);
709 	q->page_pool = NULL;
710 }
711 
712 static void mt76u_free_rx(struct mt76_dev *dev)
713 {
714 	int i;
715 
716 	mt76_worker_teardown(&dev->usb.rx_worker);
717 
718 	mt76_for_each_q_rx(dev, i)
719 		mt76u_free_rx_queue(dev, &dev->q_rx[i]);
720 }
721 
722 void mt76u_stop_rx(struct mt76_dev *dev)
723 {
724 	int i;
725 
726 	mt76_worker_disable(&dev->usb.rx_worker);
727 
728 	mt76_for_each_q_rx(dev, i) {
729 		struct mt76_queue *q = &dev->q_rx[i];
730 		int j;
731 
732 		for (j = 0; j < q->ndesc; j++)
733 			usb_poison_urb(q->entry[j].urb);
734 	}
735 }
736 EXPORT_SYMBOL_GPL(mt76u_stop_rx);
737 
738 int mt76u_resume_rx(struct mt76_dev *dev)
739 {
740 	int i;
741 
742 	mt76_for_each_q_rx(dev, i) {
743 		struct mt76_queue *q = &dev->q_rx[i];
744 		int err, j;
745 
746 		for (j = 0; j < q->ndesc; j++)
747 			usb_unpoison_urb(q->entry[j].urb);
748 
749 		err = mt76u_submit_rx_buffers(dev, i);
750 		if (err < 0)
751 			return err;
752 	}
753 
754 	mt76_worker_enable(&dev->usb.rx_worker);
755 
756 	return 0;
757 }
758 EXPORT_SYMBOL_GPL(mt76u_resume_rx);
759 
760 static void mt76u_status_worker(struct mt76_worker *w)
761 {
762 	struct mt76_usb *usb = container_of(w, struct mt76_usb, status_worker);
763 	struct mt76_dev *dev = container_of(usb, struct mt76_dev, usb);
764 	struct mt76_queue_entry entry;
765 	struct mt76_queue *q;
766 	int i;
767 
768 	if (!test_bit(MT76_STATE_RUNNING, &dev->phy.state))
769 		return;
770 
771 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
772 		q = dev->phy.q_tx[i];
773 		if (!q)
774 			continue;
775 
776 		while (q->queued > 0) {
777 			if (!q->entry[q->tail].done)
778 				break;
779 
780 			entry = q->entry[q->tail];
781 			q->entry[q->tail].done = false;
782 
783 			mt76_queue_tx_complete(dev, q, &entry);
784 		}
785 
786 		if (!q->queued)
787 			wake_up(&dev->tx_wait);
788 
789 		mt76_worker_schedule(&dev->tx_worker);
790 	}
791 
792 	if (dev->drv->tx_status_data &&
793 	    !test_and_set_bit(MT76_READING_STATS, &dev->phy.state))
794 		queue_work(dev->wq, &dev->usb.stat_work);
795 }
796 
797 static void mt76u_tx_status_data(struct work_struct *work)
798 {
799 	struct mt76_usb *usb;
800 	struct mt76_dev *dev;
801 	u8 update = 1;
802 	u16 count = 0;
803 
804 	usb = container_of(work, struct mt76_usb, stat_work);
805 	dev = container_of(usb, struct mt76_dev, usb);
806 
807 	while (true) {
808 		if (test_bit(MT76_REMOVED, &dev->phy.state))
809 			break;
810 
811 		if (!dev->drv->tx_status_data(dev, &update))
812 			break;
813 		count++;
814 	}
815 
816 	if (count && test_bit(MT76_STATE_RUNNING, &dev->phy.state))
817 		queue_work(dev->wq, &usb->stat_work);
818 	else
819 		clear_bit(MT76_READING_STATS, &dev->phy.state);
820 }
821 
822 static void mt76u_complete_tx(struct urb *urb)
823 {
824 	struct mt76_dev *dev = dev_get_drvdata(&urb->dev->dev);
825 	struct mt76_queue_entry *e = urb->context;
826 
827 	if (mt76u_urb_error(urb))
828 		dev_err(dev->dev, "tx urb failed: %d\n", urb->status);
829 	e->done = true;
830 
831 	mt76_worker_schedule(&dev->usb.status_worker);
832 }
833 
834 static int
835 mt76u_tx_setup_buffers(struct mt76_dev *dev, struct sk_buff *skb,
836 		       struct urb *urb)
837 {
838 	urb->transfer_buffer_length = skb->len;
839 
840 	if (!dev->usb.sg_en) {
841 		urb->transfer_buffer = skb->data;
842 		return 0;
843 	}
844 
845 	sg_init_table(urb->sg, MT_TX_SG_MAX_SIZE);
846 	urb->num_sgs = skb_to_sgvec(skb, urb->sg, 0, skb->len);
847 	if (!urb->num_sgs)
848 		return -ENOMEM;
849 
850 	return urb->num_sgs;
851 }
852 
853 static int
854 mt76u_tx_queue_skb(struct mt76_dev *dev, struct mt76_queue *q,
855 		   enum mt76_txq_id qid, struct sk_buff *skb,
856 		   struct mt76_wcid *wcid, struct ieee80211_sta *sta)
857 {
858 	struct mt76_tx_info tx_info = {
859 		.skb = skb,
860 	};
861 	u16 idx = q->head;
862 	int err;
863 
864 	if (q->queued == q->ndesc)
865 		return -ENOSPC;
866 
867 	skb->prev = skb->next = NULL;
868 	err = dev->drv->tx_prepare_skb(dev, NULL, qid, wcid, sta, &tx_info);
869 	if (err < 0)
870 		return err;
871 
872 	err = mt76u_tx_setup_buffers(dev, tx_info.skb, q->entry[idx].urb);
873 	if (err < 0)
874 		return err;
875 
876 	mt76u_fill_bulk_urb(dev, USB_DIR_OUT, q2ep(q->hw_idx),
877 			    q->entry[idx].urb, mt76u_complete_tx,
878 			    &q->entry[idx]);
879 
880 	q->head = (q->head + 1) % q->ndesc;
881 	q->entry[idx].skb = tx_info.skb;
882 	q->entry[idx].wcid = 0xffff;
883 	q->queued++;
884 
885 	return idx;
886 }
887 
888 static void mt76u_tx_kick(struct mt76_dev *dev, struct mt76_queue *q)
889 {
890 	struct urb *urb;
891 	int err;
892 
893 	while (q->first != q->head) {
894 		urb = q->entry[q->first].urb;
895 
896 		trace_submit_urb(dev, urb);
897 		err = usb_submit_urb(urb, GFP_ATOMIC);
898 		if (err < 0) {
899 			if (err == -ENODEV)
900 				set_bit(MT76_REMOVED, &dev->phy.state);
901 			else
902 				dev_err(dev->dev, "tx urb submit failed:%d\n",
903 					err);
904 			break;
905 		}
906 		q->first = (q->first + 1) % q->ndesc;
907 	}
908 }
909 
910 static u8 mt76u_ac_to_hwq(struct mt76_dev *dev, u8 ac)
911 {
912 	if (mt76_chip(dev) == 0x7663) {
913 		static const u8 lmac_queue_map[] = {
914 			/* ac to lmac mapping */
915 			[IEEE80211_AC_BK] = 0,
916 			[IEEE80211_AC_BE] = 1,
917 			[IEEE80211_AC_VI] = 2,
918 			[IEEE80211_AC_VO] = 4,
919 		};
920 
921 		if (WARN_ON(ac >= ARRAY_SIZE(lmac_queue_map)))
922 			return 1; /* BE */
923 
924 		return lmac_queue_map[ac];
925 	}
926 
927 	return mt76_ac_to_hwq(ac);
928 }
929 
930 static int mt76u_alloc_tx(struct mt76_dev *dev)
931 {
932 	struct mt76_queue *q;
933 	int i, j, err;
934 
935 	for (i = 0; i <= MT_TXQ_PSD; i++) {
936 		if (i >= IEEE80211_NUM_ACS) {
937 			dev->phy.q_tx[i] = dev->phy.q_tx[0];
938 			continue;
939 		}
940 
941 		q = devm_kzalloc(dev->dev, sizeof(*q), GFP_KERNEL);
942 		if (!q)
943 			return -ENOMEM;
944 
945 		spin_lock_init(&q->lock);
946 		q->hw_idx = mt76u_ac_to_hwq(dev, i);
947 
948 		dev->phy.q_tx[i] = q;
949 
950 		q->entry = devm_kcalloc(dev->dev,
951 					MT_NUM_TX_ENTRIES, sizeof(*q->entry),
952 					GFP_KERNEL);
953 		if (!q->entry)
954 			return -ENOMEM;
955 
956 		q->ndesc = MT_NUM_TX_ENTRIES;
957 		for (j = 0; j < q->ndesc; j++) {
958 			err = mt76u_urb_alloc(dev, &q->entry[j],
959 					      MT_TX_SG_MAX_SIZE);
960 			if (err < 0)
961 				return err;
962 		}
963 	}
964 	return 0;
965 }
966 
967 static void mt76u_free_tx(struct mt76_dev *dev)
968 {
969 	int i;
970 
971 	mt76_worker_teardown(&dev->usb.status_worker);
972 
973 	for (i = 0; i < IEEE80211_NUM_ACS; i++) {
974 		struct mt76_queue *q;
975 		int j;
976 
977 		q = dev->phy.q_tx[i];
978 		if (!q)
979 			continue;
980 
981 		for (j = 0; j < q->ndesc; j++) {
982 			usb_free_urb(q->entry[j].urb);
983 			q->entry[j].urb = NULL;
984 		}
985 	}
986 }
987 
988 void mt76u_stop_tx(struct mt76_dev *dev)
989 {
990 	int ret;
991 
992 	mt76_worker_disable(&dev->usb.status_worker);
993 
994 	ret = wait_event_timeout(dev->tx_wait, !mt76_has_tx_pending(&dev->phy),
995 				 HZ / 5);
996 	if (!ret) {
997 		struct mt76_queue_entry entry;
998 		struct mt76_queue *q;
999 		int i, j;
1000 
1001 		dev_err(dev->dev, "timed out waiting for pending tx\n");
1002 
1003 		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
1004 			q = dev->phy.q_tx[i];
1005 			if (!q)
1006 				continue;
1007 
1008 			for (j = 0; j < q->ndesc; j++)
1009 				usb_kill_urb(q->entry[j].urb);
1010 		}
1011 
1012 		mt76_worker_disable(&dev->tx_worker);
1013 
1014 		/* On device removal we maight queue skb's, but mt76u_tx_kick()
1015 		 * will fail to submit urb, cleanup those skb's manually.
1016 		 */
1017 		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
1018 			q = dev->phy.q_tx[i];
1019 			if (!q)
1020 				continue;
1021 
1022 			while (q->queued > 0) {
1023 				entry = q->entry[q->tail];
1024 				q->entry[q->tail].done = false;
1025 				mt76_queue_tx_complete(dev, q, &entry);
1026 			}
1027 		}
1028 
1029 		mt76_worker_enable(&dev->tx_worker);
1030 	}
1031 
1032 	cancel_work_sync(&dev->usb.stat_work);
1033 	clear_bit(MT76_READING_STATS, &dev->phy.state);
1034 
1035 	mt76_worker_enable(&dev->usb.status_worker);
1036 
1037 	mt76_tx_status_check(dev, true);
1038 }
1039 EXPORT_SYMBOL_GPL(mt76u_stop_tx);
1040 
1041 void mt76u_queues_deinit(struct mt76_dev *dev)
1042 {
1043 	mt76u_stop_rx(dev);
1044 	mt76u_stop_tx(dev);
1045 
1046 	mt76u_free_rx(dev);
1047 	mt76u_free_tx(dev);
1048 }
1049 EXPORT_SYMBOL_GPL(mt76u_queues_deinit);
1050 
1051 int mt76u_alloc_queues(struct mt76_dev *dev)
1052 {
1053 	int err;
1054 
1055 	err = mt76u_alloc_rx_queue(dev, MT_RXQ_MAIN);
1056 	if (err < 0)
1057 		return err;
1058 
1059 	return mt76u_alloc_tx(dev);
1060 }
1061 EXPORT_SYMBOL_GPL(mt76u_alloc_queues);
1062 
1063 static const struct mt76_queue_ops usb_queue_ops = {
1064 	.tx_queue_skb = mt76u_tx_queue_skb,
1065 	.kick = mt76u_tx_kick,
1066 };
1067 
1068 int __mt76u_init(struct mt76_dev *dev, struct usb_interface *intf,
1069 		 struct mt76_bus_ops *ops)
1070 {
1071 	struct usb_device *udev = interface_to_usbdev(intf);
1072 	struct mt76_usb *usb = &dev->usb;
1073 	int err;
1074 
1075 	INIT_WORK(&usb->stat_work, mt76u_tx_status_data);
1076 
1077 	usb->data_len = usb_maxpacket(udev, usb_sndctrlpipe(udev, 0));
1078 	if (usb->data_len < 32)
1079 		usb->data_len = 32;
1080 
1081 	usb->data = devm_kmalloc(dev->dev, usb->data_len, GFP_KERNEL);
1082 	if (!usb->data)
1083 		return -ENOMEM;
1084 
1085 	mutex_init(&usb->usb_ctrl_mtx);
1086 	dev->bus = ops;
1087 	dev->queue_ops = &usb_queue_ops;
1088 
1089 	dev_set_drvdata(&udev->dev, dev);
1090 
1091 	usb->sg_en = mt76u_check_sg(dev);
1092 
1093 	err = mt76u_set_endpoints(intf, usb);
1094 	if (err < 0)
1095 		return err;
1096 
1097 	err = mt76_worker_setup(dev->hw, &usb->rx_worker, mt76u_rx_worker,
1098 				"usb-rx");
1099 	if (err)
1100 		return err;
1101 
1102 	err = mt76_worker_setup(dev->hw, &usb->status_worker,
1103 				mt76u_status_worker, "usb-status");
1104 	if (err)
1105 		return err;
1106 
1107 	sched_set_fifo_low(usb->rx_worker.task);
1108 	sched_set_fifo_low(usb->status_worker.task);
1109 
1110 	return 0;
1111 }
1112 EXPORT_SYMBOL_GPL(__mt76u_init);
1113 
1114 int mt76u_init(struct mt76_dev *dev, struct usb_interface *intf)
1115 {
1116 	static struct mt76_bus_ops bus_ops = {
1117 		.rr = mt76u_rr,
1118 		.wr = mt76u_wr,
1119 		.rmw = mt76u_rmw,
1120 		.read_copy = mt76u_read_copy,
1121 		.write_copy = mt76u_copy,
1122 		.wr_rp = mt76u_wr_rp,
1123 		.rd_rp = mt76u_rd_rp,
1124 		.type = MT76_BUS_USB,
1125 	};
1126 
1127 	return __mt76u_init(dev, intf, &bus_ops);
1128 }
1129 EXPORT_SYMBOL_GPL(mt76u_init);
1130 
1131 MODULE_AUTHOR("Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>");
1132 MODULE_LICENSE("Dual BSD/GPL");
1133