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