Bluetooth: Implement hci_reassembly helper to reassemble RX packets

Implements feature to reassemble received HCI frames from any input stream

Signed-off-by: Suraj Sumangala <suraj@atheros.com>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

1 files changed, 109 insertions, 0 deletions

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 0ded790bfb67..477c4a60a079 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1033,6 +1033,115 @@ int hci_recv_frame(struct sk_buff *skb)
 }
 EXPORT_SYMBOL(hci_recv_frame);
 
+static int hci_reassembly(struct hci_dev *hdev, int type, void *data,
+                          int count, __u8 index, gfp_t gfp_mask)
+{
+        int len = 0;
+        int hlen = 0;
+        int remain = count;
+        struct sk_buff *skb;
+        struct bt_skb_cb *scb;
+
+        if ((type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) ||
+                                index >= NUM_REASSEMBLY)
+                return -EILSEQ;
+
+        skb = hdev->reassembly[index];
+
+        if (!skb) {
+                switch (type) {
+                case HCI_ACLDATA_PKT:
+                        len = HCI_MAX_FRAME_SIZE;
+                        hlen = HCI_ACL_HDR_SIZE;
+                        break;
+                case HCI_EVENT_PKT:
+                        len = HCI_MAX_EVENT_SIZE;
+                        hlen = HCI_EVENT_HDR_SIZE;
+                        break;
+                case HCI_SCODATA_PKT:
+                        len = HCI_MAX_SCO_SIZE;
+                        hlen = HCI_SCO_HDR_SIZE;
+                        break;
+                }
+
+                skb = bt_skb_alloc(len, gfp_mask);
+                if (!skb)
+                        return -ENOMEM;
+
+                scb = (void *) skb->cb;
+                scb->expect = hlen;
+                scb->pkt_type = type;
+
+                skb->dev = (void *) hdev;
+                hdev->reassembly[index] = skb;
+        }
+
+        while (count) {
+                scb = (void *) skb->cb;
+                len = min(scb->expect, (__u16)count);
+
+                memcpy(skb_put(skb, len), data, len);
+
+                count -= len;
+                data += len;
+                scb->expect -= len;
+                remain = count;
+
+                switch (type) {
+                case HCI_EVENT_PKT:
+                        if (skb->len == HCI_EVENT_HDR_SIZE) {
+                                struct hci_event_hdr *h = hci_event_hdr(skb);
+                                scb->expect = h->plen;
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
+                        }
+                        break;
+
+                case HCI_ACLDATA_PKT:
+                        if (skb->len  == HCI_ACL_HDR_SIZE) {
+                                struct hci_acl_hdr *h = hci_acl_hdr(skb);
+                                scb->expect = __le16_to_cpu(h->dlen);
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
+                        }
+                        break;
+
+                case HCI_SCODATA_PKT:
+                        if (skb->len == HCI_SCO_HDR_SIZE) {
+                                struct hci_sco_hdr *h = hci_sco_hdr(skb);
+                                scb->expect = h->dlen;
+
+                                if (skb_tailroom(skb) < scb->expect) {
+                                        kfree_skb(skb);
+                                        hdev->reassembly[index] = NULL;
+                                        return -ENOMEM;
+                                }
+                        }
+                        break;
+                }
+
+                if (scb->expect == 0) {
+                        /* Complete frame */
+
+                        bt_cb(skb)->pkt_type = type;
+                        hci_recv_frame(skb);
+
+                        hdev->reassembly[index] = NULL;
+                        return remain;
+                }
+        }
+
+        return remain;
+}
+
 /* Receive packet type fragment */
 #define __reassembly(hdev, type)  ((hdev)->reassembly[(type) - 1])