git.oblomov.eu Git - linux-2.6/blob - drivers/net/wireless/b43legacy/pio.c

   1 /*
   2
   3   Broadcom B43legacy wireless driver
   4
   5   PIO Transmission
   6
   7   Copyright (c) 2005 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 "b43legacy.h"
  27 #include "pio.h"
  28 #include "main.h"
  29 #include "xmit.h"
  30
  31 #include <linux/delay.h>
  32
  33
  34 static void tx_start(struct b43legacy_pioqueue *queue)
  35 {
  36         b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
  37                             B43legacy_PIO_TXCTL_INIT);
  38 }
  39
  40 static void tx_octet(struct b43legacy_pioqueue *queue,
  41                      u8 octet)
  42 {
  43         if (queue->need_workarounds) {
  44                 b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, octet);
  45                 b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
  46                                     B43legacy_PIO_TXCTL_WRITELO);
  47         } else {
  48                 b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
  49                                     B43legacy_PIO_TXCTL_WRITELO);
  50                 b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, octet);
  51         }
  52 }
  53
  54 static u16 tx_get_next_word(const u8 *txhdr,
  55                             const u8 *packet,
  56                             size_t txhdr_size,
  57                             unsigned int *pos)
  58 {
  59         const u8 *source;
  60         unsigned int i = *pos;
  61         u16 ret;
  62
  63         if (i < txhdr_size)
  64                 source = txhdr;
  65         else {
  66                 source = packet;
  67                 i -= txhdr_size;
  68         }
  69         ret = le16_to_cpu(*((__le16 *)(source + i)));
  70         *pos += 2;
  71
  72         return ret;
  73 }
  74
  75 static void tx_data(struct b43legacy_pioqueue *queue,
  76                     u8 *txhdr,
  77                     const u8 *packet,
  78                     unsigned int octets)
  79 {
  80         u16 data;
  81         unsigned int i = 0;
  82
  83         if (queue->need_workarounds) {
  84                 data = tx_get_next_word(txhdr, packet,
  85                                         sizeof(struct b43legacy_txhdr_fw3), &i);
  86                 b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, data);
  87         }
  88         b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
  89                             B43legacy_PIO_TXCTL_WRITELO |
  90                             B43legacy_PIO_TXCTL_WRITEHI);
  91         while (i < octets - 1) {
  92                 data = tx_get_next_word(txhdr, packet,
  93                                         sizeof(struct b43legacy_txhdr_fw3), &i);
  94                 b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, data);
  95         }
  96         if (octets % 2)
  97                 tx_octet(queue, packet[octets -
  98                          sizeof(struct b43legacy_txhdr_fw3) - 1]);
  99 }
 100
 101 static void tx_complete(struct b43legacy_pioqueue *queue,
 102                         struct sk_buff *skb)
 103 {
 104         if (queue->need_workarounds) {
 105                 b43legacy_pio_write(queue, B43legacy_PIO_TXDATA,
 106                                     skb->data[skb->len - 1]);
 107                 b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
 108                                     B43legacy_PIO_TXCTL_WRITELO |
 109                                     B43legacy_PIO_TXCTL_COMPLETE);
 110         } else
 111                 b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
 112                                     B43legacy_PIO_TXCTL_COMPLETE);
 113 }
 114
 115 static u16 generate_cookie(struct b43legacy_pioqueue *queue,
 116                            struct b43legacy_pio_txpacket *packet)
 117 {
 118         u16 cookie = 0x0000;
 119         int packetindex;
 120
 121         /* We use the upper 4 bits for the PIO
 122          * controller ID and the lower 12 bits
 123          * for the packet index (in the cache).
 124          */
 125         switch (queue->mmio_base) {
 126         case B43legacy_MMIO_PIO1_BASE:
 127                 break;
 128         case B43legacy_MMIO_PIO2_BASE:
 129                 cookie = 0x1000;
 130                 break;
 131         case B43legacy_MMIO_PIO3_BASE:
 132                 cookie = 0x2000;
 133                 break;
 134         case B43legacy_MMIO_PIO4_BASE:
 135                 cookie = 0x3000;
 136                 break;
 137         default:
 138                 B43legacy_WARN_ON(1);
 139         }
 140         packetindex = pio_txpacket_getindex(packet);
 141         B43legacy_WARN_ON(!(((u16)packetindex & 0xF000) == 0x0000));
 142         cookie |= (u16)packetindex;
 143
 144         return cookie;
 145 }
 146
 147 static
 148 struct b43legacy_pioqueue *parse_cookie(struct b43legacy_wldev *dev,
 149                                         u16 cookie,
 150                                         struct b43legacy_pio_txpacket **packet)
 151 {
 152         struct b43legacy_pio *pio = &dev->pio;
 153         struct b43legacy_pioqueue *queue = NULL;
 154         int packetindex;
 155
 156         switch (cookie & 0xF000) {
 157         case 0x0000:
 158                 queue = pio->queue0;
 159                 break;
 160         case 0x1000:
 161                 queue = pio->queue1;
 162                 break;
 163         case 0x2000:
 164                 queue = pio->queue2;
 165                 break;
 166         case 0x3000:
 167                 queue = pio->queue3;
 168                 break;
 169         default:
 170                 B43legacy_WARN_ON(1);
 171         }
 172         packetindex = (cookie & 0x0FFF);
 173         B43legacy_WARN_ON(!(packetindex >= 0 && packetindex
 174                           < B43legacy_PIO_MAXTXPACKETS));
 175         *packet = &(queue->tx_packets_cache[packetindex]);
 176
 177         return queue;
 178 }
 179
 180 union txhdr_union {
 181         struct b43legacy_txhdr_fw3 txhdr_fw3;
 182 };
 183
 184 static void pio_tx_write_fragment(struct b43legacy_pioqueue *queue,
 185                                   struct sk_buff *skb,
 186                                   struct b43legacy_pio_txpacket *packet,
 187                                   size_t txhdr_size)
 188 {
 189         union txhdr_union txhdr_data;
 190         u8 *txhdr = NULL;
 191         unsigned int octets;
 192
 193         txhdr = (u8 *)(&txhdr_data.txhdr_fw3);
 194
 195         B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags != 0);
 196         b43legacy_generate_txhdr(queue->dev,
 197                                  txhdr, skb->data, skb->len,
 198                                  &packet->txstat.control,
 199                                  generate_cookie(queue, packet));
 200
 201         tx_start(queue);
 202         octets = skb->len + txhdr_size;
 203         if (queue->need_workarounds)
 204                 octets--;
 205         tx_data(queue, txhdr, (u8 *)skb->data, octets);
 206         tx_complete(queue, skb);
 207 }
 208
 209 static void free_txpacket(struct b43legacy_pio_txpacket *packet,
 210                           int irq_context)
 211 {
 212         struct b43legacy_pioqueue *queue = packet->queue;
 213
 214         if (packet->skb) {
 215                 if (irq_context)
 216                         dev_kfree_skb_irq(packet->skb);
 217                 else
 218                         dev_kfree_skb(packet->skb);
 219         }
 220         list_move(&packet->list, &queue->txfree);
 221         queue->nr_txfree++;
 222 }
 223
 224 static int pio_tx_packet(struct b43legacy_pio_txpacket *packet)
 225 {
 226         struct b43legacy_pioqueue *queue = packet->queue;
 227         struct sk_buff *skb = packet->skb;
 228         u16 octets;
 229
 230         octets = (u16)skb->len + sizeof(struct b43legacy_txhdr_fw3);
 231         if (queue->tx_devq_size < octets) {
 232                 b43legacywarn(queue->dev->wl, "PIO queue too small. "
 233                         "Dropping packet.\n");
 234                 /* Drop it silently (return success) */
 235                 free_txpacket(packet, 1);
 236                 return 0;
 237         }
 238         B43legacy_WARN_ON(queue->tx_devq_packets >
 239                           B43legacy_PIO_MAXTXDEVQPACKETS);
 240         B43legacy_WARN_ON(queue->tx_devq_used > queue->tx_devq_size);
 241         /* Check if there is sufficient free space on the device
 242          * TX queue. If not, return and let the TX tasklet
 243          * retry later.
 244          */
 245         if (queue->tx_devq_packets == B43legacy_PIO_MAXTXDEVQPACKETS)
 246                 return -EBUSY;
 247         if (queue->tx_devq_used + octets > queue->tx_devq_size)
 248                 return -EBUSY;
 249         /* Now poke the device. */
 250         pio_tx_write_fragment(queue, skb, packet,
 251                               sizeof(struct b43legacy_txhdr_fw3));
 252
 253         /* Account for the packet size.
 254          * (We must not overflow the device TX queue)
 255          */
 256         queue->tx_devq_packets++;
 257         queue->tx_devq_used += octets;
 258
 259         /* Transmission started, everything ok, move the
 260          * packet to the txrunning list.
 261          */
 262         list_move_tail(&packet->list, &queue->txrunning);
 263
 264         return 0;
 265 }
 266
 267 static void tx_tasklet(unsigned long d)
 268 {
 269         struct b43legacy_pioqueue *queue = (struct b43legacy_pioqueue *)d;
 270         struct b43legacy_wldev *dev = queue->dev;
 271         unsigned long flags;
 272         struct b43legacy_pio_txpacket *packet, *tmp_packet;
 273         int err;
 274         u16 txctl;
 275
 276         spin_lock_irqsave(&dev->wl->irq_lock, flags);
 277         if (queue->tx_frozen)
 278                 goto out_unlock;
 279         txctl = b43legacy_pio_read(queue, B43legacy_PIO_TXCTL);
 280         if (txctl & B43legacy_PIO_TXCTL_SUSPEND)
 281                 goto out_unlock;
 282
 283         list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list) {
 284                 /* Try to transmit the packet. This can fail, if
 285                  * the device queue is full. In case of failure, the
 286                  * packet is left in the txqueue.
 287                  * If transmission succeed, the packet is moved to txrunning.
 288                  * If it is impossible to transmit the packet, it
 289                  * is dropped.
 290                  */
 291                 err = pio_tx_packet(packet);
 292                 if (err)
 293                         break;
 294         }
 295 out_unlock:
 296         spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
 297 }
 298
 299 static void setup_txqueues(struct b43legacy_pioqueue *queue)
 300 {
 301         struct b43legacy_pio_txpacket *packet;
 302         int i;
 303
 304         queue->nr_txfree = B43legacy_PIO_MAXTXPACKETS;
 305         for (i = 0; i < B43legacy_PIO_MAXTXPACKETS; i++) {
 306                 packet = &(queue->tx_packets_cache[i]);
 307
 308                 packet->queue = queue;
 309                 INIT_LIST_HEAD(&packet->list);
 310
 311                 list_add(&packet->list, &queue->txfree);
 312         }
 313 }
 314
 315 static
 316 struct b43legacy_pioqueue *b43legacy_setup_pioqueue(struct b43legacy_wldev *dev,
 317                                                     u16 pio_mmio_base)
 318 {
 319         struct b43legacy_pioqueue *queue;
 320         u32 value;
 321         u16 qsize;
 322
 323         queue = kzalloc(sizeof(*queue), GFP_KERNEL);
 324         if (!queue)
 325                 goto out;
 326
 327         queue->dev = dev;
 328         queue->mmio_base = pio_mmio_base;
 329         queue->need_workarounds = (dev->dev->id.revision < 3);
 330
 331         INIT_LIST_HEAD(&queue->txfree);
 332         INIT_LIST_HEAD(&queue->txqueue);
 333         INIT_LIST_HEAD(&queue->txrunning);
 334         tasklet_init(&queue->txtask, tx_tasklet,
 335                      (unsigned long)queue);
 336
 337         value = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
 338         value &= ~B43legacy_SBF_XFER_REG_BYTESWAP;
 339         b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, value);
 340
 341         qsize = b43legacy_read16(dev, queue->mmio_base
 342                                  + B43legacy_PIO_TXQBUFSIZE);
 343         if (qsize == 0) {
 344                 b43legacyerr(dev->wl, "This card does not support PIO "
 345                        "operation mode. Please use DMA mode "
 346                        "(module parameter pio=0).\n");
 347                 goto err_freequeue;
 348         }
 349         if (qsize <= B43legacy_PIO_TXQADJUST) {
 350                 b43legacyerr(dev->wl, "PIO tx device-queue too small (%u)\n",
 351                        qsize);
 352                 goto err_freequeue;
 353         }
 354         qsize -= B43legacy_PIO_TXQADJUST;
 355         queue->tx_devq_size = qsize;
 356
 357         setup_txqueues(queue);
 358
 359 out:
 360         return queue;
 361
 362 err_freequeue:
 363         kfree(queue);
 364         queue = NULL;
 365         goto out;
 366 }
 367
 368 static void cancel_transfers(struct b43legacy_pioqueue *queue)
 369 {
 370         struct b43legacy_pio_txpacket *packet, *tmp_packet;
 371
 372         tasklet_disable(&queue->txtask);
 373
 374         list_for_each_entry_safe(packet, tmp_packet, &queue->txrunning, list)
 375                 free_txpacket(packet, 0);
 376         list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list)
 377                 free_txpacket(packet, 0);
 378 }
 379
 380 static void b43legacy_destroy_pioqueue(struct b43legacy_pioqueue *queue)
 381 {
 382         if (!queue)
 383                 return;
 384
 385         cancel_transfers(queue);
 386         kfree(queue);
 387 }
 388
 389 void b43legacy_pio_free(struct b43legacy_wldev *dev)
 390 {
 391         struct b43legacy_pio *pio;
 392
 393         if (!b43legacy_using_pio(dev))
 394                 return;
 395         pio = &dev->pio;
 396
 397         b43legacy_destroy_pioqueue(pio->queue3);
 398         pio->queue3 = NULL;
 399         b43legacy_destroy_pioqueue(pio->queue2);
 400         pio->queue2 = NULL;
 401         b43legacy_destroy_pioqueue(pio->queue1);
 402         pio->queue1 = NULL;
 403         b43legacy_destroy_pioqueue(pio->queue0);
 404         pio->queue0 = NULL;
 405 }
 406
 407 int b43legacy_pio_init(struct b43legacy_wldev *dev)
 408 {
 409         struct b43legacy_pio *pio = &dev->pio;
 410         struct b43legacy_pioqueue *queue;
 411         int err = -ENOMEM;
 412
 413         queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO1_BASE);
 414         if (!queue)
 415                 goto out;
 416         pio->queue0 = queue;
 417
 418         queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO2_BASE);
 419         if (!queue)
 420                 goto err_destroy0;
 421         pio->queue1 = queue;
 422
 423         queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO3_BASE);
 424         if (!queue)
 425                 goto err_destroy1;
 426         pio->queue2 = queue;
 427
 428         queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO4_BASE);
 429         if (!queue)
 430                 goto err_destroy2;
 431         pio->queue3 = queue;
 432
 433         if (dev->dev->id.revision < 3)
 434                 dev->irq_savedstate |= B43legacy_IRQ_PIO_WORKAROUND;
 435
 436         b43legacydbg(dev->wl, "PIO initialized\n");
 437         err = 0;
 438 out:
 439         return err;
 440
 441 err_destroy2:
 442         b43legacy_destroy_pioqueue(pio->queue2);
 443         pio->queue2 = NULL;
 444 err_destroy1:
 445         b43legacy_destroy_pioqueue(pio->queue1);
 446         pio->queue1 = NULL;
 447 err_destroy0:
 448         b43legacy_destroy_pioqueue(pio->queue0);
 449         pio->queue0 = NULL;
 450         goto out;
 451 }
 452
 453 int b43legacy_pio_tx(struct b43legacy_wldev *dev,
 454                      struct sk_buff *skb,
 455                      struct ieee80211_tx_control *ctl)
 456 {
 457         struct b43legacy_pioqueue *queue = dev->pio.queue1;
 458         struct b43legacy_pio_txpacket *packet;
 459
 460         B43legacy_WARN_ON(queue->tx_suspended);
 461         B43legacy_WARN_ON(list_empty(&queue->txfree));
 462
 463         packet = list_entry(queue->txfree.next, struct b43legacy_pio_txpacket,
 464                             list);
 465         packet->skb = skb;
 466
 467         memset(&packet->txstat, 0, sizeof(packet->txstat));
 468         memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
 469
 470         list_move_tail(&packet->list, &queue->txqueue);
 471         queue->nr_txfree--;
 472         queue->nr_tx_packets++;
 473         B43legacy_WARN_ON(queue->nr_txfree >= B43legacy_PIO_MAXTXPACKETS);
 474
 475         tasklet_schedule(&queue->txtask);
 476
 477         return 0;
 478 }
 479
 480 void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
 481                                    const struct b43legacy_txstatus *status)
 482 {
 483         struct b43legacy_pioqueue *queue;
 484         struct b43legacy_pio_txpacket *packet;
 485
 486         queue = parse_cookie(dev, status->cookie, &packet);
 487         B43legacy_WARN_ON(!queue);
 488
 489         queue->tx_devq_packets--;
 490         queue->tx_devq_used -= (packet->skb->len +
 491                                 sizeof(struct b43legacy_txhdr_fw3));
 492
 493         if (status->acked)
 494                 packet->txstat.flags |= IEEE80211_TX_STATUS_ACK;
 495         packet->txstat.retry_count = status->frame_count - 1;
 496         ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb,
 497                                     &(packet->txstat));
 498         packet->skb = NULL;
 499
 500         free_txpacket(packet, 1);
 501         /* If there are packets on the txqueue, poke the tasklet
 502          * to transmit them.
 503          */
 504         if (!list_empty(&queue->txqueue))
 505                 tasklet_schedule(&queue->txtask);
 506 }
 507
 508 void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
 509                                 struct ieee80211_tx_queue_stats *stats)
 510 {
 511         struct b43legacy_pio *pio = &dev->pio;
 512         struct b43legacy_pioqueue *queue;
 513         struct ieee80211_tx_queue_stats_data *data;
 514
 515         queue = pio->queue1;
 516         data = &(stats->data[0]);
 517         data->len = B43legacy_PIO_MAXTXPACKETS - queue->nr_txfree;
 518         data->limit = B43legacy_PIO_MAXTXPACKETS;
 519         data->count = queue->nr_tx_packets;
 520 }
 521
 522 static void pio_rx_error(struct b43legacy_pioqueue *queue,
 523                          int clear_buffers,
 524                          const char *error)
 525 {
 526         int i;
 527
 528         b43legacyerr(queue->dev->wl, "PIO RX error: %s\n", error);
 529         b43legacy_pio_write(queue, B43legacy_PIO_RXCTL,
 530                             B43legacy_PIO_RXCTL_READY);
 531         if (clear_buffers) {
 532                 B43legacy_WARN_ON(queue->mmio_base != B43legacy_MMIO_PIO1_BASE);
 533                 for (i = 0; i < 15; i++) {
 534                         /* Dummy read. */
 535                         b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
 536                 }
 537         }
 538 }
 539
 540 void b43legacy_pio_rx(struct b43legacy_pioqueue *queue)
 541 {
 542         __le16 preamble[21] = { 0 };
 543         struct b43legacy_rxhdr_fw3 *rxhdr;
 544         u16 tmp;
 545         u16 len;
 546         u16 macstat;
 547         int i;
 548         int preamble_readwords;
 549         struct sk_buff *skb;
 550
 551         tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXCTL);
 552         if (!(tmp & B43legacy_PIO_RXCTL_DATAAVAILABLE))
 553                 return;
 554         b43legacy_pio_write(queue, B43legacy_PIO_RXCTL,
 555                             B43legacy_PIO_RXCTL_DATAAVAILABLE);
 556
 557         for (i = 0; i < 10; i++) {
 558                 tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXCTL);
 559                 if (tmp & B43legacy_PIO_RXCTL_READY)
 560                         goto data_ready;
 561                 udelay(10);
 562         }
 563         b43legacydbg(queue->dev->wl, "PIO RX timed out\n");
 564         return;
 565 data_ready:
 566
 567         len = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
 568         if (unlikely(len > 0x700)) {
 569                 pio_rx_error(queue, 0, "len > 0x700");
 570                 return;
 571         }
 572         if (unlikely(len == 0 && queue->mmio_base !=
 573                      B43legacy_MMIO_PIO4_BASE)) {
 574                 pio_rx_error(queue, 0, "len == 0");
 575                 return;
 576         }
 577         preamble[0] = cpu_to_le16(len);
 578         if (queue->mmio_base == B43legacy_MMIO_PIO4_BASE)
 579                 preamble_readwords = 14 / sizeof(u16);
 580         else
 581                 preamble_readwords = 18 / sizeof(u16);
 582         for (i = 0; i < preamble_readwords; i++) {
 583                 tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
 584                 preamble[i + 1] = cpu_to_le16(tmp);
 585         }
 586         rxhdr = (struct b43legacy_rxhdr_fw3 *)preamble;
 587         macstat = le16_to_cpu(rxhdr->mac_status);
 588         if (macstat & B43legacy_RX_MAC_FCSERR) {
 589                 pio_rx_error(queue,
 590                              (queue->mmio_base == B43legacy_MMIO_PIO1_BASE),
 591                              "Frame FCS error");
 592                 return;
 593         }
 594         if (queue->mmio_base == B43legacy_MMIO_PIO4_BASE) {
 595                 /* We received an xmit status. */
 596                 struct b43legacy_hwtxstatus *hw;
 597
 598                 hw = (struct b43legacy_hwtxstatus *)(preamble + 1);
 599                 b43legacy_handle_hwtxstatus(queue->dev, hw);
 600
 601                 return;
 602         }
 603
 604         skb = dev_alloc_skb(len);
 605         if (unlikely(!skb)) {
 606                 pio_rx_error(queue, 1, "OOM");
 607                 return;
 608         }
 609         skb_put(skb, len);
 610         for (i = 0; i < len - 1; i += 2) {
 611                 tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
 612                 *((__le16 *)(skb->data + i)) = cpu_to_le16(tmp);
 613         }
 614         if (len % 2) {
 615                 tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
 616                 skb->data[len - 1] = (tmp & 0x00FF);
 617         }
 618         b43legacy_rx(queue->dev, skb, rxhdr);
 619 }
 620
 621 void b43legacy_pio_tx_suspend(struct b43legacy_pioqueue *queue)
 622 {
 623         b43legacy_power_saving_ctl_bits(queue->dev, -1, 1);
 624         b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
 625                             b43legacy_pio_read(queue, B43legacy_PIO_TXCTL)
 626                             | B43legacy_PIO_TXCTL_SUSPEND);
 627 }
 628
 629 void b43legacy_pio_tx_resume(struct b43legacy_pioqueue *queue)
 630 {
 631         b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
 632                             b43legacy_pio_read(queue, B43legacy_PIO_TXCTL)
 633                             & ~B43legacy_PIO_TXCTL_SUSPEND);
 634         b43legacy_power_saving_ctl_bits(queue->dev, -1, -1);
 635         tasklet_schedule(&queue->txtask);
 636 }
 637
 638 void b43legacy_pio_freeze_txqueues(struct b43legacy_wldev *dev)
 639 {
 640         struct b43legacy_pio *pio;
 641
 642         B43legacy_WARN_ON(!b43legacy_using_pio(dev));
 643         pio = &dev->pio;
 644         pio->queue0->tx_frozen = 1;
 645         pio->queue1->tx_frozen = 1;
 646         pio->queue2->tx_frozen = 1;
 647         pio->queue3->tx_frozen = 1;
 648 }
 649
 650 void b43legacy_pio_thaw_txqueues(struct b43legacy_wldev *dev)
 651 {
 652         struct b43legacy_pio *pio;
 653
 654         B43legacy_WARN_ON(!b43legacy_using_pio(dev));
 655         pio = &dev->pio;
 656         pio->queue0->tx_frozen = 0;
 657         pio->queue1->tx_frozen = 0;
 658         pio->queue2->tx_frozen = 0;
 659         pio->queue3->tx_frozen = 0;
 660         if (!list_empty(&pio->queue0->txqueue))
 661                 tasklet_schedule(&pio->queue0->txtask);
 662         if (!list_empty(&pio->queue1->txqueue))
 663                 tasklet_schedule(&pio->queue1->txtask);
 664         if (!list_empty(&pio->queue2->txqueue))
 665                 tasklet_schedule(&pio->queue2->txtask);
 666         if (!list_empty(&pio->queue3->txqueue))
 667                 tasklet_schedule(&pio->queue3->txtask);
 668 }