Bluetooth: AMP: Handle Accept phylink command status evt

When receiving HCI Command Status event for Accept Physical Link
execute HCI Write Remote AMP Assoc with data saved from A2MP Create
Physical Link Request.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 1b06d7b01359d169635446f807a8b5180974288f..b1e54903dd42966528dc9f195482ad9c8d1d73e8 100644 (file)
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -25,6 +25,7 @@ struct amp_ctrl {
 };
 
 int amp_ctrl_put(struct amp_ctrl *ctrl);
+void amp_ctrl_get(struct amp_ctrl *ctrl);
 struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr);
 struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id);
 void amp_ctrl_list_flush(struct amp_mgr *mgr);

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index dbfdbbb9707c3f0162832cea0fef30d5cf08cc0d..47565d28b27feea1d8861ce4171318ca6be4c8ac 100644 (file)
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -438,6 +438,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
        struct a2mp_physlink_rsp rsp;
        struct hci_dev *hdev;
        struct hci_conn *hcon;
+       struct amp_ctrl *ctrl;
 
        if (le16_to_cpu(hdr->len) < sizeof(*req))
                return -EINVAL;
@@ -453,6 +454,37 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
                goto send_rsp;
        }
 
+       ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
+       if (!ctrl) {
+               ctrl = amp_ctrl_add(mgr);
+               if (ctrl) {
+                       amp_ctrl_get(ctrl);
+               } else {
+                       rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+                       goto send_rsp;
+               }
+       }
+
+       if (ctrl) {
+               u8 *assoc, assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
+
+               ctrl->id = rsp.remote_id;
+
+               assoc = kzalloc(assoc_len, GFP_KERNEL);
+               if (!assoc) {
+                       amp_ctrl_put(ctrl);
+                       return -ENOMEM;
+               }
+
+               memcpy(assoc, req->amp_assoc, assoc_len);
+               ctrl->assoc = assoc;
+               ctrl->assoc_len = assoc_len;
+               ctrl->assoc_rem_len = assoc_len;
+               ctrl->assoc_len_so_far = 0;
+
+               amp_ctrl_put(ctrl);
+       }
+
        hcon = phylink_add(hdev, mgr, req->local_id);
        if (hcon) {
                amp_accept_phylink(hdev, mgr, hcon);

diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 845e43073c40b6fb241dbb7644a5bc7f527820cc..5dab2d1c7c8284f2896b08cbbc00507684f5a2ff 100644 (file)
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -19,7 +19,7 @@
 #include <crypto/hash.h>
 
 /* Remote AMP Controllers interface */
-static void amp_ctrl_get(struct amp_ctrl *ctrl)
+void amp_ctrl_get(struct amp_ctrl *ctrl)
 {
        BT_DBG("ctrl %p orig refcnt %d", ctrl,
               atomic_read(&ctrl->kref.refcount));

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index d702ba1c171c2588a29687361483347669863096..7e716698fe6459dc0e2d7a4000758b2b4adfb3e2 100644 (file)
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1729,6 +1729,22 @@ static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
        amp_write_remote_assoc(hdev, cp->phy_handle);
 }
 
+static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
+{
+       struct hci_cp_accept_phy_link *cp;
+
+       BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+       if (status)
+               return;
+
+       cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK);
+       if (!cp)
+               return;
+
+       amp_write_remote_assoc(hdev, cp->phy_handle);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
        __u8 status = *((__u8 *) skb->data);
@@ -2551,6 +2567,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                hci_cs_create_phylink(hdev, ev->status);
                break;
 
+       case HCI_OP_ACCEPT_PHY_LINK:
+               hci_cs_accept_phylink(hdev, ev->status);
+               break;
+
        default:
                BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                break;