b43: do not stack-allocate pio rx/tx header and tail buffers
[linux-2.6.git] / 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                 /* Write the last byte. */
345                 ctl &= ~B43_PIO_TXCTL_WRITEHI;
346                 b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
347                 wl->tx_tail[0] = data[data_len - 1];
348                 wl->tx_tail[1] = 0;
349                 ssb_block_write(dev->dev, wl->tx_tail, 2,
350                                 q->mmio_base + B43_PIO_TXDATA,
351                                 sizeof(u16));
352         }
353
354         return ctl;
355 }
356
357 static void pio_tx_frame_2byte_queue(struct b43_pio_txpacket *pack,
358                                      const u8 *hdr, unsigned int hdrlen)
359 {
360         struct b43_pio_txqueue *q = pack->queue;
361         const char *frame = pack->skb->data;
362         unsigned int frame_len = pack->skb->len;
363         u16 ctl;
364
365         ctl = b43_piotx_read16(q, B43_PIO_TXCTL);
366         ctl |= B43_PIO_TXCTL_FREADY;
367         ctl &= ~B43_PIO_TXCTL_EOF;
368
369         /* Transfer the header data. */
370         ctl = tx_write_2byte_queue(q, ctl, hdr, hdrlen);
371         /* Transfer the frame data. */
372         ctl = tx_write_2byte_queue(q, ctl, frame, frame_len);
373
374         ctl |= B43_PIO_TXCTL_EOF;
375         b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
376 }
377
378 static u32 tx_write_4byte_queue(struct b43_pio_txqueue *q,
379                                 u32 ctl,
380                                 const void *_data,
381                                 unsigned int data_len)
382 {
383         struct b43_wldev *dev = q->dev;
384         struct b43_wl *wl = dev->wl;
385         const u8 *data = _data;
386
387         ctl |= B43_PIO8_TXCTL_0_7 | B43_PIO8_TXCTL_8_15 |
388                B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_24_31;
389         b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
390
391         ssb_block_write(dev->dev, data, (data_len & ~3),
392                         q->mmio_base + B43_PIO8_TXDATA,
393                         sizeof(u32));
394         if (data_len & 3) {
395                 wl->tx_tail[3] = 0;
396                 /* Write the last few bytes. */
397                 ctl &= ~(B43_PIO8_TXCTL_8_15 | B43_PIO8_TXCTL_16_23 |
398                          B43_PIO8_TXCTL_24_31);
399                 switch (data_len & 3) {
400                 case 3:
401                         ctl |= B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_8_15;
402                         wl->tx_tail[0] = data[data_len - 3];
403                         wl->tx_tail[1] = data[data_len - 2];
404                         wl->tx_tail[2] = data[data_len - 1];
405                         break;
406                 case 2:
407                         ctl |= B43_PIO8_TXCTL_8_15;
408                         wl->tx_tail[0] = data[data_len - 2];
409                         wl->tx_tail[1] = data[data_len - 1];
410                         wl->tx_tail[2] = 0;
411                         break;
412                 case 1:
413                         wl->tx_tail[0] = data[data_len - 1];
414                         wl->tx_tail[1] = 0;
415                         wl->tx_tail[2] = 0;
416                         break;
417                 }
418                 b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
419                 ssb_block_write(dev->dev, wl->tx_tail, 4,
420                                 q->mmio_base + B43_PIO8_TXDATA,
421                                 sizeof(u32));
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_wldev *dev = q->dev;
452         struct b43_wl *wl = dev->wl;
453         struct b43_pio_txpacket *pack;
454         u16 cookie;
455         int err;
456         unsigned int hdrlen;
457         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
458
459         B43_WARN_ON(list_empty(&q->packets_list));
460         pack = list_entry(q->packets_list.next,
461                           struct b43_pio_txpacket, list);
462
463         cookie = generate_cookie(q, pack);
464         hdrlen = b43_txhdr_size(dev);
465         err = b43_generate_txhdr(dev, (u8 *)&wl->txhdr, skb,
466                                  info, cookie);
467         if (err)
468                 return err;
469
470         if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
471                 /* Tell the firmware about the cookie of the last
472                  * mcast frame, so it can clear the more-data bit in it. */
473                 b43_shm_write16(dev, B43_SHM_SHARED,
474                                 B43_SHM_SH_MCASTCOOKIE, cookie);
475         }
476
477         pack->skb = skb;
478         if (q->rev >= 8)
479                 pio_tx_frame_4byte_queue(pack, (const u8 *)&wl->txhdr, hdrlen);
480         else
481                 pio_tx_frame_2byte_queue(pack, (const u8 *)&wl->txhdr, hdrlen);
482
483         /* Remove it from the list of available packet slots.
484          * It will be put back when we receive the status report. */
485         list_del(&pack->list);
486
487         /* Update the queue statistics. */
488         q->buffer_used += roundup(skb->len + hdrlen, 4);
489         q->free_packet_slots -= 1;
490
491         return 0;
492 }
493
494 int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb)
495 {
496         struct b43_pio_txqueue *q;
497         struct ieee80211_hdr *hdr;
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         hdrlen = b43_txhdr_size(dev);
516         total_len = roundup(skb->len + hdrlen, 4);
517
518         if (unlikely(total_len > q->buffer_size)) {
519                 err = -ENOBUFS;
520                 b43dbg(dev->wl, "PIO: TX packet longer than queue.\n");
521                 goto out;
522         }
523         if (unlikely(q->free_packet_slots == 0)) {
524                 err = -ENOBUFS;
525                 b43warn(dev->wl, "PIO: TX packet overflow.\n");
526                 goto out;
527         }
528         B43_WARN_ON(q->buffer_used > q->buffer_size);
529
530         if (total_len > (q->buffer_size - q->buffer_used)) {
531                 /* Not enough memory on the queue. */
532                 err = -EBUSY;
533                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
534                 q->stopped = 1;
535                 goto out;
536         }
537
538         /* Assign the queue number to the ring (if not already done before)
539          * so TX status handling can use it. The mac80211-queue to b43-queue
540          * mapping is static, so we don't need to store it per frame. */
541         q->queue_prio = skb_get_queue_mapping(skb);
542
543         err = pio_tx_frame(q, skb);
544         if (unlikely(err == -ENOKEY)) {
545                 /* Drop this packet, as we don't have the encryption key
546                  * anymore and must not transmit it unencrypted. */
547                 dev_kfree_skb_any(skb);
548                 err = 0;
549                 goto out;
550         }
551         if (unlikely(err)) {
552                 b43err(dev->wl, "PIO transmission failure\n");
553                 goto out;
554         }
555         q->nr_tx_packets++;
556
557         B43_WARN_ON(q->buffer_used > q->buffer_size);
558         if (((q->buffer_size - q->buffer_used) < roundup(2 + 2 + 6, 4)) ||
559             (q->free_packet_slots == 0)) {
560                 /* The queue is full. */
561                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
562                 q->stopped = 1;
563         }
564
565 out:
566         return err;
567 }
568
569 void b43_pio_handle_txstatus(struct b43_wldev *dev,
570                              const struct b43_txstatus *status)
571 {
572         struct b43_pio_txqueue *q;
573         struct b43_pio_txpacket *pack = NULL;
574         unsigned int total_len;
575         struct ieee80211_tx_info *info;
576
577         q = parse_cookie(dev, status->cookie, &pack);
578         if (unlikely(!q))
579                 return;
580         B43_WARN_ON(!pack);
581
582         info = IEEE80211_SKB_CB(pack->skb);
583
584         b43_fill_txstatus_report(dev, info, status);
585
586         total_len = pack->skb->len + b43_txhdr_size(dev);
587         total_len = roundup(total_len, 4);
588         q->buffer_used -= total_len;
589         q->free_packet_slots += 1;
590
591         ieee80211_tx_status(dev->wl->hw, pack->skb);
592         pack->skb = NULL;
593         list_add(&pack->list, &q->packets_list);
594
595         if (q->stopped) {
596                 ieee80211_wake_queue(dev->wl->hw, q->queue_prio);
597                 q->stopped = 0;
598         }
599 }
600
601 void b43_pio_get_tx_stats(struct b43_wldev *dev,
602                           struct ieee80211_tx_queue_stats *stats)
603 {
604         const int nr_queues = dev->wl->hw->queues;
605         struct b43_pio_txqueue *q;
606         int i;
607
608         for (i = 0; i < nr_queues; i++) {
609                 q = select_queue_by_priority(dev, i);
610
611                 stats[i].len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
612                 stats[i].limit = B43_PIO_MAX_NR_TXPACKETS;
613                 stats[i].count = q->nr_tx_packets;
614         }
615 }
616
617 /* Returns whether we should fetch another frame. */
618 static bool pio_rx_frame(struct b43_pio_rxqueue *q)
619 {
620         struct b43_wldev *dev = q->dev;
621         struct b43_wl *wl = dev->wl;
622         u16 len;
623         u32 macstat;
624         unsigned int i, padding;
625         struct sk_buff *skb;
626         const char *err_msg = NULL;
627
628         memset(&wl->rxhdr, 0, sizeof(wl->rxhdr));
629
630         /* Check if we have data and wait for it to get ready. */
631         if (q->rev >= 8) {
632                 u32 ctl;
633
634                 ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
635                 if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
636                         return 0;
637                 b43_piorx_write32(q, B43_PIO8_RXCTL,
638                                   B43_PIO8_RXCTL_FRAMERDY);
639                 for (i = 0; i < 10; i++) {
640                         ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
641                         if (ctl & B43_PIO8_RXCTL_DATARDY)
642                                 goto data_ready;
643                         udelay(10);
644                 }
645         } else {
646                 u16 ctl;
647
648                 ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
649                 if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
650                         return 0;
651                 b43_piorx_write16(q, B43_PIO_RXCTL,
652                                   B43_PIO_RXCTL_FRAMERDY);
653                 for (i = 0; i < 10; i++) {
654                         ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
655                         if (ctl & B43_PIO_RXCTL_DATARDY)
656                                 goto data_ready;
657                         udelay(10);
658                 }
659         }
660         b43dbg(q->dev->wl, "PIO RX timed out\n");
661         return 1;
662 data_ready:
663
664         /* Get the preamble (RX header) */
665         if (q->rev >= 8) {
666                 ssb_block_read(dev->dev, &wl->rxhdr, sizeof(wl->rxhdr),
667                                q->mmio_base + B43_PIO8_RXDATA,
668                                sizeof(u32));
669         } else {
670                 ssb_block_read(dev->dev, &wl->rxhdr, sizeof(wl->rxhdr),
671                                q->mmio_base + B43_PIO_RXDATA,
672                                sizeof(u16));
673         }
674         /* Sanity checks. */
675         len = le16_to_cpu(wl->rxhdr.frame_len);
676         if (unlikely(len > 0x700)) {
677                 err_msg = "len > 0x700";
678                 goto rx_error;
679         }
680         if (unlikely(len == 0)) {
681                 err_msg = "len == 0";
682                 goto rx_error;
683         }
684
685         macstat = le32_to_cpu(wl->rxhdr.mac_status);
686         if (macstat & B43_RX_MAC_FCSERR) {
687                 if (!(q->dev->wl->filter_flags & FIF_FCSFAIL)) {
688                         /* Drop frames with failed FCS. */
689                         err_msg = "Frame FCS error";
690                         goto rx_error;
691                 }
692         }
693
694         /* We always pad 2 bytes, as that's what upstream code expects
695          * due to the RX-header being 30 bytes. In case the frame is
696          * unaligned, we pad another 2 bytes. */
697         padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
698         skb = dev_alloc_skb(len + padding + 2);
699         if (unlikely(!skb)) {
700                 err_msg = "Out of memory";
701                 goto rx_error;
702         }
703         skb_reserve(skb, 2);
704         skb_put(skb, len + padding);
705         if (q->rev >= 8) {
706                 ssb_block_read(dev->dev, skb->data + padding, (len & ~3),
707                                q->mmio_base + B43_PIO8_RXDATA,
708                                sizeof(u32));
709                 if (len & 3) {
710                         /* Read the last few bytes. */
711                         ssb_block_read(dev->dev, wl->rx_tail, 4,
712                                        q->mmio_base + B43_PIO8_RXDATA,
713                                        sizeof(u32));
714                         switch (len & 3) {
715                         case 3:
716                                 skb->data[len + padding - 3] = wl->rx_tail[0];
717                                 skb->data[len + padding - 2] = wl->rx_tail[1];
718                                 skb->data[len + padding - 1] = wl->rx_tail[2];
719                                 break;
720                         case 2:
721                                 skb->data[len + padding - 2] = wl->rx_tail[0];
722                                 skb->data[len + padding - 1] = wl->rx_tail[1];
723                                 break;
724                         case 1:
725                                 skb->data[len + padding - 1] = wl->rx_tail[0];
726                                 break;
727                         }
728                 }
729         } else {
730                 ssb_block_read(dev->dev, skb->data + padding, (len & ~1),
731                                q->mmio_base + B43_PIO_RXDATA,
732                                sizeof(u16));
733                 if (len & 1) {
734                         /* Read the last byte. */
735                         ssb_block_read(dev->dev, wl->rx_tail, 2,
736                                        q->mmio_base + B43_PIO_RXDATA,
737                                        sizeof(u16));
738                         skb->data[len + padding - 1] = wl->rx_tail[0];
739                 }
740         }
741
742         b43_rx(q->dev, skb, &wl->rxhdr);
743
744         return 1;
745
746 rx_error:
747         if (err_msg)
748                 b43dbg(q->dev->wl, "PIO RX error: %s\n", err_msg);
749         b43_piorx_write16(q, B43_PIO_RXCTL, B43_PIO_RXCTL_DATARDY);
750         return 1;
751 }
752
753 void b43_pio_rx(struct b43_pio_rxqueue *q)
754 {
755         unsigned int count = 0;
756         bool stop;
757
758         while (1) {
759                 stop = (pio_rx_frame(q) == 0);
760                 if (stop)
761                         break;
762                 cond_resched();
763                 if (WARN_ON_ONCE(++count > 10000))
764                         break;
765         }
766 }
767
768 static void b43_pio_tx_suspend_queue(struct b43_pio_txqueue *q)
769 {
770         if (q->rev >= 8) {
771                 b43_piotx_write32(q, B43_PIO8_TXCTL,
772                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
773                                   | B43_PIO8_TXCTL_SUSPREQ);
774         } else {
775                 b43_piotx_write16(q, B43_PIO_TXCTL,
776                                   b43_piotx_read16(q, B43_PIO_TXCTL)
777                                   | B43_PIO_TXCTL_SUSPREQ);
778         }
779 }
780
781 static void b43_pio_tx_resume_queue(struct b43_pio_txqueue *q)
782 {
783         if (q->rev >= 8) {
784                 b43_piotx_write32(q, B43_PIO8_TXCTL,
785                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
786                                   & ~B43_PIO8_TXCTL_SUSPREQ);
787         } else {
788                 b43_piotx_write16(q, B43_PIO_TXCTL,
789                                   b43_piotx_read16(q, B43_PIO_TXCTL)
790                                   & ~B43_PIO_TXCTL_SUSPREQ);
791         }
792 }
793
794 void b43_pio_tx_suspend(struct b43_wldev *dev)
795 {
796         b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
797         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BK);
798         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BE);
799         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VI);
800         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VO);
801         b43_pio_tx_suspend_queue(dev->pio.tx_queue_mcast);
802 }
803
804 void b43_pio_tx_resume(struct b43_wldev *dev)
805 {
806         b43_pio_tx_resume_queue(dev->pio.tx_queue_mcast);
807         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VO);
808         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VI);
809         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BE);
810         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BK);
811         b43_power_saving_ctl_bits(dev, 0);
812 }