cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

pio.c (20099B)


      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
     24static 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
     43static
     44struct 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
     79static 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
    109static u16 pio_txqueue_offset(struct b43_wldev *dev)
    110{
    111	if (dev->dev->core_rev >= 11)
    112		return 0x18;
    113	return 0;
    114}
    115
    116static u16 pio_rxqueue_offset(struct b43_wldev *dev)
    117{
    118	if (dev->dev->core_rev >= 11)
    119		return 0x38;
    120	return 8;
    121}
    122
    123static 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
    159static 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
    178static 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
    192static 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
    201static 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
    219void 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
    235int 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;
    270out:
    271	return err;
    272
    273err_destroy_mcast:
    274	destroy_queue_tx(pio, tx_queue_mcast);
    275err_destroy_vo:
    276	destroy_queue_tx(pio, tx_queue_AC_VO);
    277err_destroy_vi:
    278	destroy_queue_tx(pio, tx_queue_AC_VI);
    279err_destroy_be:
    280	destroy_queue_tx(pio, tx_queue_AC_BE);
    281err_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. */
    287static 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
    317static 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
    349static 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
    370static 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
    419static 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
    440static 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
    489int 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
    559out:
    560	return err;
    561}
    562
    563void 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. */
    596static 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;
    653data_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
    751rx_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
    762void 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
    777static 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
    790static 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
    803void 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
    813void 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}