3 files changed, 72 insertions, 5 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);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index c4e10e656c68..14cad155af3c 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1835,6 +1835,11 @@ static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
        amp_write_remote_assoc(hdev, cp->phy_handle);
 }
+static void hci_cs_create_logical_link(struct hci_dev *hdev, u8 status)
+{
+        BT_DBG("%s status 0x%2.2x", hdev->name, status);
+}
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
        __u8 status = *((__u8 *) skb->data);
@@ -2669,6 +2674,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
                hci_cs_accept_phylink(hdev, ev->status);
                break;
+        case HCI_OP_CREATE_LOGICAL_LINK:
+                hci_cs_create_logical_link(hdev, ev->status);
+                break;
        default:
                BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
                break;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 782e49c97e7e..ecc5020c9242 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -38,6 +38,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 bool disable_ertm;
@@ -1013,6 +1014,12 @@ static bool __amp_capable(struct l2cap_chan *chan)
                return false;
 }
+static bool l2cap_check_efs(struct l2cap_chan *chan)
+{
+        /* Check EFS parameters */
+        return true;
+}
 void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
        struct l2cap_conn *conn = chan->conn;
@@ -3957,13 +3964,15 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn,
                                goto done;
                        }
-                        /* check compatibility */
+                        if (!chan->ctrl_id) {
-                        if (!chan->ctrl_id)
                                l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
                                                        0);
-                        else
+                        } else {
-                                chan->ident = cmd->ident;
+                                if (l2cap_check_efs(chan)) {
+                                        amp_create_logical_link(chan);
+                                        chan->ident = cmd->ident;
+                                }
+                        }
                }
                goto done;

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,
372		372
373	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);	373	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
374	}	374	}
		375
		376	void amp_create_logical_link(struct l2cap_chan *chan)
		377	{
		378	struct hci_cp_create_accept_logical_link cp;
		379	struct hci_conn *hcon;
		380	struct hci_dev *hdev;
		381
		382	BT_DBG("chan %p", chan);
		383
		384	if (!chan->hs_hcon)
		385	return;
		386
		387	hdev = hci_dev_hold(chan->hs_hcon->hdev);
		388	if (!hdev)
		389	return;
		390
		391	BT_DBG("chan %p ctrl_id %d dst %pMR", chan, chan->ctrl_id,
		392	chan->conn->dst);
		393
		394	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, chan->conn->dst);
		395	if (!hcon)
		396	goto done;
		397
		398	cp.phy_handle = hcon->handle;
		399
		400	cp.tx_flow_spec.id = chan->local_id;
		401	cp.tx_flow_spec.stype = chan->local_stype;
		402	cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu);
		403	cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime);
		404	cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat);
		405	cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to);
		406
		407	cp.rx_flow_spec.id = chan->remote_id;
		408	cp.rx_flow_spec.stype = chan->remote_stype;
		409	cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu);
		410	cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime);
		411	cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat);
		412	cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to);
		413
		414	if (hcon->out)
		415	hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp),
		416	&cp);
		417	else
		418	hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp),
		419	&cp);
		420
		421	done:
		422	hci_dev_put(hdev);
		423	}


diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index c4e10e656c68..14cad155af3c 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c
@@ -1835,6 +1835,11 @@ static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status)
1835	amp_write_remote_assoc(hdev, cp->phy_handle);	1835	amp_write_remote_assoc(hdev, cp->phy_handle);
1836	}	1836	}
1837		1837
		1838	static void hci_cs_create_logical_link(struct hci_dev *hdev, u8 status)
		1839	{
		1840	BT_DBG("%s status 0x%2.2x", hdev->name, status);
		1841	}
		1842
1838	static void hci_inquiry_complete_evt(struct hci_dev hdev, struct sk_buff skb)	1843	static void hci_inquiry_complete_evt(struct hci_dev hdev, struct sk_buff skb)
1839	{	1844	{
1840	__u8 status = ((__u8 ) skb->data);	1845	__u8 status = ((__u8 ) skb->data);
@@ -2669,6 +2674,10 @@ static void hci_cmd_status_evt(struct hci_dev hdev, struct sk_buff skb)
2669	hci_cs_accept_phylink(hdev, ev->status);	2674	hci_cs_accept_phylink(hdev, ev->status);
2670	break;	2675	break;
2671		2676
		2677	case HCI_OP_CREATE_LOGICAL_LINK:
		2678	hci_cs_create_logical_link(hdev, ev->status);
		2679	break;
		2680
2672	default:	2681	default:
2673	BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);	2682	BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
2674	break;	2683	break;


diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 782e49c97e7e..ecc5020c9242 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c
@@ -38,6 +38,7 @@
38	#include <net/bluetooth/l2cap.h>	38	#include <net/bluetooth/l2cap.h>
39	#include <net/bluetooth/smp.h>	39	#include <net/bluetooth/smp.h>
40	#include <net/bluetooth/a2mp.h>	40	#include <net/bluetooth/a2mp.h>
		41	#include <net/bluetooth/amp.h>
41		42
42	bool disable_ertm;	43	bool disable_ertm;
43		44
@@ -1013,6 +1014,12 @@ static bool __amp_capable(struct l2cap_chan *chan)
1013	return false;	1014	return false;
1014	}	1015	}
1015		1016
		1017	static bool l2cap_check_efs(struct l2cap_chan *chan)
		1018	{
		1019	/* Check EFS parameters */
		1020	return true;
		1021	}
		1022
1016	void l2cap_send_conn_req(struct l2cap_chan *chan)	1023	void l2cap_send_conn_req(struct l2cap_chan *chan)
1017	{	1024	{
1018	struct l2cap_conn *conn = chan->conn;	1025	struct l2cap_conn *conn = chan->conn;
@@ -3957,13 +3964,15 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn,
3957	goto done;	3964	goto done;
3958	}	3965	}
3959		3966
3960	/* check compatibility */	3967	if (!chan->ctrl_id) {
3961
3962	if (!chan->ctrl_id)
3963	l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,	3968	l2cap_send_efs_conf_rsp(chan, buf, cmd->ident,
3964	0);	3969	0);
3965	else	3970	} else {
3966	chan->ident = cmd->ident;	3971	if (l2cap_check_efs(chan)) {
		3972	amp_create_logical_link(chan);
		3973	chan->ident = cmd->ident;
		3974	}
		3975	}
3967	}	3976	}
3968	goto done;	3977	goto done;
3969		3978