diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h index 1b06d7b01359d169635446f807a8b5180974288f..b1e54903dd42966528dc9f195482ad9c8d1d73e8 100644 --- a/include/net/bluetooth/amp.h +++ b/include/net/bluetooth/amp.h @@ -25,6 +25,7 @@ struct amp_ctrl { }; int amp_ctrl_put(struct amp_ctrl *ctrl); +void amp_ctrl_get(struct amp_ctrl *ctrl); struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr); struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id); void amp_ctrl_list_flush(struct amp_mgr *mgr); diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c index dbfdbbb9707c3f0162832cea0fef30d5cf08cc0d..47565d28b27feea1d8861ce4171318ca6be4c8ac 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); diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c index 845e43073c40b6fb241dbb7644a5bc7f527820cc..5dab2d1c7c8284f2896b08cbbc00507684f5a2ff 100644 --- a/net/bluetooth/amp.c +++ b/net/bluetooth/amp.c @@ -19,7 +19,7 @@ #include /* Remote AMP Controllers interface */ -static void amp_ctrl_get(struct amp_ctrl *ctrl) +void amp_ctrl_get(struct amp_ctrl *ctrl) { BT_DBG("ctrl %p orig refcnt %d", ctrl, atomic_read(&ctrl->kref.refcount)); diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index d702ba1c171c2588a29687361483347669863096..7e716698fe6459dc0e2d7a4000758b2b4adfb3e2 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -1729,6 +1729,22 @@ static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status) amp_write_remote_assoc(hdev, cp->phy_handle); } +static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_accept_phy_link *cp; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK); + if (!cp) + return; + + amp_write_remote_assoc(hdev, cp->phy_handle); +} + static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) { __u8 status = *((__u8 *) skb->data); @@ -2551,6 +2567,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb) hci_cs_create_phylink(hdev, ev->status); break; + case HCI_OP_ACCEPT_PHY_LINK: + hci_cs_accept_phylink(hdev, ev->status); + break; + default: BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode); break;