6 files changed, 83 insertions, 1 deletions

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 9fda7c94913f..e776ab2dc572 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -22,6 +22,7 @@
 enum amp_mgr_state {
         READ_LOC_AMP_INFO,
         READ_LOC_AMP_ASSOC,
+        READ_LOC_AMP_ASSOC_FINAL,
 };
 
 struct amp_mgr {
@@ -134,6 +135,7 @@ extern struct mutex amp_mgr_list_lock;
 
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+u8 __next_ident(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
                                        struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
@@ -141,5 +143,6 @@ void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_discover_amp(struct l2cap_chan *chan);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
+void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 8f8032965eaf..70496c07afaa 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -37,6 +37,8 @@ int phylink_gen_key(struct hci_conn *hcon, u8 *data, u8 *len, u8 *type);
 void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
 void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+                                   struct hci_conn *hcon);
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
                         struct hci_conn *hcon);
 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle);
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 0967f9e33750..ab58b81fb7c5 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -508,6 +508,8 @@ struct l2cap_chan {
         __u32           remote_acc_lat;
         __u32           remote_flush_to;
 
+        __u8            ctrl_id;
+
         struct delayed_work     chan_timer;
         struct delayed_work     retrans_timer;
         struct delayed_work     monitor_timer;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 28d1246958be..375a67f501d0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,7 +67,7 @@ void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
         kfree(cmd);
 }
 
-static u8 __next_ident(struct amp_mgr *mgr)
+u8 __next_ident(struct amp_mgr *mgr)
 {
         if (++mgr->ident == 0)
                 mgr->ident = 1;
@@ -420,6 +420,8 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
         BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
 
+        mgr->bredr_chan->ctrl_id = rsp->id;
+
         amp_create_phylink(hdev, mgr, hcon);
 
 done:
@@ -876,6 +878,43 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
         kfree(rsp);
 }
 
+void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
+{
+        struct amp_mgr *mgr;
+        struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+        struct a2mp_physlink_req *req;
+        struct l2cap_chan *bredr_chan;
+        size_t len;
+
+        mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
+        if (!mgr)
+                return;
+
+        len = sizeof(*req) + loc_assoc->len;
+
+        BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
+
+        req = kzalloc(len, GFP_KERNEL);
+        if (!req) {
+                amp_mgr_put(mgr);
+                return;
+        }
+
+        bredr_chan = mgr->bredr_chan;
+        if (!bredr_chan)
+                goto clean;
+
+        req->local_id = hdev->id;
+        req->remote_id = bredr_chan->ctrl_id;
+        memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+        a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+        amp_mgr_put(mgr);
+        kfree(req);
+}
+
 void a2mp_discover_amp(struct l2cap_chan *chan)
 {
         struct l2cap_conn *conn = chan->conn;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 5895ad0d2ac9..4f7b2647d5e9 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -233,6 +233,21 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
         hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
 
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+                                   struct hci_conn *hcon)
+{
+        struct hci_cp_read_local_amp_assoc cp;
+        struct amp_mgr *mgr = hcon->amp_mgr;
+
+        cp.phy_handle = hcon->handle;
+        cp.len_so_far = cpu_to_le16(0);
+        cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+        mgr->state = READ_LOC_AMP_ASSOC_FINAL;
+
+        /* Read Local AMP Assoc final link information data */
+        hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
 
 /* Write AMP Assoc data fragments, returns true with last fragment written*/
 static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0b7ba1e39d45..d702ba1c171c 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -901,6 +901,7 @@ static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
 a2mp_rsp:
         /* Send A2MP Rsp when all fragments are received */
         a2mp_send_getampassoc_rsp(hdev, rp->status);
+        a2mp_send_create_phy_link_req(hdev, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
@@ -3641,6 +3642,22 @@ static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb)
         }
 }
 
+static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb)
+{
+        struct hci_ev_channel_selected *ev = (void *) skb->data;
+        struct hci_conn *hcon;
+
+        BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle);
+
+        skb_pull(skb, sizeof(*ev));
+
+        hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+        if (!hcon)
+                return;
+
+        amp_read_loc_assoc_final_data(hdev, hcon);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3805,6 +3822,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
                 hci_le_meta_evt(hdev, skb);
                 break;
 
+        case HCI_EV_CHANNEL_SELECTED:
+                hci_chan_selected_evt(hdev, skb);
+                break;
+
         case HCI_EV_REMOTE_OOB_DATA_REQUEST:
                 hci_remote_oob_data_request_evt(hdev, skb);
                 break;