Bluetooth: AMP: Process Chan Selected event

Channel Selected event indicates that link information data is available.
Read it with Read Local AMP Assoc command. The data shall be sent in the
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/a2mp.h b/include/net/bluetooth/a2mp.h
index 9fda7c94913f87c8c53c1c4217f18dca207ed981..e776ab2dc572d12551534a14a5a21e1fe3f167fa 100644 (file)
--- 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 8f8032965eafe35665f95979e791d7e72edb287a..70496c07afaa7e56581ce0e0aee7308871ec7314 100644 (file)
--- 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 0967f9e337505eb9a64a43221a408455a8437a29..ab58b81fb7c5280e3fe710a2020cfc115ea5fc05 100644 (file)
--- 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 28d1246958befbd6e624d0299111081b70daa729..375a67f501d034ced410f92b7edbb607fab66c4b 100644 (file)
--- 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 5895ad0d2ac9de42cfc4a9e160229acabe70ba26..4f7b2647d5e91de823691c3ee45c0657b60e7d94 100644 (file)
--- 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 0b7ba1e39d45d4bd0e8aff1c748a6b169712b509..d702ba1c171c2588a29687361483347669863096 100644 (file)
--- 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;