diff --git a/drivers/bluetooth/btmtksdio.c b/drivers/bluetooth/btmtksdio.c index 8052e24d503b..94aa60d9cc20 100644 --- a/drivers/bluetooth/btmtksdio.c +++ b/drivers/bluetooth/btmtksdio.c @@ -272,6 +272,7 @@ static int btmtksdio_tx_packet(struct btmtksdio_dev *bdev, struct sk_buff *skb) { struct mtkbtsdio_hdr *sdio_hdr; + unsigned int len, pad_len; int err; /* Make sure that the data buffer is not shared with anyone else and @@ -281,6 +282,18 @@ static int btmtksdio_tx_packet(struct btmtksdio_dev *bdev, if (err < 0) return err; + /* The transfer is rounded up to the SDIO block size, so the buffer + * has to provide tailroom for the padding as well + */ + len = skb->len + sizeof(*sdio_hdr); + pad_len = round_up(len, MTK_SDIO_BLOCK_SIZE) - len; + + if (unlikely(skb_tailroom(skb) < pad_len)) { + err = pskb_expand_head(skb, 0, pad_len, GFP_ATOMIC); + if (err < 0) + return err; + } + /* Prepend MediaTek SDIO Specific Header */ skb_push(skb, sizeof(*sdio_hdr)); @@ -289,19 +302,22 @@ static int btmtksdio_tx_packet(struct btmtksdio_dev *bdev, sdio_hdr->reserved = cpu_to_le16(0); sdio_hdr->bt_type = hci_skb_pkt_type(skb); - clear_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state); - err = sdio_writesb(bdev->func, MTK_REG_CTDR, skb->data, - round_up(skb->len, MTK_SDIO_BLOCK_SIZE)); - if (err < 0) - goto err_skb_pull; + /* Zero the padding so that no uninitialised memory is sent out */ + skb_put_zero(skb, pad_len); - bdev->hdev->stat.byte_tx += skb->len; + clear_bit(BTMTKSDIO_HW_TX_READY, &bdev->tx_state); + err = sdio_writesb(bdev->func, MTK_REG_CTDR, skb->data, skb->len); + if (err < 0) + goto err_skb_restore; + + bdev->hdev->stat.byte_tx += len; kfree_skb(skb); return 0; -err_skb_pull: +err_skb_restore: + skb_trim(skb, len); skb_pull(skb, sizeof(*sdio_hdr)); return err;