diff options
| -rw-r--r-- | include/net/bluetooth/hci_core.h | 3 | ||||
| -rw-r--r-- | net/bluetooth/hci_core.c | 97 |
2 files changed, 98 insertions, 2 deletions
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index c0fc39620f36..7c78744ec0fd 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -109,6 +109,7 @@ struct hci_dev { struct sk_buff_head cmd_q; struct sk_buff *sent_cmd; + struct sk_buff *reassembly[3]; struct semaphore req_lock; wait_queue_head_t req_wait_q; @@ -437,6 +438,8 @@ static inline int hci_recv_frame(struct sk_buff *skb) return 0; } +int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count); + int hci_register_sysfs(struct hci_dev *hdev); void hci_unregister_sysfs(struct hci_dev *hdev); void hci_conn_add_sysfs(struct hci_conn *conn); diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index aa4b56a8c3ea..9c71cffbc6b0 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -826,7 +826,7 @@ EXPORT_SYMBOL(hci_free_dev); int hci_register_dev(struct hci_dev *hdev) { struct list_head *head = &hci_dev_list, *p; - int id = 0; + int i, id = 0; BT_DBG("%p name %s type %d owner %p", hdev, hdev->name, hdev->type, hdev->owner); @@ -865,6 +865,9 @@ int hci_register_dev(struct hci_dev *hdev) skb_queue_head_init(&hdev->cmd_q); skb_queue_head_init(&hdev->raw_q); + for (i = 0; i < 3; i++) + hdev->reassembly[i] = NULL; + init_waitqueue_head(&hdev->req_wait_q); init_MUTEX(&hdev->req_lock); @@ -889,6 +892,8 @@ EXPORT_SYMBOL(hci_register_dev); /* Unregister HCI device */ int hci_unregister_dev(struct hci_dev *hdev) { + int i; + BT_DBG("%p name %s type %d", hdev, hdev->name, hdev->type); hci_unregister_sysfs(hdev); @@ -899,9 +904,13 @@ int hci_unregister_dev(struct hci_dev *hdev) hci_dev_do_close(hdev); + for (i = 0; i < 3; i++) + kfree_skb(hdev->reassembly[i]); + hci_notify(hdev, HCI_DEV_UNREG); __hci_dev_put(hdev); + return 0; } EXPORT_SYMBOL(hci_unregister_dev); @@ -922,6 +931,90 @@ int hci_resume_dev(struct hci_dev *hdev) } EXPORT_SYMBOL(hci_resume_dev); +/* Receive packet type fragment */ +#define __reassembly(hdev, type) ((hdev)->reassembly[(type) - 2]) + +int hci_recv_fragment(struct hci_dev *hdev, int type, void *data, int count) +{ + if (type < HCI_ACLDATA_PKT || type > HCI_EVENT_PKT) + return -EILSEQ; + + while (count) { + struct sk_buff *skb = __reassembly(hdev, type); + struct { int expect; } *scb; + int len = 0; + + if (!skb) { + /* Start of the frame */ + + switch (type) { + case HCI_EVENT_PKT: + if (count >= HCI_EVENT_HDR_SIZE) { + struct hci_event_hdr *h = data; + len = HCI_EVENT_HDR_SIZE + h->plen; + } else + return -EILSEQ; + break; + + case HCI_ACLDATA_PKT: + if (count >= HCI_ACL_HDR_SIZE) { + struct hci_acl_hdr *h = data; + len = HCI_ACL_HDR_SIZE + __le16_to_cpu(h->dlen); + } else + return -EILSEQ; + break; + + case HCI_SCODATA_PKT: + if (count >= HCI_SCO_HDR_SIZE) { + struct hci_sco_hdr *h = data; + len = HCI_SCO_HDR_SIZE + h->dlen; + } else + return -EILSEQ; + break; + } + + skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!skb) { + BT_ERR("%s no memory for packet", hdev->name); + return -ENOMEM; + } + + skb->dev = (void *) hdev; + bt_cb(skb)->pkt_type = type; + + __reassembly(hdev, type) = skb; + + scb = (void *) skb->cb; + scb->expect = len; + } else { + /* Continuation */ + + scb = (void *) skb->cb; + len = scb->expect; + } + + len = min(len, count); + + memcpy(skb_put(skb, len), data, len); + + scb->expect -= len; + + if (scb->expect == 0) { + /* Complete frame */ + + __reassembly(hdev, type) = NULL; + + bt_cb(skb)->pkt_type = type; + hci_recv_frame(skb); + } + + count -= len; data += len; + } + + return 0; +} +EXPORT_SYMBOL(hci_recv_fragment); + /* ---- Interface to upper protocols ---- */ /* Register/Unregister protocols. @@ -1029,7 +1122,7 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 ogf, __u16 ocf, __u32 plen, void *p skb = bt_skb_alloc(len, GFP_ATOMIC); if (!skb) { - BT_ERR("%s Can't allocate memory for HCI command", hdev->name); + BT_ERR("%s no memory for command", hdev->name); return -ENOMEM; } |