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;