diff options
author | Suraj Sumangala <suraj@atheros.com> | 2010-07-14 07:32:17 (GMT) |
---|---|---|
committer | Marcel Holtmann <marcel@holtmann.org> | 2010-07-21 17:39:12 (GMT) |
commit | 33e882a5f2301a23a85ef2994e30fd9f48d39d9b (patch) | |
tree | 4048a954d11698581e123b25389a9f8f72125f90 /net/bluetooth/hci_core.c | |
parent | cd4c53919ed50b0d532f106aeb76e79077bece98 (diff) | |
download | linux-fsl-qoriq-33e882a5f2301a23a85ef2994e30fd9f48d39d9b.tar.xz |
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>
Diffstat (limited to 'net/bluetooth/hci_core.c')
-rw-r--r-- | net/bluetooth/hci_core.c | 109 |
1 files changed, 109 insertions, 0 deletions
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index 0ded790..477c4a6 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]) |