brcmfmac: streamline SDIO read frame routine (4754fcee) · Commits · e / devices / android_kernel_oneplus_sm7250

drivers/net/wireless/brcm80211/brcmfmac/dhd_sdio.c

+233 −388

Original line number	Diff line number	Diff line
		@@ -482,6 +482,15 @@ struct sdpcm_shared_le {
		__le32 brpt_addr;
		};

		/* SDIO read frame info */
		struct brcmf_sdio_read {
		u8 seq_num;
		u8 channel;
		u16 len;
		u16 len_left;
		u16 len_nxtfrm;
		u8 dat_offset;
		};

		/* misc chip info needed by some of the routines */
		/* Private data for SDIO bus interaction */
		@@ -507,9 +516,11 @@ struct brcmf_sdio {

		u8 hdrbuf[MAX_HDR_READ + BRCMF_SDALIGN];
		u8 rxhdr; / Header of current rx frame (in hdrbuf) */
		u16 nextlen; /* Next Read Len from last header */
		u8 rx_seq; /* Receive sequence number (expected) */
		struct brcmf_sdio_read cur_read;
		/* info of current read frame */
		bool rxskip; /* Skip receive (awaiting NAK ACK) */
		bool rxpending; /* Data frame pending in dongle */

		uint rxbound; /* Rx frames to read before resched */
		uint txbound; /* Tx frames to send before resched */
		@@ -551,8 +562,6 @@ struct brcmf_sdio {
		bool rxflow_mode; /* Rx flow control mode */
		bool rxflow; /* Is rx flow control on */
		bool alp_only; /* Don't use HT clock (ALP only) */
		/* Field to decide if rx of control frames happen in rxbuf or lb-pool */
		bool usebufpool;

		u8 *ctrl_frame_buf;
		u32 ctrl_frame_len;
		@@ -655,15 +664,6 @@ w_sdreg32(struct brcmf_sdio *bus, u32 regval, u32 reg_offset)

		#define HOSTINTMASK (I_HMB_SW_MASK \| I_CHIPACTIVE)

		/* Packet free applicable unconditionally for sdio and sdspi.
		* Conditional if bufpool was present for gspi bus.
		*/
		static void brcmf_sdbrcm_pktfree2(struct brcmf_sdio bus, struct sk_buff pkt)
		{
		if (bus->usebufpool)
		brcmu_pkt_buf_free_skb(pkt);
		}

		/* Turn backplane clock on or off */
		static int brcmf_sdbrcm_htclk(struct brcmf_sdio *bus, bool on, bool pendok)
		{
		@@ -979,7 +979,7 @@ static void brcmf_sdbrcm_rxfail(struct brcmf_sdio *bus, bool abort, bool rtx)
		}

		/* Clear partial in any case */
		bus->nextlen = 0;
		bus->cur_read.len = 0;

		/* If we can't reach the device, signal failure */
		if (err)
		@@ -1031,6 +1031,96 @@ static void brcmf_sdbrcm_free_glom(struct brcmf_sdio *bus)
		}
		}

		static bool brcmf_sdio_hdparser(struct brcmf_sdio bus, u8 header,
		struct brcmf_sdio_read *rd)
		{
		u16 len, checksum;
		u8 rx_seq, fc, tx_seq_max;

		/*
		* 4 bytes hardware header (frame tag)
		* Byte 0~1: Frame length
		* Byte 2~3: Checksum, bit-wise inverse of frame length
		*/
		len = get_unaligned_le16(header);
		checksum = get_unaligned_le16(header + sizeof(u16));
		/* All zero means no more to read */
		if (!(len \| checksum)) {
		bus->rxpending = false;
		return false;
		}
		if ((u16)(~(len ^ checksum))) {
		brcmf_dbg(ERROR, "HW header checksum error\n");
		bus->sdcnt.rx_badhdr++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		return false;
		}
		if (len < SDPCM_HDRLEN) {
		brcmf_dbg(ERROR, "HW header length error\n");
		return false;
		}
		rd->len = len;

		/*
		* 8 bytes hardware header
		* Byte 0: Rx sequence number
		* Byte 1: 4 MSB Channel number, 4 LSB arbitrary flag
		* Byte 2: Length of next data frame
		* Byte 3: Data offset
		* Byte 4: Flow control bits
		* Byte 5: Maximum Sequence number allow for Tx
		* Byte 6~7: Reserved
		*/
		rx_seq = SDPCM_PACKET_SEQUENCE(&header[SDPCM_FRAMETAG_LEN]);
		rd->channel = SDPCM_PACKET_CHANNEL(&header[SDPCM_FRAMETAG_LEN]);
		if (len > MAX_RX_DATASZ && rd->channel != SDPCM_CONTROL_CHANNEL) {
		brcmf_dbg(ERROR, "HW header length too long\n");
		bus->sdiodev->bus_if->dstats.rx_errors++;
		bus->sdcnt.rx_toolong++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		rd->len = 0;
		return false;
		}
		rd->dat_offset = SDPCM_DOFFSET_VALUE(&header[SDPCM_FRAMETAG_LEN]);
		if (rd->dat_offset < SDPCM_HDRLEN \|\| rd->dat_offset > rd->len) {
		brcmf_dbg(ERROR, "seq %d: bad data offset\n", rx_seq);
		bus->sdcnt.rx_badhdr++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		rd->len = 0;
		return false;
		}
		if (rd->seq_num != rx_seq) {
		brcmf_dbg(ERROR, "seq %d: sequence number error, expect %d\n",
		rx_seq, rd->seq_num);
		bus->sdcnt.rx_badseq++;
		rd->seq_num = rx_seq;
		}
		rd->len_nxtfrm = header[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
		if (rd->len_nxtfrm << 4 > MAX_RX_DATASZ) {
		/* only warm for NON glom packet */
		if (rd->channel != SDPCM_GLOM_CHANNEL)
		brcmf_dbg(ERROR, "seq %d: next length error\n", rx_seq);
		rd->len_nxtfrm = 0;
		}
		fc = SDPCM_FCMASK_VALUE(&header[SDPCM_FRAMETAG_LEN]);
		if (bus->flowcontrol != fc) {
		if (~bus->flowcontrol & fc)
		bus->sdcnt.fc_xoff++;
		if (bus->flowcontrol & ~fc)
		bus->sdcnt.fc_xon++;
		bus->sdcnt.fc_rcvd++;
		bus->flowcontrol = fc;
		}
		tx_seq_max = SDPCM_WINDOW_VALUE(&header[SDPCM_FRAMETAG_LEN]);
		if ((u8)(tx_seq_max - bus->tx_seq) > 0x40) {
		brcmf_dbg(ERROR, "seq %d: max tx seq number error\n", rx_seq);
		tx_seq_max = bus->tx_seq + 2;
		}
		bus->tx_max = tx_seq_max;

		return true;
		}

		static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
		{
		u16 dlen, totlen;
		@@ -1045,6 +1135,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)

		int ifidx = 0;
		bool usechain = bus->use_rxchain;
		u16 next_len;

		/* If packets, issue read(s) and send up packet chain */
		/* Return sequence numbers consumed? */
		@@ -1108,10 +1199,10 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
		if (pnext) {
		brcmf_dbg(GLOM, "allocated %d-byte packet chain for %d subframes\n",
		totlen, num);
		if (BRCMF_GLOM_ON() && bus->nextlen &&
		totlen != bus->nextlen) {
		if (BRCMF_GLOM_ON() && bus->cur_read.len &&
		totlen != bus->cur_read.len) {
		brcmf_dbg(GLOM, "glomdesc mismatch: nextlen %d glomdesc %d rxseq %d\n",
		bus->nextlen, totlen, rxseq);
		bus->cur_read.len, totlen, rxseq);
		}
		pfirst = pnext = NULL;
		} else {
		@@ -1122,7 +1213,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
		/* Done with descriptor packet */
		brcmu_pkt_buf_free_skb(bus->glomd);
		bus->glomd = NULL;
		bus->nextlen = 0;
		bus->cur_read.len = 0;
		}

		/* Ok -- either we just generated a packet chain,
		@@ -1195,12 +1286,13 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)

		chan = SDPCM_PACKET_CHANNEL(&dptr[SDPCM_FRAMETAG_LEN]);
		seq = SDPCM_PACKET_SEQUENCE(&dptr[SDPCM_FRAMETAG_LEN]);
		bus->nextlen = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
		next_len = dptr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
		if ((next_len << 4) > MAX_RX_DATASZ) {
		brcmf_dbg(INFO, "nextlen too large (%d) seq %d\n",
		bus->nextlen, seq);
		bus->nextlen = 0;
		next_len, seq);
		next_len = 0;
		}
		bus->cur_read.len = next_len << 4;
		doff = SDPCM_DOFFSET_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);
		txmax = SDPCM_WINDOW_VALUE(&dptr[SDPCM_FRAMETAG_LEN]);

		@@ -1301,7 +1393,7 @@ static u8 brcmf_sdbrcm_rxglom(struct brcmf_sdio *bus, u8 rxseq)
		bus->sdcnt.rxglomfail++;
		brcmf_sdbrcm_free_glom(bus);
		}
		bus->nextlen = 0;
		bus->cur_read.len = 0;
		return 0;
		}

		@@ -1496,422 +1588,166 @@ static void brcmf_pad(struct brcmf_sdio bus, u16 pad, u16 *rdlen)
		}
		}

		static void
		brcmf_alloc_pkt_and_read(struct brcmf_sdio *bus, u16 rdlen,
		struct sk_buff pkt, u8 rxbuf)
		static uint brcmf_sdio_readframes(struct brcmf_sdio *bus, uint maxframes)
		{
		int sdret; /* Return code from calls */

		*pkt = brcmu_pkt_buf_get_skb(rdlen + BRCMF_SDALIGN);
		if (*pkt == NULL)
		return;

		pkt_align(*pkt, rdlen, BRCMF_SDALIGN);
		rxbuf = (u8 ) ((*pkt)->data);
		/* Read the entire frame */
		sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
		SDIO_FUNC_2, F2SYNC, *pkt);
		bus->sdcnt.f2rxdata++;

		if (sdret < 0) {
		brcmf_dbg(ERROR, "(nextlen): read %d bytes failed: %d\n",
		rdlen, sdret);
		brcmu_pkt_buf_free_skb(*pkt);
		bus->sdiodev->bus_if->dstats.rx_errors++;
		/* Force retry w/normal header read.
		* Don't attempt NAK for
		* gSPI
		*/
		brcmf_sdbrcm_rxfail(bus, true, true);
		*pkt = NULL;
		}
		}

		/* Checks the header */
		static int
		brcmf_check_rxbuf(struct brcmf_sdio bus, struct sk_buff pkt, u8 *rxbuf,
		u8 rxseq, u16 nextlen, u16 *len)
		{
		u16 check;
		bool len_consistent; /* Result of comparing readahead len and
		len from hw-hdr */

		memcpy(bus->rxhdr, rxbuf, SDPCM_HDRLEN);

		/* Extract hardware header fields */
		*len = get_unaligned_le16(bus->rxhdr);
		check = get_unaligned_le16(bus->rxhdr + sizeof(u16));

		/* All zeros means readahead info was bad */
		if (!(*len \| check)) {
		brcmf_dbg(INFO, "(nextlen): read zeros in HW header???\n");
		goto fail;
		}

		/* Validate check bytes */
		if ((u16)~(*len ^ check)) {
		brcmf_dbg(ERROR, "(nextlen): HW hdr error: nextlen/len/check 0x%04x/0x%04x/0x%04x\n",
		nextlen, *len, check);
		bus->sdcnt.rx_badhdr++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		goto fail;
		}

		/* Validate frame length */
		if (*len < SDPCM_HDRLEN) {
		brcmf_dbg(ERROR, "(nextlen): HW hdr length invalid: %d\n",
		*len);
		goto fail;
		}

		/* Check for consistency with readahead info */
		len_consistent = (nextlen != (roundup(*len, 16) >> 4));
		if (len_consistent) {
		/* Mismatch, force retry w/normal
		header (may be >4K) */
		brcmf_dbg(ERROR, "(nextlen): mismatch, nextlen %d len %d rnd %d; expected rxseq %d\n",
		nextlen, len, roundup(len, 16),
		rxseq);
		brcmf_sdbrcm_rxfail(bus, true, true);
		goto fail;
		}

		return 0;

		fail:
		brcmf_sdbrcm_pktfree2(bus, pkt);
		return -EINVAL;
		}

		/* Return true if there may be more frames to read */
		static uint
		brcmf_sdbrcm_readframes(struct brcmf_sdio bus, uint maxframes, bool finished)
		{
		u16 len, check; /* Extracted hardware header fields */
		u8 chan, seq, doff; /* Extracted software header fields */
		u8 fcbits; /* Extracted fcbits from software header */

		struct sk_buff pkt; / Packet for event or data frames */
		u16 pad; /* Number of pad bytes to read */
		u16 rdlen; /* Total number of bytes to read */
		u8 rxseq; /* Next sequence number to expect */
		uint rxleft = 0; /* Remaining number of frames allowed */
		int sdret; /* Return code from calls */
		u8 txmax; /* Maximum tx sequence offered */
		u8 *rxbuf;
		int ifidx = 0;
		uint rxcount = 0; /* Total frames read */
		struct brcmf_sdio_read *rd = &bus->cur_read, rd_new;
		u8 head_read = 0;

		brcmf_dbg(TRACE, "Enter\n");

		/* Not finished unless we encounter no more frames indication */
		*finished = false;
		bus->rxpending = true;

		for (rxseq = bus->rx_seq, rxleft = maxframes;
		for (rd->seq_num = bus->rx_seq, rxleft = maxframes;
		!bus->rxskip && rxleft &&
		bus->sdiodev->bus_if->state != BRCMF_BUS_DOWN;
		rxseq++, rxleft--) {
		rd->seq_num++, rxleft--) {

		/* Handle glomming separately */
		if (bus->glomd \|\| !skb_queue_empty(&bus->glom)) {
		u8 cnt;
		brcmf_dbg(GLOM, "calling rxglom: glomd %p, glom %p\n",
		bus->glomd, skb_peek(&bus->glom));
		cnt = brcmf_sdbrcm_rxglom(bus, rxseq);
		cnt = brcmf_sdbrcm_rxglom(bus, rd->seq_num);
		brcmf_dbg(GLOM, "rxglom returned %d\n", cnt);
		rxseq += cnt - 1;
		rd->seq_num += cnt - 1;
		rxleft = (rxleft > cnt) ? (rxleft - cnt) : 1;
		continue;
		}

		/* Try doing single read if we can */
		if (bus->nextlen) {
		u16 nextlen = bus->nextlen;
		bus->nextlen = 0;

		rdlen = len = nextlen << 4;
		brcmf_pad(bus, &pad, &rdlen);

		/*
		* After the frame is received we have to
		* distinguish whether it is data
		* or non-data frame.
		*/
		brcmf_alloc_pkt_and_read(bus, rdlen, &pkt, &rxbuf);
		if (pkt == NULL) {
		/* Give up on data, request rtx of events */
		brcmf_dbg(ERROR, "(nextlen): brcmf_alloc_pkt_and_read failed: len %d rdlen %d expected rxseq %d\n",
		len, rdlen, rxseq);
		continue;
		}

		if (brcmf_check_rxbuf(bus, pkt, rxbuf, rxseq, nextlen,
		&len) < 0)
		continue;

		/* Extract software header fields */
		chan = SDPCM_PACKET_CHANNEL(
		&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		seq = SDPCM_PACKET_SEQUENCE(
		&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		doff = SDPCM_DOFFSET_VALUE(
		&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		txmax = SDPCM_WINDOW_VALUE(
		&bus->rxhdr[SDPCM_FRAMETAG_LEN]);

		bus->nextlen =
		bus->rxhdr[SDPCM_FRAMETAG_LEN +
		SDPCM_NEXTLEN_OFFSET];
		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
		brcmf_dbg(INFO, "(nextlen): got frame w/nextlen too large (%d), seq %d\n",
		bus->nextlen, seq);
		bus->nextlen = 0;
		}

		bus->sdcnt.rx_readahead_cnt++;

		/* Handle Flow Control */
		fcbits = SDPCM_FCMASK_VALUE(
		&bus->rxhdr[SDPCM_FRAMETAG_LEN]);

		if (bus->flowcontrol != fcbits) {
		if (~bus->flowcontrol & fcbits)
		bus->sdcnt.fc_xoff++;

		if (bus->flowcontrol & ~fcbits)
		bus->sdcnt.fc_xon++;

		bus->sdcnt.fc_rcvd++;
		bus->flowcontrol = fcbits;
		}

		/* Check and update sequence number */
		if (rxseq != seq) {
		brcmf_dbg(INFO, "(nextlen): rx_seq %d, expected %d\n",
		seq, rxseq);
		bus->sdcnt.rx_badseq++;
		rxseq = seq;
		}

		/* Check window for sanity */
		if ((u8) (txmax - bus->tx_seq) > 0x40) {
		brcmf_dbg(ERROR, "got unlikely tx max %d with tx_seq %d\n",
		txmax, bus->tx_seq);
		txmax = bus->tx_seq + 2;
		}
		bus->tx_max = txmax;

		brcmf_dbg_hex_dump(BRCMF_BYTES_ON() && BRCMF_DATA_ON(),
		rxbuf, len, "Rx Data:\n");
		brcmf_dbg_hex_dump(!(BRCMF_BYTES_ON() &&
		BRCMF_DATA_ON()) &&
		BRCMF_HDRS_ON(),
		bus->rxhdr, SDPCM_HDRLEN,
		"RxHdr:\n");

		if (chan == SDPCM_CONTROL_CHANNEL) {
		brcmf_dbg(ERROR, "(nextlen): readahead on control packet %d?\n",
		seq);
		/* Force retry w/normal header read */
		bus->nextlen = 0;
		brcmf_sdbrcm_rxfail(bus, false, true);
		brcmf_sdbrcm_pktfree2(bus, pkt);
		continue;
		}

		/* Validate data offset */
		if ((doff < SDPCM_HDRLEN) \|\| (doff > len)) {
		brcmf_dbg(ERROR, "(nextlen): bad data offset %d: HW len %d min %d\n",
		doff, len, SDPCM_HDRLEN);
		brcmf_sdbrcm_rxfail(bus, false, false);
		brcmf_sdbrcm_pktfree2(bus, pkt);
		continue;
		}

		/* All done with this one -- now deliver the packet */
		goto deliver;
		}

		/* Read frame header (hardware and software) */
		sdret = brcmf_sdcard_recv_buf(bus->sdiodev, bus->sdiodev->sbwad,
		SDIO_FUNC_2, F2SYNC, bus->rxhdr,
		rd->len_left = rd->len;
		/* read header first for unknow frame length */
		if (!rd->len) {
		sdret = brcmf_sdcard_recv_buf(bus->sdiodev,
		bus->sdiodev->sbwad,
		SDIO_FUNC_2, F2SYNC,
		bus->rxhdr,
		BRCMF_FIRSTREAD);
		bus->sdcnt.f2rxhdrs++;

		if (sdret < 0) {
		brcmf_dbg(ERROR, "RXHEADER FAILED: %d\n", sdret);
		brcmf_dbg(ERROR, "RXHEADER FAILED: %d\n",
		sdret);
		bus->sdcnt.rx_hdrfail++;
		brcmf_sdbrcm_rxfail(bus, true, true);
		continue;
		}
		brcmf_dbg_hex_dump(BRCMF_BYTES_ON() \|\| BRCMF_HDRS_ON(),
		bus->rxhdr, SDPCM_HDRLEN, "RxHdr:\n");


		/* Extract hardware header fields */
		len = get_unaligned_le16(bus->rxhdr);
		check = get_unaligned_le16(bus->rxhdr + sizeof(u16));
		brcmf_dbg_hex_dump(BRCMF_BYTES_ON() \|\| BRCMF_HDRS_ON(),
		bus->rxhdr, SDPCM_HDRLEN,
		"RxHdr:\n");

		/* All zeros means no more frames */
		if (!(len \| check)) {
		*finished = true;
		if (!brcmf_sdio_hdparser(bus, bus->rxhdr, rd)) {
		if (!bus->rxpending)
		break;
		}

		/* Validate check bytes */
		if ((u16) ~(len ^ check)) {
		brcmf_dbg(ERROR, "HW hdr err: len/check 0x%04x/0x%04x\n",
		len, check);
		bus->sdcnt.rx_badhdr++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		continue;
		}

		/* Validate frame length */
		if (len < SDPCM_HDRLEN) {
		brcmf_dbg(ERROR, "HW hdr length invalid: %d\n", len);
		continue;
		}

		/* Extract software header fields */
		chan = SDPCM_PACKET_CHANNEL(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		seq = SDPCM_PACKET_SEQUENCE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		doff = SDPCM_DOFFSET_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);
		txmax = SDPCM_WINDOW_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);

		/* Validate data offset */
		if ((doff < SDPCM_HDRLEN) \|\| (doff > len)) {
		brcmf_dbg(ERROR, "Bad data offset %d: HW len %d, min %d seq %d\n",
		doff, len, SDPCM_HDRLEN, seq);
		bus->sdcnt.rx_badhdr++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		else
		continue;
		}

		/* Save the readahead length if there is one */
		bus->nextlen =
		bus->rxhdr[SDPCM_FRAMETAG_LEN + SDPCM_NEXTLEN_OFFSET];
		if ((bus->nextlen << 4) > MAX_RX_DATASZ) {
		brcmf_dbg(INFO, "(nextlen): got frame w/nextlen too large (%d), seq %d\n",
		bus->nextlen, seq);
		bus->nextlen = 0;
		}

		/* Handle Flow Control */
		fcbits = SDPCM_FCMASK_VALUE(&bus->rxhdr[SDPCM_FRAMETAG_LEN]);

		if (bus->flowcontrol != fcbits) {
		if (~bus->flowcontrol & fcbits)
		bus->sdcnt.fc_xoff++;

		if (bus->flowcontrol & ~fcbits)
		bus->sdcnt.fc_xon++;

		bus->sdcnt.fc_rcvd++;
		bus->flowcontrol = fcbits;
		}

		/* Check and update sequence number */
		if (rxseq != seq) {
		brcmf_dbg(INFO, "rx_seq %d, expected %d\n", seq, rxseq);
		bus->sdcnt.rx_badseq++;
		rxseq = seq;
		}

		/* Check window for sanity */
		if ((u8) (txmax - bus->tx_seq) > 0x40) {
		brcmf_dbg(ERROR, "unlikely tx max %d with tx_seq %d\n",
		txmax, bus->tx_seq);
		txmax = bus->tx_seq + 2;
		}
		bus->tx_max = txmax;

		/* Call a separate function for control frames */
		if (chan == SDPCM_CONTROL_CHANNEL) {
		brcmf_sdbrcm_read_control(bus, bus->rxhdr, len, doff);
		if (rd->channel == SDPCM_CONTROL_CHANNEL) {
		brcmf_sdbrcm_read_control(bus, bus->rxhdr,
		rd->len,
		rd->dat_offset);
		/* prepare the descriptor for the next read */
		rd->len = rd->len_nxtfrm << 4;
		rd->len_nxtfrm = 0;
		/* treat all packet as event if we don't know */
		rd->channel = SDPCM_EVENT_CHANNEL;
		continue;
		}

		/* precondition: chan is either SDPCM_DATA_CHANNEL,
		SDPCM_EVENT_CHANNEL, SDPCM_TEST_CHANNEL or
		SDPCM_GLOM_CHANNEL */

		/* Length to read */
		rdlen = (len > BRCMF_FIRSTREAD) ? (len - BRCMF_FIRSTREAD) : 0;

		/* May pad read to blocksize for efficiency */
		if (bus->roundup && bus->blocksize &&
		(rdlen > bus->blocksize)) {
		pad = bus->blocksize - (rdlen % bus->blocksize);
		if ((pad <= bus->roundup) && (pad < bus->blocksize) &&
		((rdlen + pad + BRCMF_FIRSTREAD) < MAX_RX_DATASZ))
		rdlen += pad;
		} else if (rdlen % BRCMF_SDALIGN) {
		rdlen += BRCMF_SDALIGN - (rdlen % BRCMF_SDALIGN);
		rd->len_left = rd->len > BRCMF_FIRSTREAD ?
		rd->len - BRCMF_FIRSTREAD : 0;
		head_read = BRCMF_FIRSTREAD;
		}

		/* Satisfy length-alignment requirements */
		if (rdlen & (ALIGNMENT - 1))
		rdlen = roundup(rdlen, ALIGNMENT);

		if ((rdlen + BRCMF_FIRSTREAD) > MAX_RX_DATASZ) {
		/* Too long -- skip this frame */
		brcmf_dbg(ERROR, "too long: len %d rdlen %d\n",
		len, rdlen);
		bus->sdiodev->bus_if->dstats.rx_errors++;
		bus->sdcnt.rx_toolong++;
		brcmf_sdbrcm_rxfail(bus, false, false);
		continue;
		}
		brcmf_pad(bus, &pad, &rd->len_left);

		pkt = brcmu_pkt_buf_get_skb(rdlen +
		BRCMF_FIRSTREAD + BRCMF_SDALIGN);
		pkt = brcmu_pkt_buf_get_skb(rd->len_left + head_read +
		BRCMF_SDALIGN);
		if (!pkt) {
		/* Give up on data, request rtx of events */
		brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed: rdlen %d chan %d\n",
		rdlen, chan);
		brcmf_dbg(ERROR, "brcmu_pkt_buf_get_skb failed\n");
		bus->sdiodev->bus_if->dstats.rx_dropped++;
		brcmf_sdbrcm_rxfail(bus, false, RETRYCHAN(chan));
		brcmf_sdbrcm_rxfail(bus, false,
		RETRYCHAN(rd->channel));
		continue;
		}
		skb_pull(pkt, head_read);
		pkt_align(pkt, rd->len_left, BRCMF_SDALIGN);

		/* Leave room for what we already read, and align remainder */
		skb_pull(pkt, BRCMF_FIRSTREAD);
		pkt_align(pkt, rdlen, BRCMF_SDALIGN);

		/* Read the remaining frame data */
		sdret = brcmf_sdcard_recv_pkt(bus->sdiodev, bus->sdiodev->sbwad,
		SDIO_FUNC_2, F2SYNC, pkt);
		bus->sdcnt.f2rxdata++;

		if (sdret < 0) {
		brcmf_dbg(ERROR, "read %d %s bytes failed: %d\n", rdlen,
		((chan == SDPCM_EVENT_CHANNEL) ? "event"
		: ((chan == SDPCM_DATA_CHANNEL) ? "data"
		: "test")), sdret);
		brcmf_dbg(ERROR, "read %d bytes from channel %d failed: %d\n",
		rd->len, rd->channel, sdret);
		brcmu_pkt_buf_free_skb(pkt);
		bus->sdiodev->bus_if->dstats.rx_errors++;
		brcmf_sdbrcm_rxfail(bus, true, RETRYCHAN(chan));
		brcmf_sdbrcm_rxfail(bus, true,
		RETRYCHAN(rd->channel));
		continue;
		}

		/* Copy the already-read portion */
		skb_push(pkt, BRCMF_FIRSTREAD);
		memcpy(pkt->data, bus->rxhdr, BRCMF_FIRSTREAD);
		if (head_read) {
		skb_push(pkt, head_read);
		memcpy(pkt->data, bus->rxhdr, head_read);
		head_read = 0;
		} else {
		memcpy(bus->rxhdr, pkt->data, SDPCM_HDRLEN);
		rd_new.seq_num = rd->seq_num;
		if (!brcmf_sdio_hdparser(bus, bus->rxhdr, &rd_new)) {
		rd->len = 0;
		brcmu_pkt_buf_free_skb(pkt);
		}
		bus->sdcnt.rx_readahead_cnt++;
		if (rd->len != roundup(rd_new.len, 16)) {
		brcmf_dbg(ERROR, "frame length mismatch:read %d, should be %d\n",
		rd->len,
		roundup(rd_new.len, 16) >> 4);
		rd->len = 0;
		brcmf_sdbrcm_rxfail(bus, true, true);
		brcmu_pkt_buf_free_skb(pkt);
		continue;
		}
		rd->len_nxtfrm = rd_new.len_nxtfrm;
		rd->channel = rd_new.channel;
		rd->dat_offset = rd_new.dat_offset;

		brcmf_dbg_hex_dump(!(BRCMF_BYTES_ON() &&
		BRCMF_DATA_ON()) &&
		BRCMF_HDRS_ON(),
		bus->rxhdr, SDPCM_HDRLEN,
		"RxHdr:\n");

		if (rd_new.channel == SDPCM_CONTROL_CHANNEL) {
		brcmf_dbg(ERROR, "readahead on control packet %d?\n",
		rd_new.seq_num);
		/* Force retry w/normal header read */
		rd->len = 0;
		brcmf_sdbrcm_rxfail(bus, false, true);
		brcmu_pkt_buf_free_skb(pkt);
		continue;
		}
		}

		brcmf_dbg_hex_dump(BRCMF_BYTES_ON() && BRCMF_DATA_ON(),
		pkt->data, len, "Rx Data:\n");
		pkt->data, rd->len, "Rx Data:\n");

		deliver:
		/* Save superframe descriptor and allocate packet frame */
		if (chan == SDPCM_GLOM_CHANNEL) {
		if (rd->channel == SDPCM_GLOM_CHANNEL) {
		if (SDPCM_GLOMDESC(&bus->rxhdr[SDPCM_FRAMETAG_LEN])) {
		brcmf_dbg(GLOM, "glom descriptor, %d bytes:\n",
		len);
		rd->len);
		brcmf_dbg_hex_dump(BRCMF_GLOM_ON(),
		pkt->data, len,
		pkt->data, rd->len,
		"Glom Data:\n");
		__skb_trim(pkt, len);
		__skb_trim(pkt, rd->len);
		skb_pull(pkt, SDPCM_HDRLEN);
		bus->glomd = pkt;
		} else {
		@@ -1919,12 +1755,23 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio bus, uint maxframes, bool finished)
		"descriptor!\n", __func__);
		brcmf_sdbrcm_rxfail(bus, false, false);
		}
		/* prepare the descriptor for the next read */
		rd->len = rd->len_nxtfrm << 4;
		rd->len_nxtfrm = 0;
		/* treat all packet as event if we don't know */
		rd->channel = SDPCM_EVENT_CHANNEL;
		continue;
		}

		/* Fill in packet len and prio, deliver upward */
		__skb_trim(pkt, len);
		skb_pull(pkt, doff);
		__skb_trim(pkt, rd->len);
		skb_pull(pkt, rd->dat_offset);

		/* prepare the descriptor for the next read */
		rd->len = rd->len_nxtfrm << 4;
		rd->len_nxtfrm = 0;
		/* treat all packet as event if we don't know */
		rd->channel = SDPCM_EVENT_CHANNEL;

		if (pkt->len == 0) {
		brcmu_pkt_buf_free_skb(pkt);
		@@ -1942,17 +1789,17 @@ brcmf_sdbrcm_readframes(struct brcmf_sdio bus, uint maxframes, bool finished)
		brcmf_rx_packet(bus->sdiodev->dev, ifidx, pkt);
		down(&bus->sdsem);
		}

		rxcount = maxframes - rxleft;
		/* Message if we hit the limit */
		if (!rxleft)
		brcmf_dbg(DATA, "hit rx limit of %d frames\n",
		maxframes);
		brcmf_dbg(DATA, "hit rx limit of %d frames\n", maxframes);
		else
		brcmf_dbg(DATA, "processed %d frames\n", rxcount);
		/* Back off rxseq if awaiting rtx, update rx_seq */
		if (bus->rxskip)
		rxseq--;
		bus->rx_seq = rxseq;
		rd->seq_num--;
		bus->rx_seq = rd->seq_num;

		return rxcount;
		}
		@@ -2313,7 +2160,6 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
		uint rxlimit = bus->rxbound; /* Rx frames to read before resched */
		uint txlimit = bus->txbound; /* Tx frames to send before resched */
		uint framecnt = 0; /* Temporary counter of tx/rx frames */
		bool rxdone = true; /* Flag for no more read data */
		int err = 0, n;

		brcmf_dbg(TRACE, "Enter\n");
		@@ -2431,8 +2277,8 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)

		/* On frame indication, read available frames */
		if (PKT_AVAILABLE() && bus->clkstate == CLK_AVAIL) {
		framecnt = brcmf_sdbrcm_readframes(bus, rxlimit, &rxdone);
		if (rxdone \|\| bus->rxskip)
		framecnt = brcmf_sdio_readframes(bus, rxlimit);
		if (!bus->rxpending)
		intstatus &= ~I_HMB_FRAME_IND;
		rxlimit -= min(framecnt, rxlimit);
		}
		@@ -2489,7 +2335,8 @@ static void brcmf_sdbrcm_dpc(struct brcmf_sdio *bus)
		else if ((bus->clkstate == CLK_AVAIL) && !atomic_read(&bus->fcstate) &&
		brcmu_pktq_mlen(&bus->txq, ~bus->flowcontrol) && txlimit
		&& data_ok(bus)) {
		framecnt = rxdone ? txlimit : min(txlimit, bus->txminmax);
		framecnt = bus->rxpending ? min(txlimit, bus->txminmax) :
		txlimit;
		framecnt = brcmf_sdbrcm_sendfromq(bus, framecnt);
		txlimit -= framecnt;
		}
		@@ -4056,8 +3903,6 @@ void brcmf_sdbrcm_probe(u32 regsva, struct brcmf_sdio_dev sdiodev)
		bus->rxbound = BRCMF_RXBOUND;
		bus->txminmax = BRCMF_TXMINMAX;
		bus->tx_seq = SDPCM_SEQUENCE_WRAP - 1;
		bus->usebufpool = false; /* Use bufpool if allocated,
		else use locally malloced rxbuf */

		/* attempt to attach to the dongle */
		if (!(brcmf_sdbrcm_probe_attach(bus, regsva))) {