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