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 <pavel@ucw.cz>
Signed-off-by: Sebastian Reichel <sre@kernel.org>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/drivers/bluetooth/hci_h4.c b/drivers/bluetooth/hci_h4.c
index 635597b..82e5a32 100644 (file)
--- 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;

diff --git a/drivers/bluetooth/hci_ldisc.c b/drivers/bluetooth/hci_ldisc.c
index 9497c46..0ec8a94 100644 (file)
--- a/drivers/bluetooth/hci_ldisc.c
+++ b/drivers/bluetooth/hci_ldisc.c
@@ -459,6 +459,10 @@ static int hci_uart_tty_open(struct tty_struct *tty)
        hu->tty = tty;
        tty->receive_room = 65536;
 
+       /* disable alignment support by default */
+       hu->alignment = 1;
+       hu->padding = 0;
+
        INIT_WORK(&hu->init_ready, hci_uart_init_work);
        INIT_WORK(&hu->write_work, hci_uart_write_work);
 

diff --git a/drivers/bluetooth/hci_uart.h b/drivers/bluetooth/hci_uart.h
index 0701395..4aff509 100644 (file)
--- a/drivers/bluetooth/hci_uart.h
+++ b/drivers/bluetooth/hci_uart.h
@@ -92,6 +92,9 @@ struct hci_uart {
 
        unsigned int init_speed;
        unsigned int oper_speed;
+
+       u8                      alignment;
+       u8                      padding;
 };
 
 /* HCI_UART proto flag bits */