Merge master.kernel.org:/pub/scm/linux/kernel/git/torvalds/linux-2.6 into next
[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         memset(&info->status, 0, sizeof(info->status));
591
592         b43_fill_txstatus_report(info, status);
593
594         total_len = pack->skb->len + b43_txhdr_size(dev);
595         total_len = roundup(total_len, 4);
596         q->buffer_used -= total_len;
597         q->free_packet_slots += 1;
598
599         ieee80211_tx_status_irqsafe(dev->wl->hw, pack->skb);
600         pack->skb = NULL;
601         list_add(&pack->list, &q->packets_list);
602
603         if (q->stopped) {
604                 ieee80211_wake_queue(dev->wl->hw, q->queue_prio);
605                 q->stopped = 0;
606         }
607
608         spin_unlock(&q->lock);
609 }
610
611 void b43_pio_get_tx_stats(struct b43_wldev *dev,
612                           struct ieee80211_tx_queue_stats *stats)
613 {
614         const int nr_queues = dev->wl->hw->queues;
615         struct b43_pio_txqueue *q;
616         unsigned long flags;
617         int i;
618
619         for (i = 0; i < nr_queues; i++) {
620                 q = select_queue_by_priority(dev, i);
621
622                 spin_lock_irqsave(&q->lock, flags);
623                 stats[i].len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
624                 stats[i].limit = B43_PIO_MAX_NR_TXPACKETS;
625                 stats[i].count = q->nr_tx_packets;
626                 spin_unlock_irqrestore(&q->lock, flags);
627         }
628 }
629
630 /* Returns whether we should fetch another frame. */
631 static bool pio_rx_frame(struct b43_pio_rxqueue *q)
632 {
633         struct b43_wldev *dev = q->dev;
634         struct b43_rxhdr_fw4 rxhdr;
635         u16 len;
636         u32 macstat;
637         unsigned int i, padding;
638         struct sk_buff *skb;
639         const char *err_msg = NULL;
640
641         memset(&rxhdr, 0, sizeof(rxhdr));
642
643         /* Check if we have data and wait for it to get ready. */
644         if (q->rev >= 8) {
645                 u32 ctl;
646
647                 ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
648                 if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
649                         return 0;
650                 b43_piorx_write32(q, B43_PIO8_RXCTL,
651                                   B43_PIO8_RXCTL_FRAMERDY);
652                 for (i = 0; i < 10; i++) {
653                         ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
654                         if (ctl & B43_PIO8_RXCTL_DATARDY)
655                                 goto data_ready;
656                         udelay(10);
657                 }
658         } else {
659                 u16 ctl;
660
661                 ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
662                 if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
663                         return 0;
664                 b43_piorx_write16(q, B43_PIO_RXCTL,
665                                   B43_PIO_RXCTL_FRAMERDY);
666                 for (i = 0; i < 10; i++) {
667                         ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
668                         if (ctl & B43_PIO_RXCTL_DATARDY)
669                                 goto data_ready;
670                         udelay(10);
671                 }
672         }
673         b43dbg(q->dev->wl, "PIO RX timed out\n");
674         return 1;
675 data_ready:
676
677         /* Get the preamble (RX header) */
678         if (q->rev >= 8) {
679                 ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
680                                q->mmio_base + B43_PIO8_RXDATA,
681                                sizeof(u32));
682         } else {
683                 ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
684                                q->mmio_base + B43_PIO_RXDATA,
685                                sizeof(u16));
686         }
687         /* Sanity checks. */
688         len = le16_to_cpu(rxhdr.frame_len);
689         if (unlikely(len > 0x700)) {
690                 err_msg = "len > 0x700";
691                 goto rx_error;
692         }
693         if (unlikely(len == 0)) {
694                 err_msg = "len == 0";
695                 goto rx_error;
696         }
697
698         macstat = le32_to_cpu(rxhdr.mac_status);
699         if (macstat & B43_RX_MAC_FCSERR) {
700                 if (!(q->dev->wl->filter_flags & FIF_FCSFAIL)) {
701                         /* Drop frames with failed FCS. */
702                         err_msg = "Frame FCS error";
703                         goto rx_error;
704                 }
705         }
706
707         /* We always pad 2 bytes, as that's what upstream code expects
708          * due to the RX-header being 30 bytes. In case the frame is
709          * unaligned, we pad another 2 bytes. */
710         padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
711         skb = dev_alloc_skb(len + padding + 2);
712         if (unlikely(!skb)) {
713                 err_msg = "Out of memory";
714                 goto rx_error;
715         }
716         skb_reserve(skb, 2);
717         skb_put(skb, len + padding);
718         if (q->rev >= 8) {
719                 ssb_block_read(dev->dev, skb->data + padding, (len & ~3),
720                                q->mmio_base + B43_PIO8_RXDATA,
721                                sizeof(u32));
722                 if (len & 3) {
723                         u32 value;
724                         char *data;
725
726                         /* Read the last few bytes. */
727                         value = b43_piorx_read32(q, B43_PIO8_RXDATA);
728                         data = &(skb->data[len + padding - 1]);
729                         switch (len & 3) {
730                         case 3:
731                                 *data = (value >> 16);
732                                 data--;
733                         case 2:
734                                 *data = (value >> 8);
735                                 data--;
736                         case 1:
737                                 *data = value;
738                         }
739                 }
740         } else {
741                 ssb_block_read(dev->dev, skb->data + padding, (len & ~1),
742                                q->mmio_base + B43_PIO_RXDATA,
743                                sizeof(u16));
744                 if (len & 1) {
745                         u16 value;
746
747                         /* Read the last byte. */
748                         value = b43_piorx_read16(q, B43_PIO_RXDATA);
749                         skb->data[len + padding - 1] = value;
750                 }
751         }
752
753         b43_rx(q->dev, skb, &rxhdr);
754
755         return 1;
756
757 rx_error:
758         if (err_msg)
759                 b43dbg(q->dev->wl, "PIO RX error: %s\n", err_msg);
760         b43_piorx_write16(q, B43_PIO_RXCTL, B43_PIO_RXCTL_DATARDY);
761         return 1;
762 }
763
764 /* RX workqueue. We can sleep, yay! */
765 static void b43_pio_rx_work(struct work_struct *work)
766 {
767         struct b43_pio_rxqueue *q = container_of(work, struct b43_pio_rxqueue,
768                                                  rx_work);
769         unsigned int budget = 50;
770         bool stop;
771
772         do {
773                 spin_lock_irq(&q->lock);
774                 stop = (pio_rx_frame(q) == 0);
775                 spin_unlock_irq(&q->lock);
776                 cond_resched();
777                 if (stop)
778                         break;
779         } while (--budget);
780 }
781
782 /* Called with IRQs disabled. */
783 void b43_pio_rx(struct b43_pio_rxqueue *q)
784 {
785         /* Due to latency issues we must run the RX path in
786          * a workqueue to be able to schedule between packets. */
787         queue_work(q->dev->wl->hw->workqueue, &q->rx_work);
788 }
789
790 static void b43_pio_tx_suspend_queue(struct b43_pio_txqueue *q)
791 {
792         unsigned long flags;
793
794         spin_lock_irqsave(&q->lock, flags);
795         if (q->rev >= 8) {
796                 b43_piotx_write32(q, B43_PIO8_TXCTL,
797                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
798                                   | B43_PIO8_TXCTL_SUSPREQ);
799         } else {
800                 b43_piotx_write16(q, B43_PIO_TXCTL,
801                                   b43_piotx_read16(q, B43_PIO_TXCTL)
802                                   | B43_PIO_TXCTL_SUSPREQ);
803         }
804         spin_unlock_irqrestore(&q->lock, flags);
805 }
806
807 static void b43_pio_tx_resume_queue(struct b43_pio_txqueue *q)
808 {
809         unsigned long flags;
810
811         spin_lock_irqsave(&q->lock, flags);
812         if (q->rev >= 8) {
813                 b43_piotx_write32(q, B43_PIO8_TXCTL,
814                                   b43_piotx_read32(q, B43_PIO8_TXCTL)
815                                   & ~B43_PIO8_TXCTL_SUSPREQ);
816         } else {
817                 b43_piotx_write16(q, B43_PIO_TXCTL,
818                                   b43_piotx_read16(q, B43_PIO_TXCTL)
819                                   & ~B43_PIO_TXCTL_SUSPREQ);
820         }
821         spin_unlock_irqrestore(&q->lock, flags);
822 }
823
824 void b43_pio_tx_suspend(struct b43_wldev *dev)
825 {
826         b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
827         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BK);
828         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BE);
829         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VI);
830         b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VO);
831         b43_pio_tx_suspend_queue(dev->pio.tx_queue_mcast);
832 }
833
834 void b43_pio_tx_resume(struct b43_wldev *dev)
835 {
836         b43_pio_tx_resume_queue(dev->pio.tx_queue_mcast);
837         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VO);
838         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VI);
839         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BE);
840         b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BK);
841         b43_power_saving_ctl_bits(dev, 0);
842 }