Bluetooth: AMP: Add Logical Link Create function

After physical link is created logical link needs to be created.
The process starts after L2CAP channel is created and L2CAP
Configuration Response with result PENDING is received.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Acked-by: Marcel Holtmann <marcel@holtmann.org>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>

1 files changed, 49 insertions, 0 deletions

diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 231d7ef53ecb..fbb63605a27e 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -372,3 +372,52 @@ void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 
         hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
 }
+
+void amp_create_logical_link(struct l2cap_chan *chan)
+{
+        struct hci_cp_create_accept_logical_link cp;
+        struct hci_conn *hcon;
+        struct hci_dev *hdev;
+
+        BT_DBG("chan %p", chan);
+
+        if (!chan->hs_hcon)
+                return;
+
+        hdev = hci_dev_hold(chan->hs_hcon->hdev);
+        if (!hdev)
+                return;
+
+        BT_DBG("chan %p ctrl_id %d dst %pMR", chan, chan->ctrl_id,
+               chan->conn->dst);
+
+        hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, chan->conn->dst);
+        if (!hcon)
+                goto done;
+
+        cp.phy_handle = hcon->handle;
+
+        cp.tx_flow_spec.id = chan->local_id;
+        cp.tx_flow_spec.stype = chan->local_stype;
+        cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
+        cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
+        cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
+        cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
+
+        cp.rx_flow_spec.id = chan->remote_id;
+        cp.rx_flow_spec.stype = chan->remote_stype;
+        cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
+        cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
+        cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
+        cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
+
+        if (hcon->out)
+                hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
+                             &cp);
+        else
+                hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
+                             &cp);
+
+done:
+        hci_dev_put(hdev);
+}