drivers/net/wireless/b43/pio.c

   1 /*
   2
   3   Broadcom B43 wireless driver
   4
   5   PIO data transfer
   6
   7   Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
   8
   9   This program is free software; you can redistribute it and/or modify
  10   it under the terms of the GNU General Public License as published by
  11   the Free Software Foundation; either version 2 of the License, or
  12   (at your option) any later version.
  13
  14   This program is distributed in the hope that it will be useful,
  15   but WITHOUT ANY WARRANTY; without even the implied warranty of
  16   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17   GNU General Public License for more details.
  18
  19   You should have received a copy of the GNU General Public License
  20   along with this program; see the file COPYING.  If not, write to
  21   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  22   Boston, MA 02110-1301, USA.
  23
  24 */
  25
  26 #include "b43.h"
  27 #include "pio.h"
  28 #include "dma.h"
  29 #include "main.h"
  30 #include "xmit.h"
  31
  32 #include <linux/delay.h>
  33
  34
  35 static u16 generate_cookie(struct b43_pio_txqueue *q,
  36                            struct b43_pio_txpacket *pack)
  37 {
  38         u16 cookie;
  39
  40         /* Use the upper 4 bits of the cookie as
  41          * PIO controller ID and store the packet index number
  42          * in the lower 12 bits.
  43          * Note that the cookie must never be 0, as this
  44          * is a special value used in RX path.
  45          * It can also not be 0xFFFF because that is special
  46          * for multicast frames.
  47          */
  48         cookie = (((u16)q->index + 1) << 12);
  49         cookie |= pack->index;
  50
  51         return cookie;
  52 }
  53
  54 static
  55 struct b43_pio_txqueue *parse_cookie(struct b43_wldev *dev,
  56                                      u16 cookie,
  57                                       struct b43_pio_txpacket **pack)
  58 {
  59         struct b43_pio *pio = &dev->pio;
  60         struct b43_pio_txqueue *q = NULL;
  61         unsigned int pack_index;
  62
  63         switch (cookie & 0xF000) {
  64         case 0x1000:
  65                 q = pio->tx_queue_AC_BK;
  66                 break;
  67         case 0x2000:
  68                 q = pio->tx_queue_AC_BE;
  69                 break;
  70         case 0x3000:
  71                 q = pio->tx_queue_AC_VI;
  72                 break;
  73         case 0x4000:
  74                 q = pio->tx_queue_AC_VO;
  75                 break;
  76         case 0x5000:
  77                 q = pio->tx_queue_mcast;
  78                 break;
  79         }
  80         if (B43_WARN_ON(!q))
  81                 return NULL;
  82         pack_index = (cookie & 0x0FFF);
  83         if (B43_WARN_ON(pack_index >= ARRAY_SIZE(q->packets)))
  84                 return NULL;
  85         *pack = &q->packets[pack_index];
  86
  87         return q;
  88 }
  89
  90 static u16 index_to_pioqueue_base(struct b43_wldev *dev,
  91                                   unsigned int index)
  92 {
  93         static const u16 bases[] = {
  94                 B43_MMIO_PIO_BASE0,
  95                 B43_MMIO_PIO_BASE1,
  96                 B43_MMIO_PIO_BASE2,
  97                 B43_MMIO_PIO_BASE3,
  98                 B43_MMIO_PIO_BASE4,
  99                 B43_MMIO_PIO_BASE5,
 100                 B43_MMIO_PIO_BASE6,
 101                 B43_MMIO_PIO_BASE7,
 102         };
 103         static const u16 bases_rev11[] = {
 104                 B43_MMIO_PIO11_BASE0,
 105                 B43_MMIO_PIO11_BASE1,
 106                 B43_MMIO_PIO11_BASE2,
 107                 B43_MMIO_PIO11_BASE3,
 108                 B43_MMIO_PIO11_BASE4,
 109                 B43_MMIO_PIO11_BASE5,
 110         };
 111
 112         if (dev->dev->id.revision >= 11) {
 113                 B43_WARN_ON(index >= ARRAY_SIZE(bases_rev11));
 114                 return bases_rev11[index];
 115         }
 116         B43_WARN_ON(index >= ARRAY_SIZE(bases));
 117         return bases[index];
 118 }
 119
 120 static u16 pio_txqueue_offset(struct b43_wldev *dev)
 121 {
 122         if (dev->dev->id.revision >= 11)
 123                 return 0x18;
 124         return 0;
 125 }
 126
 127 static u16 pio_rxqueue_offset(struct b43_wldev *dev)
 128 {
 129         if (dev->dev->id.revision >= 11)
 130                 return 0x38;
 131         return 8;
 132 }
 133
 134 static struct b43_pio_txqueue *b43_setup_pioqueue_tx(struct b43_wldev *dev,
 135                                                      unsigned int index)
 136 {
 137         struct b43_pio_txqueue *q;
 138         struct b43_pio_txpacket *p;
 139         unsigned int i;
 140
 141         q = kzalloc(sizeof(*q), GFP_KERNEL);
 142         if (!q)
 143                 return NULL;
 144         q->dev = dev;
 145         q->rev = dev->dev->id.revision;
 146         q->mmio_base = index_to_pioqueue_base(dev, index) +
 147                        pio_txqueue_offset(dev);
 148         q->index = index;
 149
 150         q->free_packet_slots = B43_PIO_MAX_NR_TXPACKETS;
 151         if (q->rev >= 8) {
 152                 q->buffer_size = 1920; //FIXME this constant is wrong.
 153         } else {
 154                 q->buffer_size = b43_piotx_read16(q, B43_PIO_TXQBUFSIZE);
 155                 q->buffer_size -= 80;
 156         }
 157
 158         INIT_LIST_HEAD(&q->packets_list);
 159         for (i = 0; i < ARRAY_SIZE(q->packets); i++) {
 160                 p = &(q->packets[i]);
 161                 INIT_LIST_HEAD(&p->list);
 162                 p->index = i;
 163                 p->queue = q;
 164                 list_add(&p->list, &q->packets_list);
 165         }
 166
 167         return q;
 168 }
 169
 170 static struct b43_pio_rxqueue *b43_setup_pioqueue_rx(struct b43_wldev *dev,
 171                                                      unsigned int index)
 172 {
 173         struct b43_pio_rxqueue *q;
 174
 175         q = kzalloc(sizeof(*q), GFP_KERNEL);
 176         if (!q)
 177                 return NULL;
 178         q->dev = dev;
 179         q->rev = dev->dev->id.revision;
 180         q->mmio_base = index_to_pioqueue_base(dev, index) +
 181                        pio_rxqueue_offset(dev);
 182
 183         /* Enable Direct FIFO RX (PIO) on the engine. */
 184         b43_dma_direct_fifo_rx(dev, index, 1);
 185
 186         return q;
 187 }
 188
 189 static void b43_pio_cancel_tx_packets(struct b43_pio_txqueue *q)
 190 {
 191         struct b43_pio_txpacket *pack;
 192         unsigned int i;
 193
 194         for (i = 0; i < ARRAY_SIZE(q->packets); i++) {
 195                 pack = &(q->packets[i]);
 196                 if (pack->skb) {
 197                         dev_kfree_skb_any(pack->skb);
 198                         pack->skb = NULL;
 199                 }
 200         }
 201 }
 202
 203 static void b43_destroy_pioqueue_tx(struct b43_pio_txqueue *q,
 204                                     const char *name)
 205 {
 206         if (!q)
 207                 return;
 208         b43_pio_cancel_tx_packets(q);
 209         kfree(q);
 210 }
 211
 212 static void b43_destroy_pioqueue_rx(struct b43_pio_rxqueue *q,
 213                                     const char *name)
 214 {
 215         if (!q)
 216                 return;
 217         kfree(q);
 218 }
 219
 220 #define destroy_queue_tx(pio, queue) do {                               \
 221         b43_destroy_pioqueue_tx((pio)->queue, __stringify(queue));      \
 222         (pio)->queue = NULL;                                            \
 223   } while (0)
 224
 225 #define destroy_queue_rx(pio, queue) do {                               \
 226         b43_destroy_pioqueue_rx((pio)->queue, __stringify(queue));      \
 227         (pio)->queue = NULL;                                            \
 228   } while (0)
 229
 230 void b43_pio_free(struct b43_wldev *dev)
 231 {
 232         struct b43_pio *pio;
 233
 234         if (!b43_using_pio_transfers(dev))
 235                 return;
 236         pio = &dev->pio;
 237
 238         destroy_queue_rx(pio, rx_queue);
 239         destroy_queue_tx(pio, tx_queue_mcast);
 240         destroy_queue_tx(pio, tx_queue_AC_VO);
 241         destroy_queue_tx(pio, tx_queue_AC_VI);
 242         destroy_queue_tx(pio, tx_queue_AC_BE);
 243         destroy_queue_tx(pio, tx_queue_AC_BK);
 244 }
 245
 246 int b43_pio_init(struct b43_wldev *dev)
 247 {
 248         struct b43_pio *pio = &dev->pio;
 249         int err = -ENOMEM;
 250
 251         b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
 252                     & ~B43_MACCTL_BE);
 253         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RXPADOFF, 0);
 254
 255         pio->tx_queue_AC_BK = b43_setup_pioqueue_tx(dev, 0);
 256         if (!pio->tx_queue_AC_BK)
 257                 goto out;
 258
 259         pio->tx_queue_AC_BE = b43_setup_pioqueue_tx(dev, 1);
 260         if (!pio->tx_queue_AC_BE)
 261                 goto err_destroy_bk;
 262
 263         pio->tx_queue_AC_VI = b43_setup_pioqueue_tx(dev, 2);
 264         if (!pio->tx_queue_AC_VI)
 265                 goto err_destroy_be;
 266
 267         pio->tx_queue_AC_VO = b43_setup_pioqueue_tx(dev, 3);
 268         if (!pio->tx_queue_AC_VO)
 269                 goto err_destroy_vi;
 270
 271         pio->tx_queue_mcast = b43_setup_pioqueue_tx(dev, 4);
 272         if (!pio->tx_queue_mcast)
 273                 goto err_destroy_vo;
 274
 275         pio->rx_queue = b43_setup_pioqueue_rx(dev, 0);
 276         if (!pio->rx_queue)
 277                 goto err_destroy_mcast;
 278
 279         b43dbg(dev->wl, "PIO initialized\n");
 280         err = 0;
 281 out:
 282         return err;
 283
 284 err_destroy_mcast:
 285         destroy_queue_tx(pio, tx_queue_mcast);
 286 err_destroy_vo:
 287         destroy_queue_tx(pio, tx_queue_AC_VO);
 288 err_destroy_vi:
 289         destroy_queue_tx(pio, tx_queue_AC_VI);
 290 err_destroy_be:
 291         destroy_queue_tx(pio, tx_queue_AC_BE);
 292 err_destroy_bk:
 293         destroy_queue_tx(pio, tx_queue_AC_BK);
 294         return err;
 295 }
 296
 297 /* Static mapping of mac80211's queues (priorities) to b43 PIO queues. */
 298 static struct b43_pio_txqueue *select_queue_by_priority(struct b43_wldev *dev,
 299                                                         u8 queue_prio)
 300 {
 301         struct b43_pio_txqueue *q;
 302
 303         if (dev->qos_enabled) {
 304                 /* 0 = highest priority */
 305                 switch (queue_prio) {
 306                 default:
 307                         B43_WARN_ON(1);
 308                         /* fallthrough */
 309                 case 0:
 310                         q = dev->pio.tx_queue_AC_VO;
 311                         break;
 312                 case 1:
 313                         q = dev->pio.tx_queue_AC_VI;
 314                         break;
 315                 case 2:
 316                         q = dev->pio.tx_queue_AC_BE;
 317                         break;
 318                 case 3:
 319                         q = dev->pio.tx_queue_AC_BK;
 320                         break;
 321                 }
 322         } else
 323                 q = dev->pio.tx_queue_AC_BE;
 324
 325         return q;
 326 }
 327
 328 static u16 tx_write_2byte_queue(struct b43_pio_txqueue *q,
 329                                 u16 ctl,
 330                                 const void *_data,
 331                                 unsigned int data_len)
 332 {
 333         struct b43_wldev *dev = q->dev;
 334         struct b43_wl *wl = dev->wl;
 335         const u8 *data = _data;
 336
 337         ctl |= B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_WRITEHI;
 338         b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
 339
 340         ssb_block_write(dev->dev, data, (data_len & ~1),
 341                         q->mmio_base + B43_PIO_TXDATA,
 342                         sizeof(u16));
 343         if (data_len & 1) {
 344                 u8 *tail = wl->pio_tailspace;
 345                 BUILD_BUG_ON(sizeof(wl->pio_tailspace) < 2);
 346
 347                 /* Write the last byte. */
 348                 ctl &= ~B43_PIO_TXCTL_WRITEHI;
 349                 b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
 350                 tail[0] = data[data_len - 1];
 351                 tail[1] = 0;
 352                 ssb_block_write(dev->dev, tail, 2,
 353                                 q->mmio_base + B43_PIO_TXDATA,
 354                                 sizeof(u16));
 355         }
 356
 357         return ctl;
 358 }
 359
 360 static void pio_tx_frame_2byte_queue(struct b43_pio_txpacket *pack,
 361                                      const u8 *hdr, unsigned int hdrlen)
 362 {
 363         struct b43_pio_txqueue *q = pack->queue;
 364         const char *frame = pack->skb->data;
 365         unsigned int frame_len = pack->skb->len;
 366         u16 ctl;
 367
 368         ctl = b43_piotx_read16(q, B43_PIO_TXCTL);
 369         ctl |= B43_PIO_TXCTL_FREADY;
 370         ctl &= ~B43_PIO_TXCTL_EOF;
 371
 372         /* Transfer the header data. */
 373         ctl = tx_write_2byte_queue(q, ctl, hdr, hdrlen);
 374         /* Transfer the frame data. */
 375         ctl = tx_write_2byte_queue(q, ctl, frame, frame_len);
 376
 377         ctl |= B43_PIO_TXCTL_EOF;
 378         b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
 379 }
 380
 381 static u32 tx_write_4byte_queue(struct b43_pio_txqueue *q,
 382                                 u32 ctl,
 383                                 const void *_data,
 384                                 unsigned int data_len)
 385 {
 386         struct b43_wldev *dev = q->dev;
 387         struct b43_wl *wl = dev->wl;
 388         const u8 *data = _data;
 389
 390         ctl |= B43_PIO8_TXCTL_0_7 | B43_PIO8_TXCTL_8_15 |
 391                B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_24_31;
 392         b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
 393
 394         ssb_block_write(dev->dev, data, (data_len & ~3),
 395                         q->mmio_base + B43_PIO8_TXDATA,
 396                         sizeof(u32));
 397         if (data_len & 3) {
 398                 u8 *tail = wl->pio_tailspace;
 399                 BUILD_BUG_ON(sizeof(wl->pio_tailspace) < 4);
 400
 401                 memset(tail, 0, 4);
 402                 /* Write the last few bytes. */
 403                 ctl &= ~(B43_PIO8_TXCTL_8_15 | B43_PIO8_TXCTL_16_23 |
 404                          B43_PIO8_TXCTL_24_31);
 405                 switch (data_len & 3) {
 406                 case 3:
 407                         ctl |= B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_8_15;
 408                         tail[0] = data[data_len - 3];
 409                         tail[1] = data[data_len - 2];
 410                         tail[2] = data[data_len - 1];
 411                         break;
 412                 case 2:
 413                         ctl |= B43_PIO8_TXCTL_8_15;
 414                         tail[0] = data[data_len - 2];
 415                         tail[1] = data[data_len - 1];
 416                         break;
 417                 case 1:
 418                         tail[0] = data[data_len - 1];
 419                         break;
 420                 }
 421                 b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
 422                 ssb_block_write(dev->dev, tail, 4,
 423                                 q->mmio_base + B43_PIO8_TXDATA,
 424                                 sizeof(u32));
 425         }
 426
 427         return ctl;
 428 }
 429
 430 static void pio_tx_frame_4byte_queue(struct b43_pio_txpacket *pack,
 431                                      const u8 *hdr, unsigned int hdrlen)
 432 {
 433         struct b43_pio_txqueue *q = pack->queue;
 434         const char *frame = pack->skb->data;
 435         unsigned int frame_len = pack->skb->len;
 436         u32 ctl;
 437
 438         ctl = b43_piotx_read32(q, B43_PIO8_TXCTL);
 439         ctl |= B43_PIO8_TXCTL_FREADY;
 440         ctl &= ~B43_PIO8_TXCTL_EOF;
 441
 442         /* Transfer the header data. */
 443         ctl = tx_write_4byte_queue(q, ctl, hdr, hdrlen);
 444         /* Transfer the frame data. */
 445         ctl = tx_write_4byte_queue(q, ctl, frame, frame_len);
 446
 447         ctl |= B43_PIO8_TXCTL_EOF;
 448         b43_piotx_write32(q, B43_PIO_TXCTL, ctl);
 449 }
 450
 451 static int pio_tx_frame(struct b43_pio_txqueue *q,
 452                         struct sk_buff *skb)
 453 {
 454         struct b43_wldev *dev = q->dev;
 455         struct b43_wl *wl = dev->wl;
 456         struct b43_pio_txpacket *pack;
 457         u16 cookie;
 458         int err;
 459         unsigned int hdrlen;
 460         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 461         struct b43_txhdr *txhdr = (struct b43_txhdr *)wl->pio_scratchspace;
 462
 463         B43_WARN_ON(list_empty(&q->packets_list));
 464         pack = list_entry(q->packets_list.next,
 465                           struct b43_pio_txpacket, list);
 466
 467         cookie = generate_cookie(q, pack);
 468         hdrlen = b43_txhdr_size(dev);
 469         BUILD_BUG_ON(sizeof(wl->pio_scratchspace) < sizeof(struct b43_txhdr));
 470         B43_WARN_ON(sizeof(wl->pio_scratchspace) < hdrlen);
 471         err = b43_generate_txhdr(dev, (u8 *)txhdr, skb,
 472                                  info, cookie);
 473         if (err)
 474                 return err;
 475
 476         if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 477                 /* Tell the firmware about the cookie of the last
 478                  * mcast frame, so it can clear the more-data bit in it. */
 479                 b43_shm_write16(dev, B43_SHM_SHARED,
 480                                 B43_SHM_SH_MCASTCOOKIE, cookie);
 481         }
 482
 483         pack->skb = skb;
 484         if (q->rev >= 8)
 485                 pio_tx_frame_4byte_queue(pack, (const u8 *)txhdr, hdrlen);
 486         else
 487                 pio_tx_frame_2byte_queue(pack, (const u8 *)txhdr, hdrlen);
 488
 489         /* Remove it from the list of available packet slots.
 490          * It will be put back when we receive the status report. */
 491         list_del(&pack->list);
 492
 493         /* Update the queue statistics. */
 494         q->buffer_used += roundup(skb->len + hdrlen, 4);
 495         q->free_packet_slots -= 1;
 496
 497         return 0;
 498 }
 499
 500 int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb)
 501 {
 502         struct b43_pio_txqueue *q;
 503         struct ieee80211_hdr *hdr;
 504         unsigned int hdrlen, total_len;
 505         int err = 0;
 506         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 507
 508         hdr = (struct ieee80211_hdr *)skb->data;
 509
 510         if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 511                 /* The multicast queue will be sent after the DTIM. */
 512                 q = dev->pio.tx_queue_mcast;
 513                 /* Set the frame More-Data bit. Ucode will clear it
 514                  * for us on the last frame. */
 515                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 516         } else {
 517                 /* Decide by priority where to put this frame. */
 518                 q = select_queue_by_priority(dev, skb_get_queue_mapping(skb));
 519         }
 520
 521         hdrlen = b43_txhdr_size(dev);
 522         total_len = roundup(skb->len + hdrlen, 4);
 523
 524         if (unlikely(total_len > q->buffer_size)) {
 525                 err = -ENOBUFS;
 526                 b43dbg(dev->wl, "PIO: TX packet longer than queue.\n");
 527                 goto out;
 528         }
 529         if (unlikely(q->free_packet_slots == 0)) {
 530                 err = -ENOBUFS;
 531                 b43warn(dev->wl, "PIO: TX packet overflow.\n");
 532                 goto out;
 533         }
 534         B43_WARN_ON(q->buffer_used > q->buffer_size);
 535
 536         if (total_len > (q->buffer_size - q->buffer_used)) {
 537                 /* Not enough memory on the queue. */
 538                 err = -EBUSY;
 539                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
 540                 q->stopped = 1;
 541                 goto out;
 542         }
 543
 544         /* Assign the queue number to the ring (if not already done before)
 545          * so TX status handling can use it. The mac80211-queue to b43-queue
 546          * mapping is static, so we don't need to store it per frame. */
 547         q->queue_prio = skb_get_queue_mapping(skb);
 548
 549         err = pio_tx_frame(q, skb);
 550         if (unlikely(err == -ENOKEY)) {
 551                 /* Drop this packet, as we don't have the encryption key
 552                  * anymore and must not transmit it unencrypted. */
 553                 dev_kfree_skb_any(skb);
 554                 err = 0;
 555                 goto out;
 556         }
 557         if (unlikely(err)) {
 558                 b43err(dev->wl, "PIO transmission failure\n");
 559                 goto out;
 560         }
 561         q->nr_tx_packets++;
 562
 563         B43_WARN_ON(q->buffer_used > q->buffer_size);
 564         if (((q->buffer_size - q->buffer_used) < roundup(2 + 2 + 6, 4)) ||
 565             (q->free_packet_slots == 0)) {
 566                 /* The queue is full. */
 567                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
 568                 q->stopped = 1;
 569         }
 570
 571 out:
 572         return err;
 573 }
 574
 575 void b43_pio_handle_txstatus(struct b43_wldev *dev,
 576                              const struct b43_txstatus *status)
 577 {
 578         struct b43_pio_txqueue *q;
 579         struct b43_pio_txpacket *pack = NULL;
 580         unsigned int total_len;
 581         struct ieee80211_tx_info *info;
 582
 583         q = parse_cookie(dev, status->cookie, &pack);
 584         if (unlikely(!q))
 585                 return;
 586         B43_WARN_ON(!pack);
 587
 588         info = IEEE80211_SKB_CB(pack->skb);
 589
 590         b43_fill_txstatus_report(dev, info, status);
 591
 592         total_len = pack->skb->len + b43_txhdr_size(dev);
 593         total_len = roundup(total_len, 4);
 594         q->buffer_used -= total_len;
 595         q->free_packet_slots += 1;
 596
 597         ieee80211_tx_status(dev->wl->hw, pack->skb);
 598         pack->skb = NULL;
 599         list_add(&pack->list, &q->packets_list);
 600
 601         if (q->stopped) {
 602                 ieee80211_wake_queue(dev->wl->hw, q->queue_prio);
 603                 q->stopped = 0;
 604         }
 605 }
 606
 607 void b43_pio_get_tx_stats(struct b43_wldev *dev,
 608                           struct ieee80211_tx_queue_stats *stats)
 609 {
 610         const int nr_queues = dev->wl->hw->queues;
 611         struct b43_pio_txqueue *q;
 612         int i;
 613
 614         for (i = 0; i < nr_queues; i++) {
 615                 q = select_queue_by_priority(dev, i);
 616
 617                 stats[i].len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
 618                 stats[i].limit = B43_PIO_MAX_NR_TXPACKETS;
 619                 stats[i].count = q->nr_tx_packets;
 620         }
 621 }
 622
 623 /* Returns whether we should fetch another frame. */
 624 static bool pio_rx_frame(struct b43_pio_rxqueue *q)
 625 {
 626         struct b43_wldev *dev = q->dev;
 627         struct b43_wl *wl = dev->wl;
 628         u16 len;
 629         u32 macstat;
 630         unsigned int i, padding;
 631         struct sk_buff *skb;
 632         const char *err_msg = NULL;
 633         struct b43_rxhdr_fw4 *rxhdr =
 634                 (struct b43_rxhdr_fw4 *)wl->pio_scratchspace;
 635
 636         BUILD_BUG_ON(sizeof(wl->pio_scratchspace) < sizeof(*rxhdr));
 637         memset(rxhdr, 0, sizeof(*rxhdr));
 638
 639         /* Check if we have data and wait for it to get ready. */
 640         if (q->rev >= 8) {
 641                 u32 ctl;
 642
 643                 ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
 644                 if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
 645                         return 0;
 646                 b43_piorx_write32(q, B43_PIO8_RXCTL,
 647                                   B43_PIO8_RXCTL_FRAMERDY);
 648                 for (i = 0; i < 10; i++) {
 649                         ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
 650                         if (ctl & B43_PIO8_RXCTL_DATARDY)
 651                                 goto data_ready;
 652                         udelay(10);
 653                 }
 654         } else {
 655                 u16 ctl;
 656
 657                 ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
 658                 if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
 659                         return 0;
 660                 b43_piorx_write16(q, B43_PIO_RXCTL,
 661                                   B43_PIO_RXCTL_FRAMERDY);
 662                 for (i = 0; i < 10; i++) {
 663                         ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
 664                         if (ctl & B43_PIO_RXCTL_DATARDY)
 665                                 goto data_ready;
 666                         udelay(10);
 667                 }
 668         }
 669         b43dbg(q->dev->wl, "PIO RX timed out\n");
 670         return 1;
 671 data_ready:
 672
 673         /* Get the preamble (RX header) */
 674         if (q->rev >= 8) {
 675                 ssb_block_read(dev->dev, rxhdr, sizeof(*rxhdr),
 676                                q->mmio_base + B43_PIO8_RXDATA,
 677                                sizeof(u32));
 678         } else {
 679                 ssb_block_read(dev->dev, rxhdr, sizeof(*rxhdr),
 680                                q->mmio_base + B43_PIO_RXDATA,
 681                                sizeof(u16));
 682         }
 683         /* Sanity checks. */
 684         len = le16_to_cpu(rxhdr->frame_len);
 685         if (unlikely(len > 0x700)) {
 686                 err_msg = "len > 0x700";
 687                 goto rx_error;
 688         }
 689         if (unlikely(len == 0)) {
 690                 err_msg = "len == 0";
 691                 goto rx_error;
 692         }
 693
 694         macstat = le32_to_cpu(rxhdr->mac_status);
 695         if (macstat & B43_RX_MAC_FCSERR) {
 696                 if (!(q->dev->wl->filter_flags & FIF_FCSFAIL)) {
 697                         /* Drop frames with failed FCS. */
 698                         err_msg = "Frame FCS error";
 699                         goto rx_error;
 700                 }
 701         }
 702
 703         /* We always pad 2 bytes, as that's what upstream code expects
 704          * due to the RX-header being 30 bytes. In case the frame is
 705          * unaligned, we pad another 2 bytes. */
 706         padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
 707         skb = dev_alloc_skb(len + padding + 2);
 708         if (unlikely(!skb)) {
 709                 err_msg = "Out of memory";
 710                 goto rx_error;
 711         }
 712         skb_reserve(skb, 2);
 713         skb_put(skb, len + padding);
 714         if (q->rev >= 8) {
 715                 ssb_block_read(dev->dev, skb->data + padding, (len & ~3),
 716                                q->mmio_base + B43_PIO8_RXDATA,
 717                                sizeof(u32));
 718                 if (len & 3) {
 719                         u8 *tail = wl->pio_tailspace;
 720                         BUILD_BUG_ON(sizeof(wl->pio_tailspace) < 4);
 721
 722                         /* Read the last few bytes. */
 723                         ssb_block_read(dev->dev, tail, 4,
 724                                        q->mmio_base + B43_PIO8_RXDATA,
 725                                        sizeof(u32));
 726                         switch (len & 3) {
 727                         case 3:
 728                                 skb->data[len + padding - 3] = tail[0];
 729                                 skb->data[len + padding - 2] = tail[1];
 730                                 skb->data[len + padding - 1] = tail[2];
 731                                 break;
 732                         case 2:
 733                                 skb->data[len + padding - 2] = tail[0];
 734                                 skb->data[len + padding - 1] = tail[1];
 735                                 break;
 736                         case 1:
 737                                 skb->data[len + padding - 1] = tail[0];
 738                                 break;
 739                         }
 740                 }
 741         } else {
 742                 ssb_block_read(dev->dev, skb->data + padding, (len & ~1),
 743                                q->mmio_base + B43_PIO_RXDATA,
 744                                sizeof(u16));
 745                 if (len & 1) {
 746                         u8 *tail = wl->pio_tailspace;
 747                         BUILD_BUG_ON(sizeof(wl->pio_tailspace) < 2);
 748
 749                         /* Read the last byte. */
 750                         ssb_block_read(dev->dev, tail, 2,
 751                                        q->mmio_base + B43_PIO_RXDATA,
 752                                        sizeof(u16));
 753                         skb->data[len + padding - 1] = tail[0];
 754                 }
 755         }
 756
 757         b43_rx(q->dev, skb, rxhdr);
 758
 759         return 1;
 760
 761 rx_error:
 762         if (err_msg)
 763                 b43dbg(q->dev->wl, "PIO RX error: %s\n", err_msg);
 764         b43_piorx_write16(q, B43_PIO_RXCTL, B43_PIO_RXCTL_DATARDY);
 765         return 1;
 766 }
 767
 768 void b43_pio_rx(struct b43_pio_rxqueue *q)
 769 {
 770         unsigned int count = 0;
 771         bool stop;
 772
 773         while (1) {
 774                 stop = (pio_rx_frame(q) == 0);
 775                 if (stop)
 776                         break;
 777                 cond_resched();
 778                 if (WARN_ON_ONCE(++count > 10000))
 779                         break;
 780         }
 781 }
 782
 783 static void b43_pio_tx_suspend_queue(struct b43_pio_txqueue *q)
 784 {
 785         if (q->rev >= 8) {
 786                 b43_piotx_write32(q, B43_PIO8_TXCTL,
 787                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
 788                                   | B43_PIO8_TXCTL_SUSPREQ);
 789         } else {
 790                 b43_piotx_write16(q, B43_PIO_TXCTL,
 791                                   b43_piotx_read16(q, B43_PIO_TXCTL)
 792                                   | B43_PIO_TXCTL_SUSPREQ);
 793         }
 794 }
 795
 796 static void b43_pio_tx_resume_queue(struct b43_pio_txqueue *q)
 797 {
 798         if (q->rev >= 8) {
 799                 b43_piotx_write32(q, B43_PIO8_TXCTL,
 800                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
 801                                   & ~B43_PIO8_TXCTL_SUSPREQ);
 802         } else {
 803                 b43_piotx_write16(q, B43_PIO_TXCTL,
 804                                   b43_piotx_read16(q, B43_PIO_TXCTL)
 805                                   & ~B43_PIO_TXCTL_SUSPREQ);
 806         }
 807 }
 808
 809 void b43_pio_tx_suspend(struct b43_wldev *dev)
 810 {
 811         b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
 812         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BK);
 813         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BE);
 814         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VI);
 815         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VO);
 816         b43_pio_tx_suspend_queue(dev->pio.tx_queue_mcast);
 817 }
 818
 819 void b43_pio_tx_resume(struct b43_wldev *dev)
 820 {
 821         b43_pio_tx_resume_queue(dev->pio.tx_queue_mcast);
 822         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VO);
 823         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VI);
 824         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BE);
 825         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BK);
 826         b43_power_saving_ctl_bits(dev, 0);
 827 }