From aeac30140694824f26d14655271e1dcf3e32fd49 Mon Sep 17 00:00:00 2001 From: Sebastian Reichel Date: Tue, 28 Mar 2017 17:59:34 +0200 Subject: Bluetooth: hci_uart: add support for word alignment This will be used by Nokia's H4+ protocol, which uses 2-byte aligned packets. Acked-by: Pavel Machek Signed-off-by: Sebastian Reichel Signed-off-by: Marcel Holtmann --- drivers/bluetooth/hci_h4.c | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) (limited to 'drivers/bluetooth/hci_h4.c') diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c index 635597b6e168..82e5a32b87a4 100644 --- a/drivers/bluetooth/hci_h4.c +++ b/drivers/bluetooth/hci_h4.c @@ -171,9 +171,20 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb, const unsigned char *buffer, int count, const struct h4_recv_pkt *pkts, int pkts_count) { + struct hci_uart *hu = hci_get_drvdata(hdev); + u8 alignment = hu->alignment; + while (count) { int i, len; + /* remove padding bytes from buffer */ + for (; hu->padding && count > 0; hu->padding--) { + count--; + buffer++; + } + if (!count) + break; + if (!skb) { for (i = 0; i < pkts_count; i++) { if (buffer[0] != (&pkts[i])->type) @@ -253,11 +264,17 @@ struct sk_buff *h4_recv_buf(struct hci_dev *hdev, struct sk_buff *skb, } if (!dlen) { + hu->padding = (skb->len - 1) % alignment; + hu->padding = (alignment - hu->padding) % alignment; + /* No more data, complete frame */ (&pkts[i])->recv(hdev, skb); skb = NULL; } } else { + hu->padding = (skb->len - 1) % alignment; + hu->padding = (alignment - hu->padding) % alignment; + /* Complete frame */ (&pkts[i])->recv(hdev, skb); skb = NULL; -- cgit v1.2.3