#ifdef DEBUG
static int qcount[NUMPRIO];
-static int tx_packets[NUMPRIO];
#endif /* DEBUG */
#define DEFAULT_SDIO_DRIVE_STRENGTH 6 /* in milliamps */
return;
}
+/* flag marking a dummy skb added for DMA alignment requirement */
+#define DUMMY_SKB_FLAG 0x10000
+/* bit mask of data length chopped from the previous packet */
+#define DUMMY_SKB_CHOP_LEN_MASK 0xffff
+/**
+ * brcmf_sdio_txpkt_prep - packet preparation for transmit
+ * @bus: brcmf_sdio structure pointer
+ * @pktq: packet list pointer
+ * @chan: virtual channel to transmit the packet
+ *
+ * Processes to be applied to the packet
+ * - Align data buffer pointer
+ * - Align data buffer length
+ * - Prepare header
+ * Return: negative value if there is error
+ */
+static int
+brcmf_sdio_txpkt_prep(struct brcmf_sdio *bus, struct sk_buff_head *pktq,
+ uint chan)
+{
+ u16 head_pad, tail_pad, tail_chop, pkt_len;
+ u16 head_align, sg_align;
+ u32 sw_header;
+ int ntail;
+ struct sk_buff *pkt_next, *pkt_new;
+ u8 *dat_buf;
+ unsigned blksize = bus->sdiodev->func[SDIO_FUNC_2]->cur_blksize;
+
+ /* SDIO ADMA requires at least 32 bit alignment */
+ head_align = 4;
+ sg_align = 4;
+ if (bus->sdiodev->pdata) {
+ head_align = bus->sdiodev->pdata->sd_head_align > 4 ?
+ bus->sdiodev->pdata->sd_head_align : 4;
+ sg_align = bus->sdiodev->pdata->sd_sgentry_align > 4 ?
+ bus->sdiodev->pdata->sd_sgentry_align : 4;
+ }
+ /* sg entry alignment should be a divisor of block size */
+ WARN_ON(blksize % sg_align);
+
+ pkt_next = pktq->next;
+ dat_buf = (u8 *)(pkt_next->data);
+
+ /* Check head padding */
+ head_pad = ((unsigned long)dat_buf % head_align);
+ if (head_pad) {
+ if (skb_headroom(pkt_next) < head_pad) {
+ bus->sdiodev->bus_if->tx_realloc++;
+ head_pad = 0;
+ if (skb_cow(pkt_next, head_pad))
+ return -ENOMEM;
+ }
+ skb_push(pkt_next, head_pad);
+ dat_buf = (u8 *)(pkt_next->data);
+ memset(dat_buf, 0, head_pad + SDPCM_HDRLEN);
+ }
+
+ /* Check tail padding */
+ pkt_new = NULL;
+ tail_chop = pkt_next->len % sg_align;
+ tail_pad = sg_align - tail_chop;
+ tail_pad += blksize - (pkt_next->len + tail_pad) % blksize;
+ if (skb_tailroom(pkt_next) < tail_pad && pkt_next->len > blksize) {
+ pkt_new = brcmu_pkt_buf_get_skb(tail_pad + tail_chop);
+ if (pkt_new == NULL)
+ return -ENOMEM;
+ memcpy(pkt_new->data,
+ pkt_next->data + pkt_next->len - tail_chop,
+ tail_chop);
+ *(u32 *)(pkt_new->cb) = DUMMY_SKB_FLAG + tail_chop;
+ skb_trim(pkt_next, pkt_next->len - tail_chop);
+ __skb_queue_after(pktq, pkt_next, pkt_new);
+ } else {
+ ntail = pkt_next->data_len + tail_pad -
+ (pkt_next->end - pkt_next->tail);
+ if (skb_cloned(pkt_next) || ntail > 0)
+ if (pskb_expand_head(pkt_next, 0, ntail, GFP_ATOMIC))
+ return -ENOMEM;
+ if (skb_linearize(pkt_next))
+ return -ENOMEM;
+ dat_buf = (u8 *)(pkt_next->data);
+ __skb_put(pkt_next, tail_pad);
+ }
+
+ /* Now prep the header */
+ /* 4 bytes hardware header (frame tag)
+ * Byte 0~1: Frame length
+ * Byte 2~3: Checksum, bit-wise inverse of frame length
+ */
+ if (pkt_new)
+ pkt_len = pkt_next->len + tail_chop;
+ else
+ pkt_len = pkt_next->len - tail_pad;
+ *(__le16 *)dat_buf = cpu_to_le16(pkt_len);
+ *(((__le16 *)dat_buf) + 1) = cpu_to_le16(~pkt_len);
+ /* 8 bytes software header
+ * Byte 0: Tx sequence number
+ * Byte 1: 4 MSB Channel number
+ * Byte 2: Reserved
+ * Byte 3: Data offset
+ * Byte 4~7: Reserved
+ */
+ sw_header = bus->tx_seq;
+ sw_header |= ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK);
+ sw_header |= ((head_pad + SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) &
+ SDPCM_DOFFSET_MASK;
+ *(((__le32 *)dat_buf) + 1) = cpu_to_le32(sw_header);
+ *(((__le32 *)dat_buf) + 2) = 0;
+
+ if (BRCMF_BYTES_ON() &&
+ ((BRCMF_CTL_ON() && chan == SDPCM_CONTROL_CHANNEL) ||
+ (BRCMF_DATA_ON() && chan != SDPCM_CONTROL_CHANNEL)))
+ brcmf_dbg_hex_dump(true, pkt_next, pkt_len, "Tx Frame:\n");
+ else if (BRCMF_HDRS_ON())
+ brcmf_dbg_hex_dump(true, pkt_next, head_pad + SDPCM_HDRLEN,
+ "Tx Header:\n");
+
+ return 0;
+}
+
+/**
+ * brcmf_sdio_txpkt_postp - packet post processing for transmit
+ * @bus: brcmf_sdio structure pointer
+ * @pktq: packet list pointer
+ *
+ * Processes to be applied to the packet
+ * - Remove head padding
+ * - Remove tail padding
+ */
+static void
+brcmf_sdio_txpkt_postp(struct brcmf_sdio *bus, struct sk_buff_head *pktq)
+{
+ u8 *hdr;
+ u32 dat_offset;
+ u32 dummy_flags, chop_len;
+ struct sk_buff *pkt_next, *tmp, *pkt_prev;
+
+ skb_queue_walk_safe(pktq, pkt_next, tmp) {
+ dummy_flags = *(u32 *)(pkt_next->cb);
+ if (dummy_flags & DUMMY_SKB_FLAG) {
+ chop_len = dummy_flags & DUMMY_SKB_CHOP_LEN_MASK;
+ if (chop_len) {
+ pkt_prev = pkt_next->prev;
+ memcpy(pkt_prev->data + pkt_prev->len,
+ pkt_next->data, chop_len);
+ skb_put(pkt_prev, chop_len);
+ }
+ __skb_unlink(pkt_next, pktq);
+ brcmu_pkt_buf_free_skb(pkt_next);
+ } else {
+ hdr = pkt_next->data + SDPCM_FRAMETAG_LEN;
+ dat_offset = le32_to_cpu(*(__le32 *)hdr);
+ dat_offset = (dat_offset & SDPCM_DOFFSET_MASK) >>
+ SDPCM_DOFFSET_SHIFT;
+ skb_pull(pkt_next, dat_offset);
+ }
+ }
+}
+
/* Writes a HW/SW header into the packet and sends it. */
/* Assumes: (a) header space already there, (b) caller holds lock */
static int brcmf_sdbrcm_txpkt(struct brcmf_sdio *bus, struct sk_buff *pkt,
uint chan)
{
int ret;
- u8 *frame;
- u16 len, pad = 0;
- u32 swheader;
int i;
+ struct sk_buff_head localq;
brcmf_dbg(TRACE, "Enter\n");
- frame = (u8 *) (pkt->data);
-
- /* Add alignment padding, allocate new packet if needed */
- pad = ((unsigned long)frame % BRCMF_SDALIGN);
- if (pad) {
- if (skb_headroom(pkt) < pad) {
- brcmf_dbg(INFO, "insufficient headroom %d for %d pad\n",
- skb_headroom(pkt), pad);
- bus->sdiodev->bus_if->tx_realloc++;
- ret = skb_cow(pkt, BRCMF_SDALIGN);
- if (ret)
- goto done;
- pad = ((unsigned long)frame % BRCMF_SDALIGN);
- }
- skb_push(pkt, pad);
- frame = (u8 *) (pkt->data);
- memset(frame, 0, pad + SDPCM_HDRLEN);
- }
- /* precondition: pad < BRCMF_SDALIGN */
-
- /* Hardware tag: 2 byte len followed by 2 byte ~len check (all LE) */
- len = (u16) (pkt->len);
- *(__le16 *) frame = cpu_to_le16(len);
- *(((__le16 *) frame) + 1) = cpu_to_le16(~len);
-
- /* Software tag: channel, sequence number, data offset */
- swheader =
- ((chan << SDPCM_CHANNEL_SHIFT) & SDPCM_CHANNEL_MASK) | bus->tx_seq |
- (((pad +
- SDPCM_HDRLEN) << SDPCM_DOFFSET_SHIFT) & SDPCM_DOFFSET_MASK);
-
- *(((__le32 *) frame) + 1) = cpu_to_le32(swheader);
- *(((__le32 *) frame) + 2) = 0;
-
-#ifdef DEBUG
- tx_packets[pkt->priority]++;
-#endif
-
- brcmf_dbg_hex_dump(BRCMF_BYTES_ON() &&
- ((BRCMF_CTL_ON() && chan == SDPCM_CONTROL_CHANNEL) ||
- (BRCMF_DATA_ON() && chan != SDPCM_CONTROL_CHANNEL)),
- frame, len, "Tx Frame:\n");
- brcmf_dbg_hex_dump(!(BRCMF_BYTES_ON() &&
- ((BRCMF_CTL_ON() &&
- chan == SDPCM_CONTROL_CHANNEL) ||
- (BRCMF_DATA_ON() &&
- chan != SDPCM_CONTROL_CHANNEL))) &&
- BRCMF_HDRS_ON(),
- frame, min_t(u16, len, 16), "TxHdr:\n");
-
- /* Raise len to next SDIO block to eliminate tail command */
- if (bus->roundup && bus->blocksize && (len > bus->blocksize)) {
- u16 pad = bus->blocksize - (len % bus->blocksize);
- if ((pad <= bus->roundup) && (pad < bus->blocksize))
- len += pad;
- } else if (len % BRCMF_SDALIGN) {
- len += BRCMF_SDALIGN - (len % BRCMF_SDALIGN);
- }
-
- /* Some controllers have trouble with odd bytes -- round to even */
- if (len & (ALIGNMENT - 1))
- len = roundup(len, ALIGNMENT);
+ __skb_queue_head_init(&localq);
+ __skb_queue_tail(&localq, pkt);
+ ret = brcmf_sdio_txpkt_prep(bus, &localq, chan);
+ if (ret)
+ goto done;
sdio_claim_host(bus->sdiodev->func[1]);
ret = brcmf_sdcard_send_pkt(bus->sdiodev, bus->sdiodev->sbwad,
- SDIO_FUNC_2, F2SYNC, pkt);
+ SDIO_FUNC_2, F2SYNC, &localq);
bus->sdcnt.f2txdata++;
if (ret < 0) {
bus->tx_seq = (bus->tx_seq + 1) % SDPCM_SEQUENCE_WRAP;
done:
- /* restore pkt buffer pointer before calling tx complete routine */
- skb_pull(pkt, SDPCM_HDRLEN + pad);
+ brcmf_sdio_txpkt_postp(bus, &localq);
+ __skb_dequeue_tail(&localq);
brcmf_txcomplete(bus->sdiodev->dev, pkt, ret == 0);
return ret;
}
projects / GitHub / MotorolaMobilityLLC / kernel-slsi.git / commitdiff