diff options
| -rw-r--r-- | include/net/bluetooth/hci_core.h | 3 | ||||
| -rw-r--r-- | net/bluetooth/hci_core.c | 102 |
2 files changed, 105 insertions, 0 deletions
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index 89eda2ef2380..755743d508aa 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -1057,6 +1057,9 @@ int hci_req_run(struct hci_request *req, hci_req_complete_t complete); void hci_req_add(struct hci_request *req, u16 opcode, u32 plen, void *param); void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status); +struct sk_buff *__hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen, + void *param, u32 timeout); + int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param); void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags); void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb); diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c index 7c323bd112ff..8b2d543fb143 100644 --- a/net/bluetooth/hci_core.c +++ b/net/bluetooth/hci_core.c @@ -79,6 +79,108 @@ static void hci_req_cancel(struct hci_dev *hdev, int err) } } +struct sk_buff *hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode) +{ + struct hci_ev_cmd_complete *ev; + struct hci_event_hdr *hdr; + struct sk_buff *skb; + + hci_dev_lock(hdev); + + skb = hdev->recv_evt; + hdev->recv_evt = NULL; + + hci_dev_unlock(hdev); + + if (!skb) + return ERR_PTR(-ENODATA); + + if (skb->len < sizeof(*hdr)) { + BT_ERR("Too short HCI event"); + goto failed; + } + + hdr = (void *) skb->data; + skb_pull(skb, HCI_EVENT_HDR_SIZE); + + if (hdr->evt != HCI_EV_CMD_COMPLETE) { + BT_DBG("Last event is not cmd complete (0x%2.2x)", hdr->evt); + goto failed; + } + + if (skb->len < sizeof(*ev)) { + BT_ERR("Too short cmd_complete event"); + goto failed; + } + + ev = (void *) skb->data; + skb_pull(skb, sizeof(*ev)); + + if (opcode == __le16_to_cpu(ev->opcode)) + return skb; + + BT_DBG("opcode doesn't match (0x%2.2x != 0x%2.2x)", opcode, + __le16_to_cpu(ev->opcode)); + +failed: + kfree_skb(skb); + return ERR_PTR(-ENODATA); +} + +struct sk_buff *__hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen, + void *param, u32 timeout) +{ + DECLARE_WAITQUEUE(wait, current); + struct hci_request req; + int err = 0; + + BT_DBG("%s", hdev->name); + + hci_req_init(&req, hdev); + + hci_req_add(&req, opcode, plen, param); + + hdev->req_status = HCI_REQ_PEND; + + err = hci_req_run(&req, hci_req_sync_complete); + if (err < 0) + return ERR_PTR(err); + + add_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_INTERRUPTIBLE); + + schedule_timeout(timeout); + + remove_wait_queue(&hdev->req_wait_q, &wait); + + if (signal_pending(current)) + return ERR_PTR(-EINTR); + + switch (hdev->req_status) { + case HCI_REQ_DONE: + err = -bt_to_errno(hdev->req_result); + break; + + case HCI_REQ_CANCELED: + err = -hdev->req_result; + break; + + default: + err = -ETIMEDOUT; + break; + } + + hdev->req_status = hdev->req_result = 0; + + BT_DBG("%s end: err %d", hdev->name, err); + + if (err < 0) + return ERR_PTR(err); + + return hci_get_cmd_complete(hdev, opcode); +} +EXPORT_SYMBOL(__hci_cmd_sync); + /* Execute request and wait for completion. */ static int __hci_req_sync(struct hci_dev *hdev, void (*func)(struct hci_request *req, |