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>

1 files changed, 32 insertions, 0 deletions

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index dbfdbbb9707c..47565d28b27f 100644
--- 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);