9c1397996e0a57753e97f372bce2ad1296abcbb9
[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         const u8 *data = _data;
335
336         ctl |= B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_WRITEHI;
337         b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
338
339         ssb_block_write(dev->dev, data, (data_len & ~1),
340                         q->mmio_base + B43_PIO_TXDATA,
341                         sizeof(u16));
342         if (data_len & 1) {
343                 u8 tail[2] = { 0, };
344
345                 /* Write the last byte. */
346                 ctl &= ~B43_PIO_TXCTL_WRITEHI;
347                 b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
348                 tail[0] = data[data_len - 1];
349                 ssb_block_write(dev->dev, 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         const u8 *data = _data;
385
386         ctl |= B43_PIO8_TXCTL_0_7 | B43_PIO8_TXCTL_8_15 |
387                B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_24_31;
388         b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
389
390         ssb_block_write(dev->dev, data, (data_len & ~3),
391                         q->mmio_base + B43_PIO8_TXDATA,
392                         sizeof(u32));
393         if (data_len & 3) {
394                 u8 tail[4] = { 0, };
395
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                         tail[0] = data[data_len - 3];
403                         tail[1] = data[data_len - 2];
404                         tail[2] = data[data_len - 1];
405                         break;
406                 case 2:
407                         ctl |= B43_PIO8_TXCTL_8_15;
408                         tail[0] = data[data_len - 2];
409                         tail[1] = data[data_len - 1];
410                         break;
411                 case 1:
412                         tail[0] = data[data_len - 1];
413                         break;
414                 }
415                 b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
416                 ssb_block_write(dev->dev, tail, 4,
417                                 q->mmio_base + B43_PIO8_TXDATA,
418                                 sizeof(u32));
419         }
420
421         return ctl;
422 }
423
424 static void pio_tx_frame_4byte_queue(struct b43_pio_txpacket *pack,
425                                      const u8 *hdr, unsigned int hdrlen)
426 {
427         struct b43_pio_txqueue *q = pack->queue;
428         const char *frame = pack->skb->data;
429         unsigned int frame_len = pack->skb->len;
430         u32 ctl;
431
432         ctl = b43_piotx_read32(q, B43_PIO8_TXCTL);
433         ctl |= B43_PIO8_TXCTL_FREADY;
434         ctl &= ~B43_PIO8_TXCTL_EOF;
435
436         /* Transfer the header data. */
437         ctl = tx_write_4byte_queue(q, ctl, hdr, hdrlen);
438         /* Transfer the frame data. */
439         ctl = tx_write_4byte_queue(q, ctl, frame, frame_len);
440
441         ctl |= B43_PIO8_TXCTL_EOF;
442         b43_piotx_write32(q, B43_PIO_TXCTL, ctl);
443 }
444
445 static int pio_tx_frame(struct b43_pio_txqueue *q,
446                         struct sk_buff *skb)
447 {
448         struct b43_pio_txpacket *pack;
449         struct b43_txhdr txhdr;
450         u16 cookie;
451         int err;
452         unsigned int hdrlen;
453         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
454
455         B43_WARN_ON(list_empty(&q->packets_list));
456         pack = list_entry(q->packets_list.next,
457                           struct b43_pio_txpacket, list);
458
459         cookie = generate_cookie(q, pack);
460         hdrlen = b43_txhdr_size(q->dev);
461         err = b43_generate_txhdr(q->dev, (u8 *)&txhdr, skb,
462                                  info, cookie);
463         if (err)
464                 return err;
465
466         if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
467                 /* Tell the firmware about the cookie of the last
468                  * mcast frame, so it can clear the more-data bit in it. */
469                 b43_shm_write16(q->dev, B43_SHM_SHARED,
470                                 B43_SHM_SH_MCASTCOOKIE, cookie);
471         }
472
473         pack->skb = skb;
474         if (q->rev >= 8)
475                 pio_tx_frame_4byte_queue(pack, (const u8 *)&txhdr, hdrlen);
476         else
477                 pio_tx_frame_2byte_queue(pack, (const u8 *)&txhdr, hdrlen);
478
479         /* Remove it from the list of available packet slots.
480          * It will be put back when we receive the status report. */
481         list_del(&pack->list);
482
483         /* Update the queue statistics. */
484         q->buffer_used += roundup(skb->len + hdrlen, 4);
485         q->free_packet_slots -= 1;
486
487         return 0;
488 }
489
490 int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb)
491 {
492         struct b43_pio_txqueue *q;
493         struct ieee80211_hdr *hdr;
494         unsigned int hdrlen, total_len;
495         int err = 0;
496         struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
497
498         hdr = (struct ieee80211_hdr *)skb->data;
499
500         if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
501                 /* The multicast queue will be sent after the DTIM. */
502                 q = dev->pio.tx_queue_mcast;
503                 /* Set the frame More-Data bit. Ucode will clear it
504                  * for us on the last frame. */
505                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
506         } else {
507                 /* Decide by priority where to put this frame. */
508                 q = select_queue_by_priority(dev, skb_get_queue_mapping(skb));
509         }
510
511         hdrlen = b43_txhdr_size(dev);
512         total_len = roundup(skb->len + hdrlen, 4);
513
514         if (unlikely(total_len > q->buffer_size)) {
515                 err = -ENOBUFS;
516                 b43dbg(dev->wl, "PIO: TX packet longer than queue.\n");
517                 goto out;
518         }
519         if (unlikely(q->free_packet_slots == 0)) {
520                 err = -ENOBUFS;
521                 b43warn(dev->wl, "PIO: TX packet overflow.\n");
522                 goto out;
523         }
524         B43_WARN_ON(q->buffer_used > q->buffer_size);
525
526         if (total_len > (q->buffer_size - q->buffer_used)) {
527                 /* Not enough memory on the queue. */
528                 err = -EBUSY;
529                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
530                 q->stopped = 1;
531                 goto out;
532         }
533
534         /* Assign the queue number to the ring (if not already done before)
535          * so TX status handling can use it. The mac80211-queue to b43-queue
536          * mapping is static, so we don't need to store it per frame. */
537         q->queue_prio = skb_get_queue_mapping(skb);
538
539         err = pio_tx_frame(q, skb);
540         if (unlikely(err == -ENOKEY)) {
541                 /* Drop this packet, as we don't have the encryption key
542                  * anymore and must not transmit it unencrypted. */
543                 dev_kfree_skb_any(skb);
544                 err = 0;
545                 goto out;
546         }
547         if (unlikely(err)) {
548                 b43err(dev->wl, "PIO transmission failure\n");
549                 goto out;
550         }
551         q->nr_tx_packets++;
552
553         B43_WARN_ON(q->buffer_used > q->buffer_size);
554         if (((q->buffer_size - q->buffer_used) < roundup(2 + 2 + 6, 4)) ||
555             (q->free_packet_slots == 0)) {
556                 /* The queue is full. */
557                 ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
558                 q->stopped = 1;
559         }
560
561 out:
562         return err;
563 }
564
565 void b43_pio_handle_txstatus(struct b43_wldev *dev,
566                              const struct b43_txstatus *status)
567 {
568         struct b43_pio_txqueue *q;
569         struct b43_pio_txpacket *pack = NULL;
570         unsigned int total_len;
571         struct ieee80211_tx_info *info;
572
573         q = parse_cookie(dev, status->cookie, &pack);
574         if (unlikely(!q))
575                 return;
576         B43_WARN_ON(!pack);
577
578         info = IEEE80211_SKB_CB(pack->skb);
579
580         b43_fill_txstatus_report(dev, info, status);
581
582         total_len = pack->skb->len + b43_txhdr_size(dev);
583         total_len = roundup(total_len, 4);
584         q->buffer_used -= total_len;
585         q->free_packet_slots += 1;
586
587         ieee80211_tx_status(dev->wl->hw, pack->skb);
588         pack->skb = NULL;
589         list_add(&pack->list, &q->packets_list);
590
591         if (q->stopped) {
592                 ieee80211_wake_queue(dev->wl->hw, q->queue_prio);
593                 q->stopped = 0;
594         }
595 }
596
597 void b43_pio_get_tx_stats(struct b43_wldev *dev,
598                           struct ieee80211_tx_queue_stats *stats)
599 {
600         const int nr_queues = dev->wl->hw->queues;
601         struct b43_pio_txqueue *q;
602         int i;
603
604         for (i = 0; i < nr_queues; i++) {
605                 q = select_queue_by_priority(dev, i);
606
607                 stats[i].len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
608                 stats[i].limit = B43_PIO_MAX_NR_TXPACKETS;
609                 stats[i].count = q->nr_tx_packets;
610         }
611 }
612
613 /* Returns whether we should fetch another frame. */
614 static bool pio_rx_frame(struct b43_pio_rxqueue *q)
615 {
616         struct b43_wldev *dev = q->dev;
617         struct b43_rxhdr_fw4 rxhdr;
618         u16 len;
619         u32 macstat;
620         unsigned int i, padding;
621         struct sk_buff *skb;
622         const char *err_msg = NULL;
623
624         memset(&rxhdr, 0, sizeof(rxhdr));
625
626         /* Check if we have data and wait for it to get ready. */
627         if (q->rev >= 8) {
628                 u32 ctl;
629
630                 ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
631                 if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
632                         return 0;
633                 b43_piorx_write32(q, B43_PIO8_RXCTL,
634                                   B43_PIO8_RXCTL_FRAMERDY);
635                 for (i = 0; i < 10; i++) {
636                         ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
637                         if (ctl & B43_PIO8_RXCTL_DATARDY)
638                                 goto data_ready;
639                         udelay(10);
640                 }
641         } else {
642                 u16 ctl;
643
644                 ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
645                 if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
646                         return 0;
647                 b43_piorx_write16(q, B43_PIO_RXCTL,
648                                   B43_PIO_RXCTL_FRAMERDY);
649                 for (i = 0; i < 10; i++) {
650                         ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
651                         if (ctl & B43_PIO_RXCTL_DATARDY)
652                                 goto data_ready;
653                         udelay(10);
654                 }
655         }
656         b43dbg(q->dev->wl, "PIO RX timed out\n");
657         return 1;
658 data_ready:
659
660         /* Get the preamble (RX header) */
661         if (q->rev >= 8) {
662                 ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
663                                q->mmio_base + B43_PIO8_RXDATA,
664                                sizeof(u32));
665         } else {
666                 ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
667                                q->mmio_base + B43_PIO_RXDATA,
668                                sizeof(u16));
669         }
670         /* Sanity checks. */
671         len = le16_to_cpu(rxhdr.frame_len);
672         if (unlikely(len > 0x700)) {
673                 err_msg = "len > 0x700";
674                 goto rx_error;
675         }
676         if (unlikely(len == 0)) {
677                 err_msg = "len == 0";
678                 goto rx_error;
679         }
680
681         macstat = le32_to_cpu(rxhdr.mac_status);
682         if (macstat & B43_RX_MAC_FCSERR) {
683                 if (!(q->dev->wl->filter_flags & FIF_FCSFAIL)) {
684                         /* Drop frames with failed FCS. */
685                         err_msg = "Frame FCS error";
686                         goto rx_error;
687                 }
688         }
689
690         /* We always pad 2 bytes, as that's what upstream code expects
691          * due to the RX-header being 30 bytes. In case the frame is
692          * unaligned, we pad another 2 bytes. */
693         padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
694         skb = dev_alloc_skb(len + padding + 2);
695         if (unlikely(!skb)) {
696                 err_msg = "Out of memory";
697                 goto rx_error;
698         }
699         skb_reserve(skb, 2);
700         skb_put(skb, len + padding);
701         if (q->rev >= 8) {
702                 ssb_block_read(dev->dev, skb->data + padding, (len & ~3),
703                                q->mmio_base + B43_PIO8_RXDATA,
704                                sizeof(u32));
705                 if (len & 3) {
706                         u8 tail[4] = { 0, };
707
708                         /* Read the last few bytes. */
709                         ssb_block_read(dev->dev, tail, 4,
710                                        q->mmio_base + B43_PIO8_RXDATA,
711                                        sizeof(u32));
712                         switch (len & 3) {
713                         case 3:
714                                 skb->data[len + padding - 3] = tail[0];
715                                 skb->data[len + padding - 2] = tail[1];
716                                 skb->data[len + padding - 1] = tail[2];
717                                 break;
718                         case 2:
719                                 skb->data[len + padding - 2] = tail[0];
720                                 skb->data[len + padding - 1] = tail[1];
721                                 break;
722                         case 1:
723                                 skb->data[len + padding - 1] = tail[0];
724                                 break;
725                         }
726                 }
727         } else {
728                 ssb_block_read(dev->dev, skb->data + padding, (len & ~1),
729                                q->mmio_base + B43_PIO_RXDATA,
730                                sizeof(u16));
731                 if (len & 1) {
732                         u8 tail[2] = { 0, };
733
734                         /* Read the last byte. */
735                         ssb_block_read(dev->dev, tail, 2,
736                                        q->mmio_base + B43_PIO_RXDATA,
737                                        sizeof(u16));
738                         skb->data[len + padding - 1] = tail[0];
739                 }
740         }
741
742         b43_rx(q->dev, skb, &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 }