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/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index dbfdbbb..47565d2 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -438,6 +438,7 @@
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 @@
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);