drivers/net/wireless/mediatek/mt76/usb.c

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