[RFCv0 00/19] Bluetooth: Create AMP physical link

[RFCv0 00/19] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Andrei Emeltchenko (18):
  Bluetooth: Define AMP controller statuses
  Bluetooth: Do not shadow hdr variable
  Bluetooth: Fix processing A2MP chan in security_cfm
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: Add callback clear to ops->teardown
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create phy link after A2MP Assoc rsp
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |    9 ++
 include/net/bluetooth/amp.h      |   25 +++
 include/net/bluetooth/hci.h      |   11 ++
 include/net/bluetooth/hci_core.h |   35 ++++
 include/net/bluetooth/l2cap.h    |    1 +
 include/net/bluetooth/pal.h      |   57 +++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  310 ++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  214 +++++++++++++++++++++++++
 net/bluetooth/hci_core.c         |  144 +++++++++++++++++
 net/bluetooth/hci_event.c        |   58 ++++++-
 net/bluetooth/l2cap_core.c       |   41 ++++-
 net/bluetooth/pal.c              |  328 ++++++++++++++++++++++++++++++++++++++
 13 files changed, 1203 insertions(+), 32 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[RFCv0 01/19] Bluetooth: Define AMP controller statuses

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

AMP status codes copied from Bluez patch sent by Peter Krystad
<pkrystad@codeaurora.org>.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |    9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index ccd723e..7f19556 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -62,6 +62,15 @@
 /* First BR/EDR Controller shall have ID = 0 */
 #define HCI_BREDR_ID	0
 
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_RESET_ON_CLOSE,
-- 
1.7.9.5

[RFCv0 02/19] Bluetooth: Do not shadow hdr variable

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Fix compile warnings below:

...
net/bluetooth/a2mp.c:505:33: warning: symbol 'hdr' shadows an earlier one
net/bluetooth/a2mp.c:498:25: originally declared here
...

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ec1ef2e..20cbbec 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -325,15 +325,17 @@ static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
-	struct a2mp_cmd *hdr = (void *) skb->data;
+	struct a2mp_cmd *hdr;
 	struct amp_mgr *mgr = chan->data;
 	int err = 0;
 
 	amp_mgr_get(mgr);
 
 	while (skb->len >= sizeof(*hdr)) {
-		struct a2mp_cmd *hdr = (void *) skb->data;
-		u16 len = le16_to_cpu(hdr->len);
+		u16 len;
+
+		hdr = (void *) skb->data;
+		len = le16_to_cpu(hdr->len);
 
 		BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
 
@@ -393,7 +395,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 
 	if (err) {
 		struct a2mp_cmd_rej rej;
+
 		rej.reason = __constant_cpu_to_le16(0);
+		hdr = (void *) skb->data;
 
 		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
 
-- 
1.7.9.5

[RFCv0 03/19] Bluetooth: Fix processing A2MP chan in security_cfm

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Do not process A2MP channel in l2cap_security_cfm

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 2f7995e..b01f9d1 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -5465,6 +5465,11 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		BT_DBG("chan->scid %d", chan->scid);
 
+		if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+			l2cap_chan_unlock(chan);
+			continue;
+		}
+
 		if (chan->scid == L2CAP_CID_LE_DATA) {
 			if (!status && encrypt) {
 				chan->sec_level = hcon->sec_level;
-- 
1.7.9.5

[RFCv0 04/19] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   21 +++++++++
 net/bluetooth/hci_core.c         |   88 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4f7c866..55b89d1 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -130,6 +130,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -242,6 +253,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1094,5 +1108,12 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 5ad508e..e3cca20 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,6 +36,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1645,6 +1646,7 @@ struct hci_dev *hci_alloc_dev(void)
 
 	mutex_init(&hdev->lock);
 	mutex_init(&hdev->req_lock);
+	mutex_init(&hdev->cb_list_lock);
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 	INIT_LIST_HEAD(&hdev->blacklist);
@@ -1652,6 +1654,7 @@ struct hci_dev *hci_alloc_dev(void)
 	INIT_LIST_HEAD(&hdev->link_keys);
 	INIT_LIST_HEAD(&hdev->long_term_keys);
 	INIT_LIST_HEAD(&hdev->remote_oob_data);
+	INIT_LIST_HEAD(&hdev->cb_list);
 
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
@@ -1817,6 +1820,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2118,6 +2122,90 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		      void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+		      void *opt,
+		      void (*destructor)(struct hci_dev *hdev,
+					 struct hci_cb_cmd *cmd),
+		      gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s: remove cmd %p", hdev->name, cmd);
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(hdev, cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv0 05/19] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use workqueue to process HCI callbacks.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    3 ++
 net/bluetooth/hci_core.c         |   57 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 55b89d1..6a3146c 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1115,5 +1115,8 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
 	       gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue);
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index e3cca20..9b721ea 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2163,6 +2163,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("%s: queue cmd %p", hdev->name, cmd);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	BT_DBG("%s: remove cmd %p", hdev->name, cmd);
@@ -2206,6 +2247,22 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	return hci_send_cmd(hdev, opcode, plen, param);
 }
 
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = hci_find_cb(hdev, opcode);
+	if (!cmd)
+		return;
+
+	hci_dev_lock(hdev);
+
+	cmd->status = status;
+	hci_queue_cb(hdev, cmd, hdev->workqueue);
+
+	hci_dev_unlock(hdev);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv0 06/19] Bluetooth: Add callback clear to ops->teardown

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

ops->teardown takes care about deleting queued callbacks for all AMP
controllers.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   17 ++++++++++++++++-
 net/bluetooth/hci_core.c         |    3 +--
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a3146c..d52e86b 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1118,5 +1118,6 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
 		  struct workqueue_struct *workqueue);
 void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
+void hci_cb_clear(struct hci_dev *hdev);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 20cbbec..2ea3dcf 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -444,16 +444,31 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
+static void a2mp_chan_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	struct hci_dev *hdev, *tmp;
+
+	BT_DBG("chan %p", chan);
+
+	list_for_each_entry_safe(hdev, tmp, &hci_dev_list, list) {
+		hci_dev_hold(hdev);
+		/* Iterate through AMP controllers */
+		if (hdev->amp_type == HCI_AMP)
+			hci_cb_clear(hdev);
+		hci_dev_put(hdev);
+	}
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
+	.teardown = a2mp_chan_teardown_cb,
 
 	/* Not implemented for A2MP */
 	.new_connection = l2cap_chan_no_new_connection,
-	.teardown = l2cap_chan_no_teardown,
 	.ready = l2cap_chan_no_ready,
 };
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 9b721ea..a36e08f 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,7 +36,6 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
-static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -2220,7 +2219,7 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 	}
 }
 
-static void hci_cb_clear(struct hci_dev *hdev)
+void hci_cb_clear(struct hci_dev *hdev)
 {
 	struct hci_cb_cmd *cmd, *tmp;
 
-- 
1.7.9.5

[RFCv0 07/19] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Adds Read Local AMP Info HCI command with callback to be executed
when receiving command complete event.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |   19 +++++++++++++
 net/bluetooth/Makefile      |    2 +-
 net/bluetooth/a2mp.c        |   25 ++++++++---------
 net/bluetooth/amp.c         |   63 +++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |    6 ++++-
 5 files changed, 101 insertions(+), 14 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..ec7bea7
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2ea3dcf..f11feb0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -189,24 +190,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
-	}
+	if (!hdev)
+		goto send_err;
 
-	if (hdev)
+	if (hdev->amp_type != HCI_BREDR) {
+		amp_read_loc_info(hdev, mgr);
+		goto done;
+	} else {
 		hci_dev_put(hdev);
+	}
+
+send_err:
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..4aea7be
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,63 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct a2mp_info_rsp rsp;
+
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+}
+
+static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	hci_dev_put(hdev);
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index f4c679b..9b34417 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -29,6 +29,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -845,7 +846,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto process_cb;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -859,6 +860,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+process_cb:
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[RFCv0 08/19] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Adds reading Local AMP Assoc in HCI callback

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    1 +
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 ++++++
 net/bluetooth/a2mp.c             |   16 +++++------
 net/bluetooth/amp.c              |   59 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
 7 files changed, 121 insertions(+), 8 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..ec77ddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ec7bea7..e861675 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 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);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 7f19556..2b19703 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index d52e86b..fb0b4e4 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -141,6 +141,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -194,6 +200,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f11feb0..31545d4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -38,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -231,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 4aea7be..ebe1fd7 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -61,3 +61,62 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
 		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		amp_read_loc_assoc_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 9b34417..cbf4983 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -865,6 +866,42 @@ process_cb:
 	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto process_cb;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+process_cb:
+	/* Run callback when all fragments received */
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2294,6 +2331,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[RFCv0 09/19] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 31545d4..517b631 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -161,6 +169,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -378,8 +435,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv0 10/19] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside mgr structure.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   44 ++++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  135 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 189 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ec77ddc..012f573 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,9 @@ struct amp_mgr {
 	__u8			ident;
 	__u8			handle;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..28bfc7e
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,44 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *hci_phylink_add(struct amp_mgr *mgr, u8 local_id,
+				 u8 remote_id, u8 *rem_assoc, u16 assoc_size);
+struct phy_link *hci_phylink_lookup(struct amp_mgr *mgr, u8 local_id,
+				    u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void hci_phylink_list_flush(struct amp_mgr *mgr);
+int phylink_security(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 517b631..efc4af8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -594,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	hci_phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -628,6 +630,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..5dcc448
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,135 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p refcnt %d -> %d", plink,
+	       atomic_read(&plink->kref.refcount),
+	       atomic_read(&plink->kref.refcount) + 1);
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p refcnt %d -> %d", plink,
+	       atomic_read(&plink->kref.refcount),
+	       atomic_read(&plink->kref.refcount) - 1);
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+struct phy_link *hci_phylink_add(struct amp_mgr *mgr, u8 local_id,
+				 u8 remote_id, u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void hci_phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void hci_phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *hci_phylink_lookup(struct amp_mgr *mgr, u8 local_id,
+				    u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[RFCv0 11/19] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers handling functions.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 +++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   93 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 115 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 012f573..8ba236c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -29,6 +29,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 28bfc7e..8bac38b 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *hci_phylink_add(struct amp_mgr *mgr, u8 local_id,
 				 u8 remote_id, u8 *rem_assoc, u16 assoc_size);
 struct phy_link *hci_phylink_lookup(struct amp_mgr *mgr, u8 local_id,
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index efc4af8..d52a843 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	amp_ctrl_list_flush(mgr);
 	hci_phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -634,6 +635,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 5dcc448..35c9934 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,99 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p refcnt %d -> %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount),
+	       atomic_read(&ctrl->kref.refcount) + 1);
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p refcnt %d -> %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount),
+	       atomic_read(&ctrl->kref.refcount) - 1);
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p: ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+static void amp_ctrl_del(struct amp_mgr *mgr, struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p", ctrl);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_del(&ctrl->list);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	amp_ctrl_put(ctrl);
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p: id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

[RFCv0 12/19] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d52a843..ee3ebe8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -309,6 +309,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -326,6 +327,12 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = hci_phylink_add(mgr, rsp.local_id, rsp.remote_id,
+				req->amp_assoc,
+				le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -345,6 +352,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -361,8 +369,18 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = hci_phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[RFCv0 13/19] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ee3ebe8..4cfc80f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -269,6 +269,35 @@ done:
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status %d", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -458,8 +487,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv0 14/19] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   59 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 58 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4cfc80f..5ff8f1a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -331,6 +331,60 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp =
+				(struct a2mp_amp_assoc_rsp *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+	u8 *rem_assoc;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status %d assoc len %d", rsp->id, rsp->status, len);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	rem_assoc = skb_pull(skb, sizeof(*rsp));
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			goto done;
+		}
+
+		memcpy(assoc, rsp->amp_assoc, assoc_len);
+		ctrl->assoc = assoc;
+		ctrl->assoc_len = assoc_len;
+
+		amp_ctrl_put(ctrl);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		goto done;
+
+	BT_DBG("Creating phylink %d -> %d", hdev->id, rsp->id);
+
+	plink = hci_phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+				ctrl->assoc_len);
+
+done:
+	skb_pull(skb, len - sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -491,8 +545,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[RFCv0 15/19] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 net/bluetooth/a2mp.c         |   22 ++++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |   34 ++++++++++++++++++++++++++++++----
 3 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 8ba236c..68866f4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -129,5 +129,6 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void l2cap_discover_amp(struct l2cap_conn *conn);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5ff8f1a..a4df006 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -766,3 +766,25 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+void l2cap_discover_amp(struct l2cap_conn *conn)
+{
+	struct a2mp_discov_req req;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+
+	BT_DBG("%p", conn);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn);
+		if (!mgr)
+			return;
+	}
+
+	amp_mgr_get(mgr);
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b01f9d1..a5256e7 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -989,6 +989,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1015,6 +1027,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable", chan);
+		l2cap_discover_amp(chan->conn);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1029,8 +1051,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1126,7 +1149,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5499,7 +5522,10 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				/* Unlock chan list since we add A2MP chan */
+				mutex_unlock(&conn->chan_lock);
+				l2cap_choose_conn(chan);
+				mutex_lock(&conn->chan_lock);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[RFCv0 16/19] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   34 ++++++++++++++++++++++++++++++++++
 1 file changed, 34 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 35c9934..1c74912 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -226,3 +226,37 @@ struct phy_link *hci_phylink_lookup(struct amp_mgr *mgr, u8 local_id,
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed");
+		return PTR_ERR(tfm);
+	}
+
+	if (ksize) {
+		ret = crypto_shash_setkey(tfm, key, ksize);
+		if (ret) {
+			BT_DBG("crypto_ahash_setkey failed");
+		} else {
+			struct {
+				struct shash_desc shash;
+				char ctx[crypto_shash_descsize(tfm)];
+			} desc;
+
+			desc.shash.tfm = tfm;
+			desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+			ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+						  output);
+		}
+	}
+
+	BT_DBG("ret 0x%x", ret);
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

Re: [RFCv0 16/19] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

Hi Mat,

On Fri, Jun 29, 2012 at 05:46:49PM +0300, Andrei Emeltchenko wrote:
> From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
> 
> hmac(sha256) will be used for AMP key generation.

What do you think about this approach for key generation?

Best regards 
Andrei Emeltchenko 

Re: [RFCv0 16/19] Bluetooth: Add function to derive AMP key using hmac

[Mat Martineau]

Andrei -

On Thu, 12 Jul 2012, Andrei Emeltchenko wrote:

> Hi Mat,
>
> On Fri, Jun 29, 2012 at 05:46:49PM +0300, Andrei Emeltchenko wrote:
>> From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
>>
>> hmac(sha256) will be used for AMP key generation.
>
> What do you think about this approach for key generation?

I think it makes a lot of sense to use the synchronous API for 
generating the key.  As long as it generates the right key, this code 
looks good to me.


--
Mat Martineau
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

Re: [RFCv0 16/19] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

Hi Mat,

On Thu, Jul 12, 2012 at 04:55:35PM -0700, Mat Martineau wrote:
> 
> Andrei -
> 
> On Thu, 12 Jul 2012, Andrei Emeltchenko wrote:
> 
> >Hi Mat,
> >
> >On Fri, Jun 29, 2012 at 05:46:49PM +0300, Andrei Emeltchenko wrote:
> >>From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
> >>
> >>hmac(sha256) will be used for AMP key generation.
> >
> >What do you think about this approach for key generation?
> 
> I think it makes a lot of sense to use the synchronous API for
> generating the key.  As long as it generates the right key, this
> code looks good to me.

Thanks for looking at it. Dmitry told me that ahash uses shash internally
anyway so there is no change.

BTW: Do you have some comment concerning the code?

Best regards 
Andrei Emeltchenko 

Re: [RFCv0 16/19] Bluetooth: Add function to derive AMP key using hmac

[Mat Martineau]

Andrei -

On Fri, 13 Jul 2012, Andrei Emeltchenko wrote:

> Hi Mat,
>
> On Thu, Jul 12, 2012 at 04:55:35PM -0700, Mat Martineau wrote:
>>
>> Andrei -
>>
>> On Thu, 12 Jul 2012, Andrei Emeltchenko wrote:
>>
>>> Hi Mat,
>>>
>>> On Fri, Jun 29, 2012 at 05:46:49PM +0300, Andrei Emeltchenko wrote:
>>>> From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
>>>>
>>>> hmac(sha256) will be used for AMP key generation.
>>>
>>> What do you think about this approach for key generation?
>>
>> I think it makes a lot of sense to use the synchronous API for
>> generating the key.  As long as it generates the right key, this
>> code looks good to me.
>
> Thanks for looking at it. Dmitry told me that ahash uses shash internally
> anyway so there is no change.
>
> BTW: Do you have some comment concerning the code?

No comments beyond the sync/async one.  I'm not very familiar with 
the crypto APIs or AMP key generation.


--
Mat Martineau
Employee of Qualcomm Innovation Center, Inc.
Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

[RFCv0 17/19] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP key using hmac_sha256 helper.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   64 +++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 64 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 1c74912..a020f97 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -260,3 +260,67 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+static void show_key(u8 *k)
+{
+	int i = 0;
+	for (i = 0; i < 32; i += 8)
+		BT_DBG("\t%02x %02x %02x %02x %02x %02x %02x %02x",
+		       *(k+i+0), *(k+i+1), *(k+i+2), *(k+i+3),
+		       *(k+i+4), *(k+i+5), *(k+i+6), *(k+i+7));
+}
+
+int phylink_security(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	u8 b802_key[HCI_AMP_LINK_KEY_SIZE];
+	int result;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("key_type %d", conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3)
+		return -EACCES;
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	hci_dev_lock(hdev);
+	key = hci_find_link_key(hdev, &conn->dst);
+	hci_dev_unlock(hdev);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	show_key(keybuf);
+
+	result = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4,
+			     gamp_key);
+	show_key(gamp_key);
+
+	if (result)
+		goto done;
+
+	if (conn->key_type == 3) {
+		BT_DBG("gamp_key");
+		show_key(gamp_key);
+		memcpy(data, gamp_key, 32);
+		goto done;
+	}
+
+	result = hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4,
+			     b802_key);
+	show_key(b802_key);
+
+	memcpy(data, b802_key, 32);
+
+done:
+	return result;
+}
-- 
1.7.9.5

[RFCv0 18/19] Bluetooth: AMP: Create phy link after A2MP Assoc rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create AMP Physical Link when receiving A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    1 +
 include/net/bluetooth/amp.h      |    4 ++
 include/net/bluetooth/hci_core.h |    2 +
 include/net/bluetooth/pal.h      |    1 -
 net/bluetooth/a2mp.c             |   12 +++++
 net/bluetooth/amp.c              |   94 +++++++++++++++++++++++++++++++++++++-
 net/bluetooth/hci_event.c        |   11 +++++
 net/bluetooth/pal.c              |    2 +
 8 files changed, 125 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 68866f4..98438a4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -130,5 +130,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void l2cap_discover_amp(struct l2cap_conn *conn);
+u8 __next_handle(struct amp_mgr *mgr);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index fb0b4e4..cb26d61 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -144,6 +144,8 @@ struct hci_cb_cmd {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 8bac38b..0245dc8 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a4df006..64bdad6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -72,6 +72,14 @@ static u8 __next_ident(struct amp_mgr *mgr)
 	return mgr->ident;
 }
 
+u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -379,6 +387,10 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = hci_phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 				ctrl->assoc_len);
+	if (!plink)
+		goto done;
+
+	amp_create_phylink(hdev, mgr, plink);
 
 done:
 	skb_pull(skb, len - sizeof(*rsp));
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index ebe1fd7..0e54b98 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 static void amp_read_loc_info_complete(struct hci_dev *hdev,
 				       struct hci_cb_cmd *cmd)
@@ -118,5 +119,96 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	amp_mgr_get(mgr);
 
 	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
-		amp_read_loc_assoc_complete, mgr, cb_destructor, GFP_KERNEL);
+		   amp_read_loc_assoc_complete, mgr, cb_destructor,
+		   GFP_KERNEL);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink);
+
+static void amp_write_rem_assoc_cs(struct hci_dev *hdev,
+				   struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+	u16 frag_len;
+
+	BT_DBG("mgr %p", mgr);
+
+	if (!cmd->status)
+		return;
+
+	plink = hci_phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	frag_len = min_t(u16, 248, plink->rem_assoc.rem_len);
+	plink->rem_assoc.len_so_far += frag_len;
+	plink->rem_assoc.rem_len -= frag_len;
+
+	if (plink->rem_assoc.rem_len > 0)
+		/* Send another fragment */
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+
+	phylink_put(plink);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	u16 frag_len, len;
+
+	frag_len = min_t(u16, 248, plink->rem_assoc.rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp)
+		return;
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(plink->rem_assoc.len_so_far);
+	cp->rem_len = cpu_to_le16(plink->rem_assoc.rem_len);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, &cp,
+		   amp_write_rem_assoc_cs, mgr, cb_destructor, GFP_KERNEL);
+
+	kfree(cp);
+}
+
+static void amp_create_phylink_cs(struct hci_dev *hdev,
+				  struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* Write Remote AMP Assoc */
+	plink = hci_phylink_lookup(mgr, hdev->id, 0);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+		phylink_put(plink);
+	}
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_security(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			     &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp,
+		   amp_create_phylink_cs, mgr, cb_destructor, GFP_KERNEL);
 }
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index cbf4983..e20f26e 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1698,6 +1698,13 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	hci_process_cb(hdev, HCI_OP_CREATE_PHY_LINK, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2488,6 +2495,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%x", hdev->name, opcode);
 		break;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index a020f97..899574e 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -155,6 +156,7 @@ struct phy_link *hci_phylink_add(struct amp_mgr *mgr, u8 local_id,
 	plink->local_id = local_id;
 	plink->remote_id = remote_id;
 	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
 
 	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
 	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
-- 
1.7.9.5

[RFCv0 19/19] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 6b85b00..dc6a9a3 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -783,5 +783,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 64bdad6..b8a496e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -185,6 +185,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -215,6 +216,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -224,6 +226,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a5256e7..0a04f96 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1001,7 +1001,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[RFCv1 00/20] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (19):
  Bluetooth: Define AMP controller statuses
  Bluetooth: Do not shadow hdr variable
  Bluetooth: Fix processing A2MP chan in security_cfm
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: Add callback clear to ops->teardown
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |    8 +
 include/net/bluetooth/amp.h      |   25 +++
 include/net/bluetooth/hci.h      |   11 ++
 include/net/bluetooth/hci_core.h |   35 +++++
 include/net/bluetooth/l2cap.h    |    1 +
 include/net/bluetooth/pal.h      |   57 +++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  302 +++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  214 +++++++++++++++++++++++++
 net/bluetooth/hci_core.c         |  144 +++++++++++++++++
 net/bluetooth/hci_event.c        |   58 ++++++-
 net/bluetooth/l2cap_core.c       |   41 ++++-
 net/bluetooth/pal.c              |  318 ++++++++++++++++++++++++++++++++++++++
 13 files changed, 1184 insertions(+), 32 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[RFCv1 01/20] Bluetooth: Define AMP controller statuses

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

AMP status codes copied from Bluez patch sent by Peter Krystad
<pkrystad@codeaurora.org>.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |    9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index ccd723e..7f19556 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -62,6 +62,15 @@
 /* First BR/EDR Controller shall have ID = 0 */
 #define HCI_BREDR_ID	0
 
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_RESET_ON_CLOSE,
-- 
1.7.9.5

[RFCv1 02/20] Bluetooth: Do not shadow hdr variable

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Fix compile warnings below:

...
net/bluetooth/a2mp.c:505:33: warning: symbol 'hdr' shadows an earlier one
net/bluetooth/a2mp.c:498:25: originally declared here
...

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index bc28c21..1133890 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -325,15 +325,17 @@ static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
-	struct a2mp_cmd *hdr = (void *) skb->data;
+	struct a2mp_cmd *hdr;
 	struct amp_mgr *mgr = chan->data;
 	int err = 0;
 
 	amp_mgr_get(mgr);
 
 	while (skb->len >= sizeof(*hdr)) {
-		struct a2mp_cmd *hdr = (void *) skb->data;
-		u16 len = le16_to_cpu(hdr->len);
+		u16 len;
+
+		hdr = (void *) skb->data;
+		len = le16_to_cpu(hdr->len);
 
 		BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
 
@@ -393,7 +395,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 
 	if (err) {
 		struct a2mp_cmd_rej rej;
+
 		rej.reason = __constant_cpu_to_le16(0);
+		hdr = (void *) skb->data;
 
 		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
 
-- 
1.7.9.5

[RFCv1 03/20] Bluetooth: Fix processing A2MP chan in security_cfm

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Do not process A2MP channel in l2cap_security_cfm

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    5 +++++
 1 file changed, 5 insertions(+)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 9fab1e6..e029fa7 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -5469,6 +5469,11 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 		BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid,
 		       state_to_string(chan->state));
 
+		if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+			l2cap_chan_unlock(chan);
+			continue;
+		}
+
 		if (chan->scid == L2CAP_CID_LE_DATA) {
 			if (!status && encrypt) {
 				chan->sec_level = hcon->sec_level;
-- 
1.7.9.5

[RFCv1 04/20] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   21 +++++++++
 net/bluetooth/hci_core.c         |   88 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index cda6437..4a5b62d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -130,6 +130,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -242,6 +253,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1100,5 +1114,12 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 28bab9d..0b63d82 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,6 +36,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1645,6 +1646,7 @@ struct hci_dev *hci_alloc_dev(void)
 
 	mutex_init(&hdev->lock);
 	mutex_init(&hdev->req_lock);
+	mutex_init(&hdev->cb_list_lock);
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 	INIT_LIST_HEAD(&hdev->blacklist);
@@ -1652,6 +1654,7 @@ struct hci_dev *hci_alloc_dev(void)
 	INIT_LIST_HEAD(&hdev->link_keys);
 	INIT_LIST_HEAD(&hdev->long_term_keys);
 	INIT_LIST_HEAD(&hdev->remote_oob_data);
+	INIT_LIST_HEAD(&hdev->cb_list);
 
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
@@ -1817,6 +1820,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2118,6 +2122,90 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		      void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+		      void *opt,
+		      void (*destructor)(struct hci_dev *hdev,
+					 struct hci_cb_cmd *cmd),
+		      gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s: remove cmd %p", hdev->name, cmd);
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(hdev, cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv1 05/20] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use workqueue to process HCI callbacks.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    3 ++
 net/bluetooth/hci_core.c         |   57 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4a5b62d..ee4cc06 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1121,5 +1121,8 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
 	       gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue);
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 0b63d82..7c50214 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2163,6 +2163,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("%s: queue cmd %p", hdev->name, cmd);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	BT_DBG("%s: remove cmd %p", hdev->name, cmd);
@@ -2206,6 +2247,22 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	return hci_send_cmd(hdev, opcode, plen, param);
 }
 
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = hci_find_cb(hdev, opcode);
+	if (!cmd)
+		return;
+
+	hci_dev_lock(hdev);
+
+	cmd->status = status;
+	hci_queue_cb(hdev, cmd, hdev->workqueue);
+
+	hci_dev_unlock(hdev);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv1 06/20] Bluetooth: Add callback clear to ops->teardown

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

ops->teardown takes care about deleting queued callbacks for all AMP
controllers.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   17 ++++++++++++++++-
 net/bluetooth/hci_core.c         |    3 +--
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index ee4cc06..81c001e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1124,5 +1124,6 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
 		  struct workqueue_struct *workqueue);
 void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
+void hci_cb_clear(struct hci_dev *hdev);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 1133890..f4e83fc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -444,16 +444,31 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
+static void a2mp_chan_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	struct hci_dev *hdev, *tmp;
+
+	BT_DBG("chan %p", chan);
+
+	list_for_each_entry_safe(hdev, tmp, &hci_dev_list, list) {
+		hci_dev_hold(hdev);
+		/* Iterate through AMP controllers */
+		if (hdev->amp_type == HCI_AMP)
+			hci_cb_clear(hdev);
+		hci_dev_put(hdev);
+	}
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
+	.teardown = a2mp_chan_teardown_cb,
 
 	/* Not implemented for A2MP */
 	.new_connection = l2cap_chan_no_new_connection,
-	.teardown = l2cap_chan_no_teardown,
 	.ready = l2cap_chan_no_ready,
 };
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 7c50214..bee7bd4 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,7 +36,6 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
-static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -2220,7 +2219,7 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 	}
 }
 
-static void hci_cb_clear(struct hci_dev *hdev)
+void hci_cb_clear(struct hci_dev *hdev)
 {
 	struct hci_cb_cmd *cmd, *tmp;
 
-- 
1.7.9.5

[RFCv1 07/20] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with callback to be executed upon receiving
command complete event. Callback will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 include/net/bluetooth/amp.h  |   19 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |   28 +++++++++----------
 net/bluetooth/amp.c          |   63 ++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c    |    6 +++-
 6 files changed, 103 insertions(+), 16 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..ec77ddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..ec7bea7
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f4e83fc..fb6f734 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -189,24 +189,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
-	}
+	if (!hdev)
+		goto send_err;
 
-	if (hdev)
+	if (hdev->dev_type != HCI_BREDR) {
+		amp_read_loc_info(hdev, mgr);
+		goto done;
+	} else {
 		hci_dev_put(hdev);
+	}
+
+send_err:
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..4aea7be
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,63 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct a2mp_info_rsp rsp;
+
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+}
+
+static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	hci_dev_put(hdev);
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 06996ff..7222421 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -29,6 +29,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -845,7 +846,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto process_cb;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -859,6 +860,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+process_cb:
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[RFCv1 08/20] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
A2MP Get AMP Assoc Response is run from HCI callback.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++
 net/bluetooth/a2mp.c             |   13 +++++----
 net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
 6 files changed, 120 insertions(+), 6 deletions(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ec7bea7..e861675 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 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);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 7f19556..2b19703 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 81c001e..e333e70 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -141,6 +141,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -194,6 +200,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index fb6f734..b0efe6f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -230,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 4aea7be..ff6c95c 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -61,3 +61,63 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
 		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		   amp_read_loc_assoc_complete, mgr, cb_destructor,
+		   GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 7222421..a1ad489 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -865,6 +866,42 @@ process_cb:
 	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto process_cb;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+process_cb:
+	/* Run callback when all fragments received */
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2294,6 +2331,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[RFCv1 09/20] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b0efe6f..4f5cd5d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -161,6 +169,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -378,8 +435,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv1 10/20] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside amp_mgr structure and represent AMP physical links.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   42 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  141 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 193 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ec77ddc..012f573 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,9 @@ struct amp_mgr {
 	__u8			ident;
 	__u8			handle;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..201c501
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size);
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void phylink_list_flush(struct amp_mgr *mgr);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4f5cd5d..e62099a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -594,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -628,6 +630,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..24fb3aa
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,141 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[RFCv1 11/20] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 012f573..8ba236c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -29,6 +29,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 201c501..3223ec2 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e62099a..f991fbe 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -634,6 +635,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 24fb3aa..440f5bf 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,87 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p: ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p: id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

[RFCv1 12/20] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/a2mp.c        |   19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3223ec2..6ce1dfb 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -52,5 +52,6 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f991fbe..c00fc65 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -309,6 +309,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -326,6 +327,11 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = phylink_add(mgr, rsp.local_id, rsp.remote_id, req->amp_assoc,
+			    le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -345,6 +351,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -361,8 +368,20 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+	phylink_del(mgr, plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[RFCv1 13/20] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c00fc65..99b6a74 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -269,6 +269,35 @@ done:
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status %d", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -459,8 +488,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv1 14/20] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   58 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 57 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 99b6a74..b34cad6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -331,6 +331,59 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+	u8 *rem_assoc;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status %d assoc len %d", rsp->id, rsp->status, len);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	rem_assoc = skb_pull(skb, sizeof(*rsp));
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			goto done;
+		}
+
+		memcpy(assoc, rsp->amp_assoc, assoc_len);
+		ctrl->assoc = assoc;
+		ctrl->assoc_len = assoc_len;
+
+		amp_ctrl_put(ctrl);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		goto done;
+
+	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+			    ctrl->assoc_len);
+
+	BT_DBG("Created plink %p: %d -> %d", plink, hdev->id, rsp->id);
+
+done:
+	skb_pull(skb, len - sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -492,8 +545,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[RFCv1 15/20] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 net/bluetooth/a2mp.c         |   22 ++++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |   34 ++++++++++++++++++++++++++++++----
 3 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 8ba236c..68866f4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -129,5 +129,6 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void l2cap_discover_amp(struct l2cap_conn *conn);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b34cad6..99fc770 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -766,3 +766,25 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+void l2cap_discover_amp(struct l2cap_conn *conn)
+{
+	struct a2mp_discov_req req;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+
+	BT_DBG("%p", conn);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn);
+		if (!mgr)
+			return;
+	}
+
+	amp_mgr_get(mgr);
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index e029fa7..8572737 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -987,6 +987,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1013,6 +1025,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable", chan);
+		l2cap_discover_amp(chan->conn);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1027,8 +1049,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1124,7 +1147,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5503,7 +5526,10 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				/* Unlock chan list since we add A2MP chan */
+				mutex_unlock(&conn->chan_lock);
+				l2cap_choose_conn(chan);
+				mutex_lock(&conn->chan_lock);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[RFCv1 16/20] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 440f5bf..e9bab6c 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -220,3 +221,37 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed");
+		return PTR_ERR(tfm);
+	}
+
+	if (ksize) {
+		ret = crypto_shash_setkey(tfm, key, ksize);
+		if (ret) {
+			BT_DBG("crypto_ahash_setkey failed");
+		} else {
+			struct {
+				struct shash_desc shash;
+				char ctx[crypto_shash_descsize(tfm)];
+			} desc;
+
+			desc.shash.tfm = tfm;
+			desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+			ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+						  output);
+		}
+	}
+
+	BT_DBG("ret 0x%x", ret);
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[RFCv1 17/20] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP key using hmac_sha256 helper.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/pal.c         |   61 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 6ce1dfb..8799285 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -53,5 +53,6 @@ int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
 void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index e9bab6c..dbca318 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -255,3 +255,64 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+static void hexdump(u8 *buf, size_t len)
+{
+	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,
+		       16, 1, buf, len, false);
+}
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	u8 b802_key[HCI_AMP_LINK_KEY_SIZE];
+	int result;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("key_type %d", conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3)
+		return -EACCES;
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	hci_dev_lock(hdev);
+	key = hci_find_link_key(hdev, &conn->dst);
+	hci_dev_unlock(hdev);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	hexdump(keybuf, HCI_AMP_LINK_KEY_SIZE);
+
+	result = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4,
+			     gamp_key);
+	hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
+
+	if (result)
+		goto done;
+
+	if (conn->key_type == 3) {
+		BT_DBG("gamp_key");
+		hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		goto done;
+	}
+
+	result = hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4,
+			     b802_key);
+	hexdump(b802_key, HCI_AMP_LINK_KEY_SIZE);
+
+	memcpy(data, b802_key, HCI_AMP_LINK_KEY_SIZE);
+
+done:
+	return result;
+}
-- 
1.7.9.5

[RFCv1 18/20] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define callback which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   31 +++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |   11 +++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 8799285..d9eb87e 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 99fc770..0513f66 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -376,9 +376,13 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 			    ctrl->assoc_len);
+	if (!plink)
+		goto done;
 
 	BT_DBG("Created plink %p: %d -> %d", plink, hdev->id, rsp->id);
 
+	amp_create_phylink(hdev, mgr, plink);
+
 done:
 	skb_pull(skb, len - sizeof(*rsp));
 	return 0;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index ff6c95c..89a6604 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 static void amp_read_loc_info_complete(struct hci_dev *hdev,
 				       struct hci_cb_cmd *cmd)
@@ -121,3 +122,33 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 		   amp_read_loc_assoc_complete, mgr, cb_destructor,
 		   GFP_KERNEL);
 }
+
+static void amp_create_phylink_cs(struct hci_dev *hdev,
+				  struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* Write Remote AMP Assoc */
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp,
+		   amp_create_phylink_cs, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index a1ad489..2b91b55 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1698,6 +1698,13 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	hci_process_cb(hdev, HCI_OP_CREATE_PHY_LINK, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2488,6 +2495,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[RFCv1 19/20] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    2 ++
 net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e333e70..7412085 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -144,6 +144,8 @@ struct hci_cb_cmd {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 89a6604..9f2a875 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -123,6 +123,61 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 		   GFP_KERNEL);
 }
 
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink);
+
+static void amp_write_rem_assoc_cs(struct hci_dev *hdev,
+				   struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+	u16 frag_len;
+
+	BT_DBG("mgr %p", mgr);
+
+	if (!cmd->status)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	frag_len = min_t(u16, 248, plink->rem_assoc.rem_len);
+	plink->rem_assoc.len_so_far += frag_len;
+	plink->rem_assoc.rem_len -= frag_len;
+
+	if (plink->rem_assoc.rem_len > 0)
+		/* Send another fragment */
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+
+	phylink_put(plink);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	u16 frag_len, len;
+
+	frag_len = min_t(u16, 248, plink->rem_assoc.rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp)
+		return;
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(plink->rem_assoc.len_so_far);
+	cp->rem_len = cpu_to_le16(plink->rem_assoc.rem_len);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, &cp,
+		   amp_write_rem_assoc_cs, mgr, cb_destructor, GFP_KERNEL);
+
+	kfree(cp);
+}
+
 static void amp_create_phylink_cs(struct hci_dev *hdev,
 				  struct hci_cb_cmd *cmd)
 {
@@ -132,6 +187,11 @@ static void amp_create_phylink_cs(struct hci_dev *hdev,
 	BT_DBG("mgr %p", mgr);
 
 	/* Write Remote AMP Assoc */
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+		phylink_put(plink);
+	}
 }
 
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
-- 
1.7.9.5

[RFCv1 20/20] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index e8b65ae..0632ee3 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -787,5 +787,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0513f66..f18d381 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -177,6 +177,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -207,6 +208,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -216,6 +218,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 8572737..39d1aa7 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -999,7 +999,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[RFCv2 00/20] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (19):
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: Add callback clear to ops->teardown
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Process physical link complete event
  Bluetooth: AMP: Send Create Chan Req

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   10 ++
 include/net/bluetooth/amp.h      |   25 +++
 include/net/bluetooth/hci.h      |    3 +
 include/net/bluetooth/hci_core.h |   46 ++++++
 include/net/bluetooth/l2cap.h    |    1 +
 include/net/bluetooth/pal.h      |   57 +++++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  294 ++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  308 ++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_core.c         |  144 +++++++++++++++++
 net/bluetooth/hci_event.c        |  120 +++++++++++++-
 net/bluetooth/l2cap_core.c       |   64 +++++++-
 net/bluetooth/pal.c              |  318 ++++++++++++++++++++++++++++++++++++++
 14 files changed, 1364 insertions(+), 29 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[RFCv2 01/20] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   21 +++++++++
 net/bluetooth/hci_core.c         |   88 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 109 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 41d9439..479e4d1 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -236,6 +247,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1092,5 +1106,12 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 28bab9d..165c235 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,6 +36,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1645,6 +1646,7 @@ struct hci_dev *hci_alloc_dev(void)
 
 	mutex_init(&hdev->lock);
 	mutex_init(&hdev->req_lock);
+	mutex_init(&hdev->cb_list_lock);
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 	INIT_LIST_HEAD(&hdev->blacklist);
@@ -1652,6 +1654,7 @@ struct hci_dev *hci_alloc_dev(void)
 	INIT_LIST_HEAD(&hdev->link_keys);
 	INIT_LIST_HEAD(&hdev->long_term_keys);
 	INIT_LIST_HEAD(&hdev->remote_oob_data);
+	INIT_LIST_HEAD(&hdev->cb_list);
 
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
@@ -1817,6 +1820,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2118,6 +2122,90 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		      void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+		      void *opt,
+		      void (*destructor)(struct hci_dev *hdev,
+					 struct hci_cb_cmd *cmd),
+		      gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s remove cmd %p", hdev->name, cmd);
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(hdev, cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

Re: [RFCv2 01/20] Bluetooth: General HCI callback implementation

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:42 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Add general HCI callback implementation. Can be used for executing
> HCI commands from A2MP protocol.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/hci_core.h |   21 +++++++++
>  net/bluetooth/hci_core.c         |   88 ++++++++++++++++++++++++++++++++++++++
>  2 files changed, 109 insertions(+)
> 
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 41d9439..479e4d1 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -124,6 +124,17 @@ struct le_scan_params {
>  
>  #define HCI_MAX_SHORT_NAME_LENGTH	10
>  
> +struct hci_dev;
> +
> +struct hci_cb_cmd {
> +	struct list_head list;
> +	u16 opcode;
> +	u8 status;
> +	void *opt;
> +	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
> +	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
> +};
> +
>  #define NUM_REASSEMBLY 4
>  struct hci_dev {
>  	struct list_head list;
> @@ -236,6 +247,9 @@ struct hci_dev {
>  
>  	struct list_head	mgmt_pending;
>  
> +	struct mutex		cb_list_lock;
> +	struct list_head	cb_list;
> +
>  	struct discovery_state	discovery;
>  	struct hci_conn_hash	conn_hash;
>  	struct list_head	blacklist;
> @@ -1092,5 +1106,12 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
>  int hci_cancel_le_scan(struct hci_dev *hdev);
>  
>  u8 bdaddr_to_le(u8 bdaddr_type);
> +struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
> +int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
> +	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
> +	       void *opt,
> +	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
> +	       gfp_t flags);
> +void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);

_cb usually meant that the function itself is a callback, we should put better
names here, maybe:

hci_callback_find()
hci_callback_send_cmd()
hci_callback_remove()

>  
>  #endif /* __HCI_CORE_H */
> diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
> index 28bab9d..165c235 100644
> --- a/net/bluetooth/hci_core.c
> +++ b/net/bluetooth/hci_core.c
> @@ -36,6 +36,7 @@
>  static void hci_rx_work(struct work_struct *work);
>  static void hci_cmd_work(struct work_struct *work);
>  static void hci_tx_work(struct work_struct *work);
> +static void hci_cb_clear(struct hci_dev *hdev);
>  
>  /* HCI device list */
>  LIST_HEAD(hci_dev_list);
> @@ -1645,6 +1646,7 @@ struct hci_dev *hci_alloc_dev(void)
>  
>  	mutex_init(&hdev->lock);
>  	mutex_init(&hdev->req_lock);
> +	mutex_init(&hdev->cb_list_lock);
>  
>  	INIT_LIST_HEAD(&hdev->mgmt_pending);
>  	INIT_LIST_HEAD(&hdev->blacklist);
> @@ -1652,6 +1654,7 @@ struct hci_dev *hci_alloc_dev(void)
>  	INIT_LIST_HEAD(&hdev->link_keys);
>  	INIT_LIST_HEAD(&hdev->long_term_keys);
>  	INIT_LIST_HEAD(&hdev->remote_oob_data);
> +	INIT_LIST_HEAD(&hdev->cb_list);
>  
>  	INIT_WORK(&hdev->rx_work, hci_rx_work);
>  	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
> @@ -1817,6 +1820,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
>  	hci_link_keys_clear(hdev);
>  	hci_smp_ltks_clear(hdev);
>  	hci_remote_oob_data_clear(hdev);
> +	hci_cb_clear(hdev);
>  	hci_dev_unlock(hdev);
>  
>  	hci_dev_put(hdev);
> @@ -2118,6 +2122,90 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
>  	return 0;
>  }
>  
> +static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
> +		      void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
> +		      void *opt,
> +		      void (*destructor)(struct hci_dev *hdev,
> +					 struct hci_cb_cmd *cmd),
> +		      gfp_t flags)

Then this on could be named hci_callback_add

> +{
> +	struct hci_cb_cmd *cmd;
> +
> +	cmd = kmalloc(sizeof(*cmd), flags);
> +	if (!cmd)
> +		return -ENOMEM;
> +
> +	cmd->cb = cb;
> +	cmd->opcode = opcode;
> +	cmd->opt = opt;
> +	cmd->status = 0;
> +	cmd->destructor = destructor;
> +
> +	mutex_lock(&hdev->cb_list_lock);
> +	list_add(&cmd->list, &hdev->cb_list);
> +	mutex_unlock(&hdev->cb_list_lock);

can't we do RCU here, of it is not possible spinlocks? Transverse a list is a
kind fast operation, we don't wanna pay two context switches here. It is
expensive I think.

> +
> +	return 0;
> +}
> +
> +struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
> +{
> +	struct hci_cb_cmd *cmd;
> +
> +	mutex_lock(&hdev->cb_list_lock);
> +	list_for_each_entry(cmd, &hdev->cb_list, list)
> +		if (cmd->opcode == opcode) {
> +			mutex_unlock(&hdev->cb_list_lock);
> +			return cmd;
> +		}
> +	mutex_unlock(&hdev->cb_list_lock);
> +
> +	return NULL;
> +}
> +
> +void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
> +{
> +	BT_DBG("%s remove cmd %p", hdev->name, cmd);
> +
> +	mutex_lock(&hdev->cb_list_lock);
> +	list_del(&cmd->list);
> +	mutex_unlock(&hdev->cb_list_lock);
> +
> +	if (cmd->destructor) {
> +		cmd->destructor(hdev, cmd);

So what is the purpose of the destructor_cb? A2MP calls hci_cmd_cb and
register it, then for some reason A2MP end up calling hci_remove_cb() later,
and then cmd->destructor_cb() calls a function that actually belongs to A2MP.
What is the advantage here?
Or is this meant to be static and only called from hci_cb_clear()?

> +	} else {
> +		kfree(cmd->opt);
> +		kfree(cmd);
> +	}
> +}
> +
> +static void hci_cb_clear(struct hci_dev *hdev)
> +{
> +	struct hci_cb_cmd *cmd, *tmp;
> +
> +	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
> +		hci_remove_cb(hdev, cmd);
> +}
> +
> +/* Send HCI command with callback */
> +int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
> +	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
> +	       void *opt,
> +	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
> +	       gfp_t flags)
> +{
> +	int ret;
> +
> +	if (!cb)
> +		return -EINVAL;
> +
> +	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);

Why do you need hci_add_cb()? just put all it code here, it is used only once.

	Gustavo

[RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use workqueue to process HCI callbacks.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    3 ++
 net/bluetooth/hci_core.c         |   57 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 60 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 479e4d1..c2506fa 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1113,5 +1113,8 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	       void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
 	       gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue);
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 165c235..4c13bed 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2163,6 +2163,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+		  struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("%s queue cmd %p", hdev->name, cmd);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	BT_DBG("%s remove cmd %p", hdev->name, cmd);
@@ -2206,6 +2247,22 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	return hci_send_cmd(hdev, opcode, plen, param);
 }
 
+void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = hci_find_cb(hdev, opcode);
+	if (!cmd)
+		return;
+
+	hci_dev_lock(hdev);
+
+	cmd->status = status;
+	hci_queue_cb(hdev, cmd, hdev->workqueue);
+
+	hci_dev_unlock(hdev);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Oliver Neukum]

On Tuesday 24 July 2012 16:21:43 Andrei Emeltchenko wrote:
> +void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
> +                 struct workqueue_struct *workqueue)
> +{
> +       struct hci_cb_work *work;
> +
> +       BT_DBG("%s queue cmd %p", hdev->name, cmd);
> +
> +       work = kmalloc(sizeof(*work), GFP_KERNEL);

This looks like prone to deadlocks. You can run networked file
systems over the link and allocating memory with GFP_KERNEL
could run into a recursion problem.

> +       if (!work)
> +               return;
> +
> +       INIT_WORK(&work->work, hci_cb_worker);
> +       work->hdev = hdev;
> +       work->cmd = cmd;
> +       hci_dev_hold(hdev);
> +
> +       if (!queue_work(workqueue, &work->work)) {
> +               kfree(work);
> +               hci_dev_put(hdev);
> +       }
> +}

	Regards
		Oliver

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

Hi Oliver,

On Tue, Jul 24, 2012 at 03:31:37PM +0200, Oliver Neukum wrote:
> On Tuesday 24 July 2012 16:21:43 Andrei Emeltchenko wrote:
> > +void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
> > +                 struct workqueue_struct *workqueue)
> > +{
> > +       struct hci_cb_work *work;
> > +
> > +       BT_DBG("%s queue cmd %p", hdev->name, cmd);
> > +
> > +       work = kmalloc(sizeof(*work), GFP_KERNEL);
> 
> This looks like prone to deadlocks. You can run networked file
> systems over the link and allocating memory with GFP_KERNEL
> could run into a recursion problem.

Sorry, how this might run into recursion problem?

Best regards 
Andrei Emeltchenko 

> 
> > +       if (!work)
> > +               return;
> > +
> > +       INIT_WORK(&work->work, hci_cb_worker);
> > +       work->hdev = hdev;
> > +       work->cmd = cmd;
> > +       hci_dev_hold(hdev);
> > +
> > +       if (!queue_work(workqueue, &work->work)) {
> > +               kfree(work);
> > +               hci_dev_put(hdev);
> > +       }
> > +}
> 
> 	Regards
> 		Oliver
> 

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Oliver Neukum]

On Tuesday 24 July 2012 16:46:41 Andrei Emeltchenko wrote:
> Hi Oliver,
> 
> On Tue, Jul 24, 2012 at 03:31:37PM +0200, Oliver Neukum wrote:
> > On Tuesday 24 July 2012 16:21:43 Andrei Emeltchenko wrote:
> > > +void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
> > > +                 struct workqueue_struct *workqueue)
> > > +{
> > > +       struct hci_cb_work *work;
> > > +
> > > +       BT_DBG("%s queue cmd %p", hdev->name, cmd);
> > > +
> > > +       work = kmalloc(sizeof(*work), GFP_KERNEL);
> > 
> > This looks like prone to deadlocks. You can run networked file
> > systems over the link and allocating memory with GFP_KERNEL
> > could run into a recursion problem.
> 
> Sorry, how this might run into recursion problem?

Suppose you transfer something necessary for writing out a dirty
page over the HCI and you call hci_queue_cb() to do so. Then
GFP_KERNEL causes another page to be written out over the same path.
(eg. NFS over PAN)

	Regards
		Oliver

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

On Tue, Jul 24, 2012 at 04:15:42PM +0200, Oliver Neukum wrote:
> On Tuesday 24 July 2012 16:46:41 Andrei Emeltchenko wrote:
> > Hi Oliver,
> > 
> > On Tue, Jul 24, 2012 at 03:31:37PM +0200, Oliver Neukum wrote:
> > > On Tuesday 24 July 2012 16:21:43 Andrei Emeltchenko wrote:
> > > > +void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
> > > > +                 struct workqueue_struct *workqueue)
> > > > +{
> > > > +       struct hci_cb_work *work;
> > > > +
> > > > +       BT_DBG("%s queue cmd %p", hdev->name, cmd);
> > > > +
> > > > +       work = kmalloc(sizeof(*work), GFP_KERNEL);
> > > 
> > > This looks like prone to deadlocks. You can run networked file
> > > systems over the link and allocating memory with GFP_KERNEL
> > > could run into a recursion problem.
> > 
> > Sorry, how this might run into recursion problem?
> 
> Suppose you transfer something necessary for writing out a dirty
> page over the HCI and you call hci_queue_cb() to do so. Then
> GFP_KERNEL causes another page to be written out over the same path.
> (eg. NFS over PAN)

Still I do not get it. We use GFP_KERNEL quite a lot in Bluetooth, how
this case is special?

Best regards 
Andrei Emeltchenko 

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:43 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Use workqueue to process HCI callbacks.

Could elaborate more on why a new workqueue is needed, I tried to figure this
out this but was unable to find a user for this function in the following
patches.

	Gustavo

Re: [RFCv2 02/20] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

Hi Gustavo,

On Tue, Jul 24, 2012 at 05:36:47PM -0300, Gustavo Padovan wrote:
> Hi Andrei,
> 
> * Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:43 +0300]:
> 
> > From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> > 
> > Use workqueue to process HCI callbacks.
> 
> Could elaborate more on why a new workqueue is needed, I tried to figure this
> out this but was unable to find a user for this function in the following
> patches.

workqueue processing was removed in RFCv3.

Best regards 
Andrei Emeltchenko 

[RFCv2 03/20] Bluetooth: Add callback clear to ops->teardown

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

ops->teardown takes care about deleting queued callbacks for all AMP
controllers.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   17 ++++++++++++++++-
 net/bluetooth/hci_core.c         |    3 +--
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index c2506fa..9626b21 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1116,5 +1116,6 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
 		  struct workqueue_struct *workqueue);
 void hci_process_cb(struct hci_dev *hdev, __u16 opcode, u8 status);
+void hci_cb_clear(struct hci_dev *hdev);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c4732b1..4de740c 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -444,16 +444,31 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
+static void a2mp_chan_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	struct hci_dev *hdev, *tmp;
+
+	BT_DBG("chan %p", chan);
+
+	list_for_each_entry_safe(hdev, tmp, &hci_dev_list, list) {
+		hci_dev_hold(hdev);
+		/* Iterate through AMP controllers */
+		if (hdev->amp_type == HCI_AMP)
+			hci_cb_clear(hdev);
+		hci_dev_put(hdev);
+	}
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
+	.teardown = a2mp_chan_teardown_cb,
 
 	/* Not implemented for A2MP */
 	.new_connection = l2cap_chan_no_new_connection,
-	.teardown = l2cap_chan_no_teardown,
 	.ready = l2cap_chan_no_ready,
 };
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 4c13bed..ccd4bd7 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,7 +36,6 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
-static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -2220,7 +2219,7 @@ void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 	}
 }
 
-static void hci_cb_clear(struct hci_dev *hdev)
+void hci_cb_clear(struct hci_dev *hdev)
 {
 	struct hci_cb_cmd *cmd, *tmp;
 
-- 
1.7.9.5

[RFCv2 04/20] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with callback to be executed upon receiving
command complete event. Callback will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 include/net/bluetooth/amp.h  |   19 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |   28 +++++++++----------
 net/bluetooth/amp.c          |   63 ++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c    |    6 +++-
 6 files changed, 103 insertions(+), 16 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..ec77ddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..ec7bea7
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4de740c..df40683 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -189,24 +189,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
-	}
+	if (!hdev)
+		goto send_err;
 
-	if (hdev)
+	if (hdev->dev_type != HCI_BREDR) {
+		amp_read_loc_info(hdev, mgr);
+		goto done;
+	} else {
 		hci_dev_put(hdev);
+	}
+
+send_err:
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..4aea7be
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,63 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct a2mp_info_rsp rsp;
+
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+}
+
+static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	hci_dev_put(hdev);
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 06996ff..7222421 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -29,6 +29,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -845,7 +846,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto process_cb;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -859,6 +860,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+process_cb:
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

Re: [RFCv2 04/20] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:45 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> When receiving A2MP Get Info Request execute Read Local AMP Info HCI
> command to AMP controller with callback to be executed upon receiving
> command complete event. Callback will handle A2MP Get Info Response.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/a2mp.h |    1 +
>  include/net/bluetooth/amp.h  |   19 +++++++++++++
>  net/bluetooth/Makefile       |    2 +-
>  net/bluetooth/a2mp.c         |   28 +++++++++----------
>  net/bluetooth/amp.c          |   63 ++++++++++++++++++++++++++++++++++++++++++
>  net/bluetooth/hci_event.c    |    6 +++-
>  6 files changed, 103 insertions(+), 16 deletions(-)
>  create mode 100644 include/net/bluetooth/amp.h
>  create mode 100644 net/bluetooth/amp.c
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 6a76e0a..ec77ddc 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
>  int amp_mgr_put(struct amp_mgr *mgr);
>  struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
>  				       struct sk_buff *skb);
> +void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
>  
>  #endif /* __A2MP_H */
> diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
> new file mode 100644
> index 0000000..ec7bea7
> --- /dev/null
> +++ b/include/net/bluetooth/amp.h
> @@ -0,0 +1,19 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#ifndef __AMP_H
> +#define __AMP_H
> +
> +void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
> +
> +#endif /* __AMP_H */
> diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
> index fa6d94a..dea6a28 100644
> --- a/net/bluetooth/Makefile
> +++ b/net/bluetooth/Makefile
> @@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
>  
>  bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
>  	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
> -	a2mp.o
> +	a2mp.o amp.o
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 4de740c..df40683 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -16,6 +16,7 @@
>  #include <net/bluetooth/hci_core.h>
>  #include <net/bluetooth/l2cap.h>
>  #include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
>  
>  /* A2MP build & send command helper functions */
>  static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
> @@ -37,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
>  	return cmd;
>  }
>  
> -static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
> -		      void *data)
> +void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
>  {
>  	struct l2cap_chan *chan = mgr->a2mp_chan;
>  	struct a2mp_cmd *cmd;
> @@ -189,24 +189,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
>  
>  	BT_DBG("id %d", req->id);
>  
> -	rsp.id = req->id;
> -	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
> -
>  	hdev = hci_dev_get(req->id);
> -	if (hdev && hdev->amp_type != HCI_BREDR) {
> -		rsp.status = 0;
> -		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
> -		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
> -		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
> -		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
> -		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
> -	}
> +	if (!hdev)
> +		goto send_err;
>  
> -	if (hdev)
> +	if (hdev->dev_type != HCI_BREDR) {
> +		amp_read_loc_info(hdev, mgr);
> +		goto done;
> +	} else {
>  		hci_dev_put(hdev);
> +	}
> +
> +send_err:
> +	rsp.id = req->id;
> +	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
>  
>  	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
>  
> +done:
>  	skb_pull(skb, sizeof(*req));
>  	return 0;
>  }
> diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
> new file mode 100644
> index 0000000..4aea7be
> --- /dev/null
> +++ b/net/bluetooth/amp.c
> @@ -0,0 +1,63 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#include <linux/workqueue.h>

Where are you using workqueue? why is this include here?

> +#include <net/bluetooth/bluetooth.h>
> +#include <net/bluetooth/hci.h>
> +#include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
> +
> +static void amp_read_loc_info_complete(struct hci_dev *hdev,
> +				       struct hci_cb_cmd *cmd)
> +{
> +	struct amp_mgr *mgr = cmd->opt;
> +	struct a2mp_info_rsp rsp;
> +
> +	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
> +
> +	rsp.id = hdev->id;
> +	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
> +
> +	if (hdev->amp_type != HCI_BREDR) {
> +		rsp.status = 0;
> +		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
> +		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
> +		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
> +		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
> +		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
> +	}
> +
> +	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
> +}
> +
> +static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
> +{
> +	struct amp_mgr *mgr = cmd->opt;
> +
> +	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
> +
> +	hci_dev_put(hdev);
> +	amp_mgr_put(mgr);
> +	kfree(cmd);
> +}
> +
> +void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
> +{
> +	BT_DBG("%s mgr %p", hdev->name, mgr);
> +
> +	amp_mgr_get(mgr);
> +
> +	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
> +		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
> +}
> diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
> index 06996ff..7222421 100644
> --- a/net/bluetooth/hci_event.c
> +++ b/net/bluetooth/hci_event.c
> @@ -29,6 +29,7 @@
>  
>  #include <net/bluetooth/bluetooth.h>
>  #include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/a2mp.h>
>  
>  /* Handle HCI Event packets */
>  
> @@ -845,7 +846,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
>  	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
>  
>  	if (rp->status)
> -		return;
> +		goto process_cb;
>  
>  	hdev->amp_status = rp->amp_status;
>  	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
> @@ -859,6 +860,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
>  	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
>  
>  	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
> +
> +process_cb:
> +	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);

So do we really need an workqueue for the callback here?

	Gustavo

Re: [RFCv2 04/20] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

Hi Gustavo,

On Tue, Jul 24, 2012 at 05:46:05PM -0300, Gustavo Padovan wrote:
> > +process_cb:
> > +	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
> 
> So do we really need an workqueue for the callback here?

Let's first decide are we going to use callbacks and then I will think can
I remove workqueues. When I have created them it solved some locking
issues with lockdep...

Best regards 
Andrei Emeltchenko 

[RFCv2 05/20] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
A2MP Get AMP Assoc Response is run from HCI callback.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++
 net/bluetooth/a2mp.c             |   13 +++++----
 net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
 6 files changed, 120 insertions(+), 6 deletions(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ec7bea7..e861675 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 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);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 7f19556..2b19703 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 9626b21..5aea1cc 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -135,6 +135,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -188,6 +194,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index df40683..9803f74 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -230,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 4aea7be..b76f369 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -61,3 +61,63 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
 		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		   amp_read_loc_assoc_complete, mgr, cb_destructor,
+		   GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 7222421..a1ad489 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -865,6 +866,42 @@ process_cb:
 	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto process_cb;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+process_cb:
+	/* Run callback when all fragments received */
+	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2294,6 +2331,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

Re: [RFCv2 05/20] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:46 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
> HCI command to AMP controller. If the AMP Assoc data is larger then it
> can fit to HCI event only fragment is read. When all fragments are read
> A2MP Get AMP Assoc Response is run from HCI callback.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/amp.h      |    2 ++
>  include/net/bluetooth/hci.h      |    2 ++
>  include/net/bluetooth/hci_core.h |    8 +++++
>  net/bluetooth/a2mp.c             |   13 +++++----
>  net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
>  net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
>  6 files changed, 120 insertions(+), 6 deletions(-)
> 
> diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
> index ec7bea7..e861675 100644
> --- a/include/net/bluetooth/amp.h
> +++ b/include/net/bluetooth/amp.h
> @@ -15,5 +15,7 @@
>  #define __AMP_H
>  
>  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);
>  
>  #endif /* __AMP_H */
> diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> index 7f19556..2b19703 100644
> --- a/include/net/bluetooth/hci.h
> +++ b/include/net/bluetooth/hci.h
> @@ -33,6 +33,8 @@
>  #define HCI_LINK_KEY_SIZE	16
>  #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
>  
> +#define HCI_MAX_AMP_ASSOC_SIZE	672
> +
>  /* HCI dev events */
>  #define HCI_DEV_REG			1
>  #define HCI_DEV_UNREG			2
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 9626b21..5aea1cc 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -135,6 +135,12 @@ struct hci_cb_cmd {
>  	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
>  };
>  
> +struct amp_assoc {
> +	__u16	len;
> +	__u16	offset;
> +	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
> +};
> +
>  #define NUM_REASSEMBLY 4
>  struct hci_dev {
>  	struct list_head list;
> @@ -188,6 +194,8 @@ struct hci_dev {
>  	__u32		amp_max_flush_to;
>  	__u32		amp_be_flush_to;
>  
> +	struct amp_assoc	loc_assoc;
> +
>  	__u8		flow_ctl_mode;
>  
>  	unsigned int	auto_accept_delay;
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index df40683..9803f74 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -230,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
>  
>  		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
>  			  &rsp);
> -		goto clean;
> -	}
>  
> -	/* Placeholder for HCI Read AMP Assoc */
> +		if (hdev)
> +			hci_dev_put(hdev);
>  
> -clean:
> -	if (hdev)
> -		hci_dev_put(hdev);
> +		goto done;
> +	}
> +
> +	amp_read_loc_assoc(hdev, mgr);
>  
> +done:
>  	skb_pull(skb, sizeof(*req));
>  	return 0;
>  }
> diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
> index 4aea7be..b76f369 100644
> --- a/net/bluetooth/amp.c
> +++ b/net/bluetooth/amp.c
> @@ -61,3 +61,63 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
>  	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
>  		   amp_read_loc_info_complete, mgr, cb_destructor, GFP_KERNEL);
>  }
> +
> +void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
> +{
> +	struct hci_cp_read_local_amp_assoc cp;
> +	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
> +
> +	BT_DBG("%s handle %d", hdev->name, phy_handle);
> +
> +	cp.phy_handle = phy_handle;
> +	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
> +	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
> +
> +	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
> +}
> +
> +static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
> +					struct hci_cb_cmd *cmd)
> +{
> +	struct amp_mgr *mgr = cmd->opt;
> +	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
> +	struct a2mp_amp_assoc_rsp *rsp;
> +	size_t len;
> +
> +	BT_DBG("%s cmd %p", hdev->name, cmd);
> +
> +	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
> +	rsp = kzalloc(len, GFP_KERNEL);
> +	if (!rsp)
> +		return;
> +
> +	rsp->id = hdev->id;
> +
> +	if (cmd->status) {
> +		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
> +		goto send;
> +	}
> +
> +	rsp->status = A2MP_STATUS_SUCCESS;
> +	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
> +
> +send:
> +	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
> +	kfree(rsp);
> +}
> +
> +void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
> +{
> +	struct hci_cp_read_local_amp_assoc cp;
> +
> +	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
> +	memset(&cp, 0, sizeof(cp));
> +
> +	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
> +
> +	amp_mgr_get(mgr);
> +
> +	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
> +		   amp_read_loc_assoc_complete, mgr, cb_destructor,
> +		   GFP_KERNEL);
> +}
> diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
> index 7222421..a1ad489 100644
> --- a/net/bluetooth/hci_event.c
> +++ b/net/bluetooth/hci_event.c
> @@ -30,6 +30,7 @@
>  #include <net/bluetooth/bluetooth.h>
>  #include <net/bluetooth/hci_core.h>
>  #include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
>  
>  /* Handle HCI Event packets */
>  
> @@ -865,6 +866,42 @@ process_cb:
>  	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
>  }
>  
> +static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
> +					struct sk_buff *skb)
> +{
> +	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
> +	struct amp_assoc *assoc = &hdev->loc_assoc;
> +	size_t rem_len, frag_len;
> +
> +	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
> +
> +	if (rp->status)
> +		goto process_cb;
> +
> +	frag_len = skb->len - sizeof(*rp);
> +	rem_len = __le16_to_cpu(rp->rem_len);
> +
> +	if (rem_len > frag_len) {
> +		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
> +
> +		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
> +		assoc->offset += frag_len;
> +
> +		/* Read other fragments */
> +		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
> +
> +		return;
> +	}
> +
> +	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
> +	assoc->len = assoc->offset + rem_len;
> +	assoc->offset = 0;
> +
> +process_cb:
> +	/* Run callback when all fragments received */
> +	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);

So, I have a question here, why are we going with this callback system here?
This code and A2MP code runs inside the same module, so why do we need
callbacks? I wonder if switch all of this to directly calls to the A2MP code,
this simplifies things a lot.
Not sure if there was any discussion regarding this when you started this
implementation.

	Gustavo

Re: [RFCv2 05/20] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

On Tue, Jul 24, 2012 at 05:55:54PM -0300, Gustavo Padovan wrote:
...

> > +static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
> > +					struct sk_buff *skb)
> > +{
> > +	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
> > +	struct amp_assoc *assoc = &hdev->loc_assoc;
> > +	size_t rem_len, frag_len;
> > +
> > +	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
> > +
> > +	if (rp->status)
> > +		goto process_cb;
> > +
> > +	frag_len = skb->len - sizeof(*rp);
> > +	rem_len = __le16_to_cpu(rp->rem_len);
> > +
> > +	if (rem_len > frag_len) {
> > +		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
> > +
> > +		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
> > +		assoc->offset += frag_len;
> > +
> > +		/* Read other fragments */
> > +		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
> > +
> > +		return;
> > +	}
> > +
> > +	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
> > +	assoc->len = assoc->offset + rem_len;
> > +	assoc->offset = 0;
> > +
> > +process_cb:
> > +	/* Run callback when all fragments received */
> > +	hci_process_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
> 
> So, I have a question here, why are we going with this callback system here?
> This code and A2MP code runs inside the same module, so why do we need
> callbacks? 

About code running in the same module please refer to management interface
calbacks like mgmt_pending_foreach, etc

> I wonder if switch all of this to directly calls to the A2MP code,
> this simplifies things a lot.

A2MP code is running above different HCI device. The problem here is that 
if we have several AMP managers and we execute some HCI command to AMP
controller then we wouldn't know which AMP manager has initiated
connection. (Some commands though provide phy_handler and this might be
used).

If we __always__ have only one AMP Manager that would work as we can have
global pointer. Then we can always call to A2MP context which I think
shall be run in a workqueue. We can have problems here if we remove and
create AMP manager...

Best regards 
Andrei Emeltchenko 

[RFCv2 06/20] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 9803f74..99beb95 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -161,6 +169,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -378,8 +435,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv2 07/20] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside amp_mgr structure and represent AMP physical links.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   42 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  141 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 193 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ec77ddc..012f573 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,9 @@ struct amp_mgr {
 	__u8			ident;
 	__u8			handle;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..201c501
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size);
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void phylink_list_flush(struct amp_mgr *mgr);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 99beb95..f711b75 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -594,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -628,6 +630,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..24fb3aa
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,141 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[RFCv2 08/20] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 012f573..8ba236c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -29,6 +29,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 201c501..3223ec2 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f711b75..7b4af39 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -634,6 +635,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 24fb3aa..335cbc3 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,87 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

Re: [RFCv2 08/20] Bluetooth: AMP: Remote AMP ctrl definitions

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:49 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Create remote AMP controllers structure. It is used to keep information
> about discovered remote AMP controllers by A2MP protocol.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/a2mp.h |    3 ++
>  include/net/bluetooth/pal.h  |   14 ++++++++
>  net/bluetooth/a2mp.c         |    5 +++
>  net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
>  4 files changed, 103 insertions(+)
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 012f573..8ba236c 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -29,6 +29,9 @@ struct amp_mgr {
>  
>  	struct list_head	phy_links;
>  	struct mutex		phy_links_lock;
> +
> +	struct list_head	amp_ctrls;
> +	struct mutex		amp_ctrls_lock;

Have you checked for RCU here, are they worthwhile? and why is it better than
spinlocks?  This is for both locks above.

	Gustavo

Re: [RFCv2 08/20] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

Hi Gustavo,

On Tue, Jul 24, 2012 at 06:13:42PM -0300, Gustavo Padovan wrote:
> > diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> > index 012f573..8ba236c 100644
> > --- a/include/net/bluetooth/a2mp.h
> > +++ b/include/net/bluetooth/a2mp.h
> > @@ -29,6 +29,9 @@ struct amp_mgr {
> >  
> >  	struct list_head	phy_links;
> >  	struct mutex		phy_links_lock;
> > +
> > +	struct list_head	amp_ctrls;
> > +	struct mutex		amp_ctrls_lock;
> 
> Have you checked for RCU here, are they worthwhile? and why is it better than
> spinlocks?  This is for both locks above.

I want to be able to sleep inside critical section and using RCU is
overkill here.

Best regards 
Andrei Emeltchenko 

[RFCv2 09/20] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/a2mp.c        |   19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3223ec2..6ce1dfb 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -52,5 +52,6 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7b4af39..112170b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -309,6 +309,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -326,6 +327,11 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = phylink_add(mgr, rsp.local_id, rsp.remote_id, req->amp_assoc,
+			    le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -345,6 +351,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -361,8 +368,20 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+	phylink_del(mgr, plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[RFCv2 10/20] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 112170b..74da39f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -269,6 +269,35 @@ done:
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -459,8 +488,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv2 11/20] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   61 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 60 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 74da39f..508cb79 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -331,6 +331,62 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+	u8 *rem_assoc;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	rem_assoc = skb_pull(skb, sizeof(*rsp));
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			goto done;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		goto done;
+
+	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+			    ctrl->assoc_len);
+
+	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
+
+done:
+	skb_pull(skb, len - sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -492,8 +548,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[RFCv2 12/20] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   21 +++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |   34 ++++++++++++++++++++++++++++++----
 3 files changed, 53 insertions(+), 4 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 8ba236c..bf18a37 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -22,6 +22,7 @@
 struct amp_mgr {
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -129,5 +130,6 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void l2cap_discover_amp(struct l2cap_chan *chan);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 508cb79..e740bb0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -769,3 +769,24 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+void l2cap_discover_amp(struct l2cap_chan *chan)
+{
+	struct a2mp_discov_req req;
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+
+	BT_DBG("%p", conn);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index c81e182..7427e7b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -988,6 +988,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1014,6 +1026,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		l2cap_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1028,8 +1050,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1125,7 +1148,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5509,7 +5532,10 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				/* Unlock chan list since we add A2MP chan */
+				mutex_unlock(&conn->chan_lock);
+				l2cap_choose_conn(chan);
+				mutex_lock(&conn->chan_lock);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

Re: [RFCv2 12/20] Bluetooth: Choose connection based on capabilities

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:53 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Choose which L2CAP connection to establish by checking support
> for HS and remote side supported features.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/a2mp.h |    2 ++
>  net/bluetooth/a2mp.c         |   21 +++++++++++++++++++++
>  net/bluetooth/l2cap_core.c   |   34 ++++++++++++++++++++++++++++++----
>  3 files changed, 53 insertions(+), 4 deletions(-)
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 8ba236c..bf18a37 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -22,6 +22,7 @@
>  struct amp_mgr {
>  	struct l2cap_conn	*l2cap_conn;
>  	struct l2cap_chan	*a2mp_chan;
> +	struct l2cap_chan	*bredr_chan;
>  	struct kref		kref;
>  	__u8			ident;
>  	__u8			handle;
> @@ -129,5 +130,6 @@ int amp_mgr_put(struct amp_mgr *mgr);
>  struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
>  				       struct sk_buff *skb);
>  void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
> +void l2cap_discover_amp(struct l2cap_chan *chan);
>  
>  #endif /* __A2MP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 508cb79..e740bb0 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -769,3 +769,24 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
>  
>  	return mgr->a2mp_chan;
>  }
> +
> +void l2cap_discover_amp(struct l2cap_chan *chan)

prefix a function with l2cap_ outside of the l2cap code is not really a good
idea, we need a better solution here..

	Gustavo

Re: [RFCv2 12/20] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

Hi Gustavo,

On Tue, Jul 24, 2012 at 06:10:38PM -0300, Gustavo Padovan wrote:
> > +void l2cap_discover_amp(struct l2cap_chan *chan)
> 
> prefix a function with l2cap_ outside of the l2cap code is not really a good
> idea, we need a better solution here..

I will rename this to a2mp_discover_amp

Best regards 
Andrei Emeltchenko 

[RFCv2 13/20] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 335cbc3..a405e88 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -220,3 +221,37 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed");
+		return PTR_ERR(tfm);
+	}
+
+	if (ksize) {
+		ret = crypto_shash_setkey(tfm, key, ksize);
+		if (ret) {
+			BT_DBG("crypto_ahash_setkey failed");
+		} else {
+			struct {
+				struct shash_desc shash;
+				char ctx[crypto_shash_descsize(tfm)];
+			} desc;
+
+			desc.shash.tfm = tfm;
+			desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+			ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+						  output);
+		}
+	}
+
+	BT_DBG("ret 0x%x", ret);
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

Re: [RFCv2 13/20] Bluetooth: Add function to derive AMP key using hmac

[Gustavo Padovan]

Hi Dmitry,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:54 +0300]:

> From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
> 
> hmac(sha256) will be used for AMP key generation.
> 
> Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
>  1 file changed, 35 insertions(+)
> 
> diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
> index 335cbc3..a405e88 100644
> --- a/net/bluetooth/pal.c
> +++ b/net/bluetooth/pal.c
> @@ -12,6 +12,7 @@
>  */
>  
>  #include <net/bluetooth/pal.h>
> +#include <crypto/hash.h>
>  
>  enum pal_states {
>  	DISCONNECTED,
> @@ -220,3 +221,37 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
>  
>  	return found;
>  }
> +
> +int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
> +{
> +	int ret = 0;
> +	struct crypto_shash *tfm;
> +
> +	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
> +	if (IS_ERR(tfm)) {
> +		BT_DBG("crypto_alloc_ahash failed");
> +		return PTR_ERR(tfm);
> +	}
> +
> +	if (ksize) {

can you move the check for ksize to the beginning of the function? This avoid
unnecessary calls to crypto_alloc_ahash() in case ksize is zero.

	Gustavo

[RFCv2 14/20] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP key using hmac_sha256 helper.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   61 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 63 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 6ce1dfb..8799285 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -53,5 +53,6 @@ int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
 void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index a405e88..c47eeac 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -255,3 +255,64 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+static void hexdump(u8 *buf, size_t len)
+{
+	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,
+		       16, 1, buf, len, false);
+}
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	u8 b802_key[HCI_AMP_LINK_KEY_SIZE];
+	int result;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("key_type %d", conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3)
+		return -EACCES;
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	hci_dev_lock(hdev);
+	key = hci_find_link_key(hdev, &conn->dst);
+	hci_dev_unlock(hdev);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	hexdump(keybuf, HCI_AMP_LINK_KEY_SIZE);
+
+	result = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4,
+			     gamp_key);
+	hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
+
+	if (result)
+		goto done;
+
+	if (conn->key_type == 3) {
+		BT_DBG("gamp_key");
+		hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		goto done;
+	}
+
+	result = hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4,
+			     b802_key);
+	hexdump(b802_key, HCI_AMP_LINK_KEY_SIZE);
+
+	memcpy(data, b802_key, HCI_AMP_LINK_KEY_SIZE);
+
+done:
+	return result;
+}
-- 
1.7.9.5

Re: [RFCv2 14/20] Bluetooth: AMP: Add AMP key calculation

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-07-24 16:21:55 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Function calculates AMP key using hmac_sha256 helper.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/pal.h |    1 +
>  net/bluetooth/Kconfig       |    1 +
>  net/bluetooth/pal.c         |   61 +++++++++++++++++++++++++++++++++++++++++++
>  3 files changed, 63 insertions(+)
> 
> diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
> index 6ce1dfb..8799285 100644
> --- a/include/net/bluetooth/pal.h
> +++ b/include/net/bluetooth/pal.h
> @@ -53,5 +53,6 @@ int phylink_put(struct phy_link *plink);
>  void phylink_get(struct phy_link *plink);
>  void phylink_list_flush(struct amp_mgr *mgr);
>  void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
> +int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
>  
>  #endif /* __PAL_H */
> diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
> index 3537d38..1c11d0d 100644
> --- a/net/bluetooth/Kconfig
> +++ b/net/bluetooth/Kconfig
> @@ -11,6 +11,7 @@ menuconfig BT
>  	select CRYPTO_BLKCIPHER
>  	select CRYPTO_AES
>  	select CRYPTO_ECB
> +	select CRYPTO_SHA256
>  	help
>  	  Bluetooth is low-cost, low-power, short-range wireless technology.
>  	  It was designed as a replacement for cables and other short-range
> diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
> index a405e88..c47eeac 100644
> --- a/net/bluetooth/pal.c
> +++ b/net/bluetooth/pal.c
> @@ -255,3 +255,64 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
>  	crypto_free_shash(tfm);
>  	return ret;
>  }
> +
> +static void hexdump(u8 *buf, size_t len)
> +{
> +	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_OFFSET,
> +		       16, 1, buf, len, false);
> +}
> +
> +int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
> +{
> +	struct hci_dev *hdev = conn->hdev;
> +	struct link_key *key;
> +	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
> +	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
> +	u8 b802_key[HCI_AMP_LINK_KEY_SIZE];
> +	int result;
> +
> +	if (!hci_conn_check_link_mode(conn))
> +		return -EACCES;
> +
> +	BT_DBG("key_type %d", conn->key_type);
> +
> +	/* Legacy key */
> +	if (conn->key_type < 3)
> +		return -EACCES;
> +
> +	*type = conn->key_type;
> +	*len = HCI_AMP_LINK_KEY_SIZE;
> +
> +	hci_dev_lock(hdev);
> +	key = hci_find_link_key(hdev, &conn->dst);
> +	hci_dev_unlock(hdev);
> +
> +	/* BR/EDR Link Key concatenated together with itself */
> +	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
> +	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
> +
> +	hexdump(keybuf, HCI_AMP_LINK_KEY_SIZE);

Is these hex prints worthwhile? Seems kind expensive, right?

> +
> +	result = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4,
> +			     gamp_key);
> +	hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
> +
> +	if (result)
> +		goto done;
> +
> +	if (conn->key_type == 3) {
> +		BT_DBG("gamp_key");
> +		hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
> +		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
> +		goto done;
> +	}
> +
> +	result = hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4,
> +			     b802_key);

Can't you pass data directly here, instead of b802_key? This could avoid an
extra memcpy.

	Gustavo

Re: [RFCv2 14/20] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

Hi Gustavo,

On Tue, Jul 24, 2012 at 06:29:44PM -0300, Gustavo Padovan wrote:
> > +
> > +	hexdump(keybuf, HCI_AMP_LINK_KEY_SIZE);
> 
> Is these hex prints worthwhile? Seems kind expensive, right?

Yes, I will remove this print.

> > +	result = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4,
> > +			     gamp_key);
> > +	hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
> > +
> > +	if (result)
> > +		goto done;
> > +
> > +	if (conn->key_type == 3) {
> > +		BT_DBG("gamp_key");
> > +		hexdump(gamp_key, HCI_AMP_LINK_KEY_SIZE);
> > +		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
> > +		goto done;
> > +	}
> > +
> > +	result = hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4,
> > +			     b802_key);
> 
> Can't you pass data directly here, instead of b802_key? This could avoid an
> extra memcpy.

Yes, will do this way.

Best regards 
Andrei Emeltchenko 

[RFCv2 15/20] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define callback which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   31 +++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |   11 +++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 8799285..d9eb87e 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e740bb0..2271cb7 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -379,9 +379,13 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 			    ctrl->assoc_len);
+	if (!plink)
+		goto done;
 
 	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
 
+	amp_create_phylink(hdev, mgr, plink);
+
 done:
 	skb_pull(skb, len - sizeof(*rsp));
 	return 0;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index b76f369..fcfdd0e 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 static void amp_read_loc_info_complete(struct hci_dev *hdev,
 				       struct hci_cb_cmd *cmd)
@@ -121,3 +122,33 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 		   amp_read_loc_assoc_complete, mgr, cb_destructor,
 		   GFP_KERNEL);
 }
+
+static void amp_create_phylink_cs(struct hci_dev *hdev,
+				  struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* Write Remote AMP Assoc */
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp,
+		   amp_create_phylink_cs, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index a1ad489..2b91b55 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1698,6 +1698,13 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	hci_process_cb(hdev, HCI_OP_CREATE_PHY_LINK, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2488,6 +2495,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[RFCv2 16/20] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   78 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   15 ++++++++
 3 files changed, 95 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 5aea1cc..50b4613 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -138,6 +138,8 @@ struct hci_cb_cmd {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index fcfdd0e..e8237fc 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -123,6 +123,79 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 		   GFP_KERNEL);
 }
 
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink);
+
+static void amp_write_rem_assoc_cs(struct hci_dev *hdev,
+				   struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p status 0x%2.2x", mgr, cmd->status);
+
+	if (cmd->status)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, mgr, plink);
+
+	phylink_put(plink);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
+	if (!ctrl)
+		return;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       plink, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp,
+		   amp_write_rem_assoc_cs, mgr, cb_destructor, GFP_KERNEL);
+
+	kfree(cp);
+}
+
 static void amp_create_phylink_cs(struct hci_dev *hdev,
 				  struct hci_cb_cmd *cmd)
 {
@@ -132,6 +205,11 @@ static void amp_create_phylink_cs(struct hci_dev *hdev,
 	BT_DBG("mgr %p", mgr);
 
 	/* Write Remote AMP Assoc */
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+		phylink_put(plink);
+	}
 }
 
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 2b91b55..3c00b79 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1214,6 +1214,17 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	hci_process_cb(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, rp->status);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -2414,6 +2425,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[RFCv2 17/20] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 718dbb1..0ed0221 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -788,5 +788,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2271cb7..b29275a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -177,6 +177,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -207,6 +208,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -216,6 +218,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 7427e7b..165b810 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1000,7 +1000,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[RFCv2 18/20] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h     |    1 +
 include/net/bluetooth/hci_core.h |    6 +++
 net/bluetooth/a2mp.c             |    2 +-
 net/bluetooth/amp.c              |   88 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/hci_core.c         |   12 +++---
 net/bluetooth/hci_event.c        |    9 ++++
 6 files changed, 105 insertions(+), 13 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index bf18a37..2b15642 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -127,6 +127,7 @@ struct a2mp_physlink_rsp {
 
 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);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 50b4613..02ec147 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1117,6 +1117,12 @@ int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
 struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev,
+				  struct hci_cb_cmd *cmd),
+	       gfp_t flags);
 int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
 	       void *opt,
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b29275a..fb785e4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -64,7 +64,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;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index e8237fc..350e130 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -123,7 +123,68 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 		   GFP_KERNEL);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Read Local AMP Assoc final link information data callback */
+static void amp_read_loc_assoc_complete_final_cb(struct hci_dev *hdev,
+						 struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_physlink_req *req;
+	struct phy_link *plink;
+	size_t len;
+
+	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)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		goto clean;
+
+	req->local_id = plink->local_id;
+	req->remote_id = plink->remote_id;
+	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+	phylink_put(plink);
+
+	a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+	kfree(req);
+}
+
+static void amp_chan_sel_evt_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct hci_cp_read_local_amp_assoc cp;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* We should have phy link */
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	cp.phy_handle = plink->handle;
+	cp.len_so_far = cpu_to_le16(0);
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	phylink_put(plink);
+
+	amp_mgr_get(mgr);
+
+	/* Read Local AMP Assoc final link information data */
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		   amp_read_loc_assoc_complete_final_cb, mgr, cb_destructor,
+		   GFP_KERNEL);
+}
+
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink);
 
 static void amp_write_rem_assoc_cs(struct hci_dev *hdev,
@@ -141,12 +202,24 @@ static void amp_write_rem_assoc_cs(struct hci_dev *hdev,
 	if (!plink)
 		return;
 
-	amp_write_rem_assoc_frag(hdev, mgr, plink);
+	/* All fragments are written */
+	if (amp_write_rem_assoc_frag(hdev, mgr, plink)) {
+		struct amp_mgr *mgr = plink->mgr;
+
+		/* Expect Channel Select event */
+		hci_dev_hold(hdev);
+		amp_mgr_get(mgr);
+
+		hci_add_cb(hdev, HCI_EV_CHANNEL_SELECTED,
+			   amp_chan_sel_evt_cb, mgr,
+			   cb_destructor, GFP_KERNEL);
+	}
 
 	phylink_put(plink);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink)
 {
 	struct hci_cp_write_remote_amp_assoc *cp;
@@ -155,7 +228,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
 	if (!ctrl)
-		return;
+		return false;
 
 	if (!ctrl->assoc_rem_len) {
 		BT_DBG("all fragments are written");
@@ -163,7 +236,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 		ctrl->assoc_len_so_far = 0;
 
 		amp_ctrl_put(ctrl);
-		return;
+		return true;
 	}
 
 	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
@@ -172,7 +245,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 	cp = kzalloc(len, GFP_KERNEL);
 	if (!cp) {
 		amp_ctrl_put(ctrl);
-		return;
+		return false;
 	}
 
 	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
@@ -194,6 +267,8 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 		   amp_write_rem_assoc_cs, mgr, cb_destructor, GFP_KERNEL);
 
 	kfree(cp);
+
+	return false;
 }
 
 static void amp_create_phylink_cs(struct hci_dev *hdev,
@@ -207,6 +282,7 @@ static void amp_create_phylink_cs(struct hci_dev *hdev,
 	/* Write Remote AMP Assoc */
 	plink = phylink_lookup(mgr, hdev->id, 0);
 	if (plink) {
+		hci_dev_hold(hdev);
 		amp_write_rem_assoc_frag(hdev, mgr, plink);
 		phylink_put(plink);
 	}
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index ccd4bd7..58f884e 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2121,12 +2121,12 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
-static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
-		      void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
-		      void *opt,
-		      void (*destructor)(struct hci_dev *hdev,
-					 struct hci_cb_cmd *cmd),
-		      gfp_t flags)
+int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+	       void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	       void *opt,
+	       void (*destructor)(struct hci_dev *hdev,
+				  struct hci_cb_cmd *cmd),
+	       gfp_t flags)
 {
 	struct hci_cb_cmd *cmd;
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 3c00b79..6615be6 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3550,6 +3550,11 @@ 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)
+{
+	hci_process_cb(hdev, HCI_EV_CHANNEL_SELECTED, 0);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3706,6 +3711,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;
-- 
1.7.9.5

[RFCv2 19/20] Bluetooth: AMP: Process physical link complete event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new hci_conn for representing AMP physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |    1 +
 net/bluetooth/hci_event.c   |   36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 2b19703..74b448a 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6615be6..cfe3ef3 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3411,6 +3411,38 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+				      struct sk_buff *skb)
+{
+	struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+	struct hci_conn *conn;
+
+	BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+	       ev->status);
+
+	if (ev->status)
+		return;
+
+	hci_dev_lock(hdev);
+
+	conn = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (conn) {
+		conn->handle = ev->phy_handle;
+		conn->state = BT_CONNECTED;
+
+		hci_conn_hold(conn);
+		conn->disc_timeout = HCI_DISCONN_TIMEOUT/2;
+		hci_conn_put(conn);
+
+		hci_conn_hold_device(conn);
+		hci_conn_add_sysfs(conn);
+	} else {
+		BT_ERR("Cannot add connection");
+	}
+
+	hci_dev_unlock(hdev);
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3719,6 +3751,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_remote_oob_data_request_evt(hdev, skb);
 		break;
 
+	case HCI_EV_PHY_LINK_COMPLETE:
+		hci_phy_link_complete_evt(hdev, skb);
+		break;
+
 	case HCI_EV_NUM_COMP_BLOCKS:
 		hci_num_comp_blocks_evt(hdev, skb);
 		break;
-- 
1.7.9.5

[RFCv2 20/20] Bluetooth: AMP: Send Create Chan Req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Send L2CAP Create Channel Request when receiving HCI Physical
Link Complete event.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    5 +++++
 net/bluetooth/hci_event.c        |    2 ++
 net/bluetooth/l2cap_core.c       |   28 ++++++++++++++++++++++++++++
 3 files changed, 35 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 02ec147..444838f 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -380,6 +380,7 @@ extern void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason);
 extern int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt);
 extern int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb,
 			      u16 flags);
+extern void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 status);
 
 extern int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr);
 extern void sco_connect_cfm(struct hci_conn *hcon, __u8 status);
@@ -786,6 +787,10 @@ static inline void hci_proto_connect_cfm(struct hci_conn *conn, __u8 status)
 		sco_connect_cfm(conn, status);
 		break;
 
+	case AMP_LINK:
+		l2cap_chan_create_cfm(conn, status);
+		break;
+
 	default:
 		BT_ERR("unknown link type %d", conn->type);
 		break;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index cfe3ef3..379db6d 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3436,6 +3436,8 @@ static void hci_phy_link_complete_evt(struct hci_dev *hdev,
 
 		hci_conn_hold_device(conn);
 		hci_conn_add_sysfs(conn);
+
+		hci_proto_connect_cfm(conn, ev->status);
 	} else {
 		BT_ERR("Cannot add connection");
 	}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 165b810..56fc0f2 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/pal.h>
 
 bool disable_ertm;
 
@@ -1015,6 +1016,19 @@ void l2cap_send_conn_req(struct l2cap_chan *chan)
 	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 }
 
+static void l2cap_send_chan_create_req(struct l2cap_chan *chan, u8 remote_id)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct l2cap_create_chan_req req;
+
+	req.scid = cpu_to_le16(chan->scid);
+	req.psm = chan->psm;
+	req.amp_id = remote_id;
+
+	l2cap_send_cmd(conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
+		       sizeof(req), &req);
+}
+
 static void l2cap_chan_ready(struct l2cap_chan *chan)
 {
 	/* This clears all conf flags, including CONF_NOT_COMPLETE */
@@ -5430,7 +5444,21 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 
 		l2cap_conn_put(conn);
 	}
+}
+
+void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 status)
+{
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct hci_dev *hdev = hcon->hdev;
+	struct phy_link *plink;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	l2cap_send_chan_create_req(mgr->bredr_chan, plink->remote_id);
 
+	phylink_put(plink);
 }
 
 int l2cap_disconn_ind(struct hci_conn *hcon)
-- 
1.7.9.5

[RFCv3 00/20] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (19):
  Bluetooth: General HCI callback implementation
  Bluetooth: Add callback clear to ops->teardown
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: A2MP: Create A2MP workqueue
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Process physical link complete event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   16 ++
 include/net/bluetooth/amp.h      |   27 +++
 include/net/bluetooth/hci.h      |    3 +
 include/net/bluetooth/hci_core.h |   42 +++++
 include/net/bluetooth/l2cap.h    |    1 +
 include/net/bluetooth/pal.h      |   58 +++++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  345 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  294 ++++++++++++++++++++++++++++++++
 net/bluetooth/hci_core.c         |  106 ++++++++++++
 net/bluetooth/hci_event.c        |  139 ++++++++++++++-
 net/bluetooth/l2cap_core.c       |   39 ++++-
 net/bluetooth/pal.c              |  330 ++++++++++++++++++++++++++++++++++++
 14 files changed, 1374 insertions(+), 29 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[RFCv3 01/20] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   25 +++++++++
 net/bluetooth/hci_core.c         |  107 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 132 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 41d9439..0985014 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -236,6 +247,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1092,5 +1106,16 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
+struct hci_cb_cmd *hci_callback_find(struct hci_dev *hdev, __u16 opcode);
+int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+			  void *param,
+			  void (*cb)(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd),
+			  void *opt,
+			  void (*destructor)(struct hci_dev *hdev,
+					     struct hci_cb_cmd *cmd),
+			  gfp_t flags);
+void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 28bab9d..ecc8644 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,6 +36,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_callback_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1645,6 +1646,7 @@ struct hci_dev *hci_alloc_dev(void)
 
 	mutex_init(&hdev->lock);
 	mutex_init(&hdev->req_lock);
+	mutex_init(&hdev->cb_list_lock);
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 	INIT_LIST_HEAD(&hdev->blacklist);
@@ -1652,6 +1654,7 @@ struct hci_dev *hci_alloc_dev(void)
 	INIT_LIST_HEAD(&hdev->link_keys);
 	INIT_LIST_HEAD(&hdev->long_term_keys);
 	INIT_LIST_HEAD(&hdev->remote_oob_data);
+	INIT_LIST_HEAD(&hdev->cb_list);
 
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
@@ -1817,6 +1820,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
+	hci_callback_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2118,6 +2122,109 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_callback_add(struct hci_dev *hdev, __u16 opcode,
+			    void (*cb)(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd),
+			    void *opt,
+			    void (*destructor)(struct hci_dev *hdev,
+					       struct hci_cb_cmd *cmd),
+			    gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_callback_find(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s remove cmd %p", hdev->name, cmd);
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(hdev, cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_callback_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_callback_remove(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+			  void *param,
+			  void (*cb)(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd),
+			  void *opt,
+			  void (*destructor)(struct hci_dev *hdev,
+					     struct hci_cb_cmd *cmd),
+			  gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_callback_add(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
+void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = hci_callback_find(hdev, opcode);
+	if (!cmd)
+		return;
+
+	cmd->status = status;
+	cmd->cb(hdev, cmd);
+
+	hci_callback_remove(hdev, cmd);
+	hci_dev_put(hdev);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv3 02/20] Bluetooth: Add callback clear to ops->teardown

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

ops->teardown takes care about deleting queued callbacks for all AMP
controllers.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   17 ++++++++++++++++-
 net/bluetooth/hci_core.c         |    3 +--
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 0985014..ccbf01d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1117,5 +1117,6 @@ int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
 			  gfp_t flags);
 void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status);
+void hci_callback_clear(struct hci_dev *hdev);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 44ef201..3d872db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -444,16 +444,31 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
+static void a2mp_chan_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	struct hci_dev *hdev, *tmp;
+
+	BT_DBG("chan %p", chan);
+
+	list_for_each_entry_safe(hdev, tmp, &hci_dev_list, list) {
+		hci_dev_hold(hdev);
+		/* Iterate through AMP controllers */
+		if (hdev->amp_type == HCI_AMP)
+			hci_callback_clear(hdev);
+		hci_dev_put(hdev);
+	}
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
+	.teardown = a2mp_chan_teardown_cb,
 
 	/* Not implemented for A2MP */
 	.new_connection = l2cap_chan_no_new_connection,
-	.teardown = l2cap_chan_no_teardown,
 	.ready = l2cap_chan_no_ready,
 };
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index ecc8644..60ba01f 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,7 +36,6 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
-static void hci_callback_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -2180,7 +2179,7 @@ void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 	}
 }
 
-static void hci_callback_clear(struct hci_dev *hdev)
+void hci_callback_clear(struct hci_dev *hdev)
 {
 	struct hci_cb_cmd *cmd, *tmp;
 
-- 
1.7.9.5

[RFCv3 03/20] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with callback to be executed upon receiving
command complete event. Callback will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 include/net/bluetooth/amp.h  |   19 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |   28 +++++++++---------
 net/bluetooth/amp.c          |   64 ++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c    |    6 +++-
 6 files changed, 104 insertions(+), 16 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..ec77ddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..ec7bea7
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3d872db..3580f3d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -189,24 +189,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
-	}
+	if (!hdev)
+		goto send_err;
 
-	if (hdev)
+	if (hdev->dev_type != HCI_BREDR) {
+		amp_read_loc_info(hdev, mgr);
+		goto done;
+	} else {
 		hci_dev_put(hdev);
+	}
+
+send_err:
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..f26a014
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,64 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete_cb(struct hci_dev *hdev,
+					  struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct a2mp_info_rsp rsp;
+
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+}
+
+static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	hci_dev_put(hdev);
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+			      amp_read_loc_info_complete_cb, mgr,
+			      cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index f3a4568..0468ab0 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -29,6 +29,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -845,7 +846,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto process_cb;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -859,6 +860,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+process_cb:
+	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[RFCv3 04/20] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
A2MP Get AMP Assoc Response is run from HCI callback.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++
 net/bluetooth/a2mp.c             |   13 +++++----
 net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
 6 files changed, 120 insertions(+), 6 deletions(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ec7bea7..e861675 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 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);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 23cf413..7335fe1 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index ccbf01d..2094e02 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -135,6 +135,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -188,6 +194,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3580f3d..3468599 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -230,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index f26a014..725a9f0 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -62,3 +62,63 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      amp_read_loc_info_complete_cb, mgr,
 			      cb_destructor, GFP_KERNEL);
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete_cb(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp),
+			      &cp, amp_read_loc_assoc_complete_cb, mgr,
+			      cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0468ab0..5f68c70 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -865,6 +866,42 @@ process_cb:
 	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto process_cb;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+process_cb:
+	/* Run callback when all fragments received */
+	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2281,6 +2318,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[RFCv3 05/20] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3468599..a264f22 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -161,6 +169,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -378,8 +435,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv3 06/20] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside amp_mgr structure and represent AMP physical links.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   42 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  141 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 193 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ec77ddc..012f573 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,9 @@ struct amp_mgr {
 	__u8			ident;
 	__u8			handle;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..201c501
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size);
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void phylink_list_flush(struct amp_mgr *mgr);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a264f22..3df7cb5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -594,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -628,6 +630,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..24fb3aa
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,141 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[RFCv3 07/20] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 012f573..8ba236c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -29,6 +29,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 201c501..3223ec2 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3df7cb5..f35d90f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -634,6 +635,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 24fb3aa..335cbc3 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,87 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

[RFCv3 08/20] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/a2mp.c        |   19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3223ec2..6ce1dfb 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -52,5 +52,6 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f35d90f..3a0e3b8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -309,6 +309,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -326,6 +327,11 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = phylink_add(mgr, rsp.local_id, rsp.remote_id, req->amp_assoc,
+			    le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -345,6 +351,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -361,8 +368,20 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+	phylink_del(mgr, plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[RFCv3 09/20] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3a0e3b8..5a238df 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -269,6 +269,35 @@ done:
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -459,8 +488,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[RFCv3 10/20] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   58 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 57 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5a238df..d582fc8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -331,6 +331,59 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			goto done;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		goto done;
+
+	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+			    ctrl->assoc_len);
+
+	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
+
+done:
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -492,8 +545,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[RFCv3 11/20] Bluetooth: A2MP: Create A2MP workqueue

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Workqueue might be used for A2MP tasks which cannot be run from
L2CAP of HCI code due to unsafe locking scenarios. For example in
l2cap_security_cfm and l2cap_conn_start we need to discover and
create amp manager from the code surrounded with &conn->chan_lock
and &chan->lock.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   38 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |    6 ++++++
 3 files changed, 46 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 8ba236c..345fbc6 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -129,5 +129,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+int a2mp_init(void);
+void a2mp_exit(void);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d582fc8..f331a81 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -19,6 +19,8 @@
 #include <net/bluetooth/amp.h>
 #include <net/bluetooth/pal.h>
 
+static struct workqueue_struct *amp_workqueue;
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -766,3 +768,39 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+void l2cap_discover_amp(struct l2cap_chan *chan)
+{
+	struct a2mp_discov_req req;
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+
+	BT_DBG("%p", conn);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
+
+int a2mp_init(void)
+{
+	amp_workqueue = create_singlethread_workqueue("a2mp");
+	if (!amp_workqueue)
+		return -EPERM;
+
+	return 0;
+}
+
+void a2mp_exit(void)
+{
+	flush_workqueue(amp_workqueue);
+	destroy_workqueue(amp_workqueue);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 4dbbb79..d4e99d6 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -5730,11 +5730,17 @@ int __init l2cap_init(void)
 			BT_ERR("Failed to create L2CAP debug file");
 	}
 
+	if (enable_hs)
+		return a2mp_init();
+
 	return 0;
 }
 
 void l2cap_exit(void)
 {
+	if (enable_hs)
+		a2mp_exit();
+
 	debugfs_remove(l2cap_debugfs);
 	l2cap_cleanup_sockets();
 }
-- 
1.7.9.5

[RFCv3 12/20] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   31 ++++++++++++++++++++++++++++---
 net/bluetooth/l2cap_core.c   |   31 +++++++++++++++++++++++++++----
 3 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 345fbc6..f3f0d7e 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -22,6 +22,7 @@
 struct amp_mgr {
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -129,6 +130,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_discover_amp(struct l2cap_chan *chan);
 int a2mp_init(void);
 void a2mp_exit(void);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f331a81..c252c63 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -769,18 +769,25 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 	return mgr->a2mp_chan;
 }
 
-void l2cap_discover_amp(struct l2cap_chan *chan)
+struct amp_work {
+	struct work_struct work;
+	struct l2cap_chan *chan;
+};
+
+static void a2mp_discover_amp_worker(struct work_struct *w)
 {
-	struct a2mp_discov_req req;
+	struct amp_work *work = (struct amp_work *) w;
+	struct l2cap_chan *chan = work->chan;
 	struct l2cap_conn *conn = chan->conn;
 	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
 
 	BT_DBG("%p", conn);
 
 	if (!mgr) {
 		mgr = amp_mgr_create(conn);
 		if (!mgr)
-			return;
+			goto clean;
 	}
 
 	mgr->bredr_chan = chan;
@@ -788,6 +795,24 @@ void l2cap_discover_amp(struct l2cap_chan *chan)
 	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
 	req.ext_feat = 0;
 	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+
+clean:
+	kfree(w);
+}
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct amp_work *work;
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, a2mp_discover_amp_worker);
+	work->chan = chan;
+
+	if (!queue_work(amp_workqueue, &work->work))
+		kfree(work);
 }
 
 int a2mp_init(void)
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index d4e99d6..a4a512e 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1005,6 +1005,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1031,6 +1043,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1045,8 +1067,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1142,7 +1165,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5526,7 +5549,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_choose_conn(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[RFCv3 13/20] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 335cbc3..e766a98 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -220,3 +221,37 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[RFCv3 14/20] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 6ce1dfb..8799285 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -53,5 +53,6 @@ int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
 void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index e766a98..b4eeb6a 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -255,3 +255,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[RFCv3 15/20] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define callback which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   31 +++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |   11 +++++++++++
 5 files changed, 50 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 8799285..d9eb87e 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c252c63..fedfdb9 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -378,9 +378,13 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 			    ctrl->assoc_len);
+	if (!plink)
+		goto done;
 
 	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
 
+	amp_create_phylink(hdev, mgr, plink);
+
 done:
 	skb_pull(skb, len);
 	return 0;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 725a9f0..7f2c0fa 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 static void amp_read_loc_info_complete_cb(struct hci_dev *hdev,
 					  struct hci_cb_cmd *cmd)
@@ -122,3 +123,33 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      &cp, amp_read_loc_assoc_complete_cb, mgr,
 			      cb_destructor, GFP_KERNEL);
 }
+
+static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* Write Remote AMP Assoc */
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp,
+			      amp_create_phylink_cs_cb, mgr, cb_destructor,
+			      GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 5f68c70..8110b94 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1685,6 +1685,13 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	hci_callback_process(hdev, HCI_OP_CREATE_PHY_LINK, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2475,6 +2482,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[RFCv3 16/20] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   15 +++++++
 3 files changed, 97 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 2094e02..fbf929a 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -138,6 +138,8 @@ struct hci_cb_cmd {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 7f2c0fa..409ca3d 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -124,14 +124,94 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      cb_destructor, GFP_KERNEL);
 }
 
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink);
+
+static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
+				      struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p status 0x%2.2x", mgr, cmd->status);
+
+	if (cmd->status)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, mgr, plink);
+
+	phylink_put(plink);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
+	if (!ctrl)
+		return;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       plink, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp,
+			      amp_write_rem_assoc_cs_cb, mgr, cb_destructor,
+			      GFP_KERNEL);
+
+	kfree(cp);
+}
+
 static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
 				     struct hci_cb_cmd *cmd)
 {
 	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
 
 	BT_DBG("mgr %p", mgr);
 
 	/* Write Remote AMP Assoc */
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+		phylink_put(plink);
+	}
 }
 
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 8110b94..2f54e76 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1214,6 +1214,17 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	hci_callback_process(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, rp->status);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -2401,6 +2412,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[RFCv3 17/20] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 26f1163..a5d6d16 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -775,5 +775,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index fedfdb9..b143331 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -179,6 +179,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -209,6 +210,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -218,6 +220,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a4a512e..c34fcd0 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1017,7 +1017,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[RFCv3 18/20] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list which is used for query for phy_link by
phy_link handler.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    4 ++++
 include/net/bluetooth/pal.h  |    1 +
 net/bluetooth/a2mp.c         |   12 ++++++++++++
 net/bluetooth/pal.c          |   28 ++++++++++++++++++++++++++++
 4 files changed, 45 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f3f0d7e..eaaa3e4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,6 +20,7 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct l2cap_chan	*bredr_chan;
@@ -125,6 +126,9 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index d9eb87e..55f4d5b 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -48,6 +48,7 @@ void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+struct phy_link *phylink_lookup_by_handle(u8 handle);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b143331..4b6350f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -21,6 +21,10 @@
 
 static struct workqueue_struct *amp_workqueue;
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -736,6 +740,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
@@ -780,6 +788,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->amp_ctrls);
 	mutex_init(&mgr->amp_ctrls_lock);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index b4eeb6a..f8bb66c 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -222,6 +222,34 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 	return found;
 }
 
+/* Returns phy_link referenced if found */
+struct phy_link *phylink_lookup_by_handle(u8 handle)
+{
+	struct amp_mgr *mgr;
+	struct phy_link *found = NULL;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		struct phy_link *plink;
+
+		mutex_lock(&mgr->phy_links_lock);
+		list_for_each_entry(plink, &mgr->phy_links, list) {
+			if (plink->handle == handle) {
+				phylink_get(plink);
+				found = plink;
+				break;
+			}
+		}
+		mutex_unlock(&mgr->phy_links_lock);
+
+		if (found)
+			break;
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return found;
+}
+
 int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 {
 	int ret = 0;
-- 
1.7.9.5

[RFCv3 19/20] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h     |    1 +
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci_core.h |    6 ++++
 net/bluetooth/a2mp.c             |    2 +-
 net/bluetooth/amp.c              |   71 ++++++++++++++++++++++++++++++++++----
 net/bluetooth/hci_core.c         |   14 ++++----
 net/bluetooth/hci_event.c        |   23 ++++++++++++
 7 files changed, 105 insertions(+), 14 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index eaaa3e4..c8ecbe5 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -131,6 +131,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);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b376cc3..ab4e195 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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 phy_link *plink);
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink);
 
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index fbf929a..e106e17 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1117,6 +1117,12 @@ int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
 struct hci_cb_cmd *hci_callback_find(struct hci_dev *hdev, __u16 opcode);
+int hci_callback_add(struct hci_dev *hdev, __u16 opcode,
+		     void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+		     void *opt,
+		     void (*destructor)(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd),
+		     gfp_t flags);
 int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
 			  void *param,
 			  void (*cb)(struct hci_dev *hdev,
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4b6350f..c92ab54 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -70,7 +70,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;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 409ca3d..72991c3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -124,7 +124,59 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      cb_destructor, GFP_KERNEL);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Read Local AMP Assoc final link information data callback */
+static void amp_read_loc_assoc_complete_final_cb(struct hci_dev *hdev,
+						 struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_physlink_req *req;
+	struct phy_link *plink;
+	size_t len;
+
+	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)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		goto clean;
+
+	req->local_id = plink->local_id;
+	req->remote_id = plink->remote_id;
+	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+	phylink_put(plink);
+
+	a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+	kfree(req);
+}
+
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+				   struct phy_link *plink)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_mgr *mgr = plink->mgr;
+
+	cp.phy_handle = plink->handle;
+	cp.len_so_far = cpu_to_le16(0);
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	/* Read Local AMP Assoc final link information data */
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp),
+			      &cp, amp_read_loc_assoc_complete_final_cb,
+			      mgr, cb_destructor, GFP_KERNEL);
+}
+
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink);
 
 static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
@@ -142,12 +194,16 @@ static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
 	if (!plink)
 		return;
 
-	amp_write_rem_assoc_frag(hdev, mgr, plink);
+	/* All fragments are written */
+	if (amp_write_rem_assoc_frag(hdev, mgr, plink)) {
+		/* Expect Channel Select event */
+	}
 
 	phylink_put(plink);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink)
 {
 	struct hci_cp_write_remote_amp_assoc *cp;
@@ -156,7 +212,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
 	if (!ctrl)
-		return;
+		return false;
 
 	if (!ctrl->assoc_rem_len) {
 		BT_DBG("all fragments are written");
@@ -164,7 +220,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 		ctrl->assoc_len_so_far = 0;
 
 		amp_ctrl_put(ctrl);
-		return;
+		return true;
 	}
 
 	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
@@ -173,7 +229,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 	cp = kzalloc(len, GFP_KERNEL);
 	if (!cp) {
 		amp_ctrl_put(ctrl);
-		return;
+		return false;
 	}
 
 	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
@@ -196,6 +252,8 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 			      GFP_KERNEL);
 
 	kfree(cp);
+
+	return false;
 }
 
 static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
@@ -209,6 +267,7 @@ static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
 	/* Write Remote AMP Assoc */
 	plink = phylink_lookup(mgr, hdev->id, 0);
 	if (plink) {
+		hci_dev_hold(hdev);
 		amp_write_rem_assoc_frag(hdev, mgr, plink);
 		phylink_put(plink);
 	}
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 60ba01f..688ea40 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2121,13 +2121,13 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
-static int hci_callback_add(struct hci_dev *hdev, __u16 opcode,
-			    void (*cb)(struct hci_dev *hdev,
-				       struct hci_cb_cmd *cmd),
-			    void *opt,
-			    void (*destructor)(struct hci_dev *hdev,
-					       struct hci_cb_cmd *cmd),
-			    gfp_t flags)
+int hci_callback_add(struct hci_dev *hdev, __u16 opcode,
+		     void (*cb)(struct hci_dev *hdev,
+				struct hci_cb_cmd *cmd),
+		     void *opt,
+		     void (*destructor)(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd),
+		     gfp_t flags)
 {
 	struct hci_cb_cmd *cmd;
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 2f54e76..bb52a0e 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3533,6 +3533,25 @@ 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;
+	__u8 phy_handle = ev->phy_handle;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x", hdev->name, phy_handle);
+
+	skb_pull(skb, sizeof(*ev));
+
+	plink = phylink_lookup_by_handle(phy_handle);
+	if (!plink)
+		return;
+
+	amp_read_loc_assoc_final_data(hdev, plink);
+
+	phylink_put(plink);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3689,6 +3708,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;
-- 
1.7.9.5

[RFCv3 20/20] Bluetooth: AMP: Process physical link complete event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new hci_conn for representing AMP physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |    1 +
 net/bluetooth/hci_event.c   |   43 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 44 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 7335fe1..581b9c3 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index bb52a0e..66a0df2 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3394,6 +3394,45 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+				      struct sk_buff *skb)
+{
+	struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+	struct hci_conn *conn;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+	       ev->status);
+
+	if (ev->status)
+		return;
+
+	plink = phylink_lookup_by_handle(ev->phy_handle);
+	if (!plink)
+		return;
+
+	hci_dev_lock(hdev);
+
+	conn = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (conn) {
+		conn->handle = ev->phy_handle;
+		conn->state = BT_CONNECTED;
+
+		hci_conn_hold(conn);
+		conn->disc_timeout = HCI_DISCONN_TIMEOUT/2;
+		hci_conn_put(conn);
+
+		hci_conn_hold_device(conn);
+		hci_conn_add_sysfs(conn);
+	} else {
+		BT_ERR("Cannot add connection");
+	}
+
+	hci_dev_unlock(hdev);
+
+	phylink_put(plink);
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3716,6 +3755,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_remote_oob_data_request_evt(hdev, skb);
 		break;
 
+	case HCI_EV_PHY_LINK_COMPLETE:
+		hci_phy_link_complete_evt(hdev, skb);
+		break;
+
 	case HCI_EV_NUM_COMP_BLOCKS:
 		hci_num_comp_blocks_evt(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv1 00/26] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (25):
  Bluetooth: debug: Print refcnt for hci_dev
  Bluetooth: trivial: Remove empty line
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: General HCI callback implementation
  Bluetooth: Add callback clear to ops->teardown
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: A2MP: Create A2MP workqueue
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Process physical link complete event
  Bluetooth: AMP: Send Create Chan Req
  Bluetooth: Set Exended Flowspec flag for HS chan
  Bluetooth: Add logical link create function

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   16 ++
 include/net/bluetooth/amp.h      |   28 +++
 include/net/bluetooth/hci.h      |   39 ++++-
 include/net/bluetooth/hci_core.h |   48 ++++++
 include/net/bluetooth/l2cap.h    |    3 +
 include/net/bluetooth/pal.h      |   58 +++++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  346 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  339 +++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_core.c         |  106 +++++++++++-
 net/bluetooth/hci_event.c        |  166 +++++++++++++++++-
 net/bluetooth/l2cap_core.c       |   75 ++++++++-
 net/bluetooth/pal.c              |  330 ++++++++++++++++++++++++++++++++++++
 14 files changed, 1526 insertions(+), 31 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[PATCHv1 01/26] Bluetooth: debug: Print refcnt for hci_dev

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1bbc109..5354e7b 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -612,11 +612,17 @@ static inline void hci_conn_put(struct hci_conn *conn)
 /* ----- HCI Devices ----- */
 static inline void hci_dev_put(struct hci_dev *d)
 {
+	BT_DBG("%s orig refcnt %d", d->name,
+	       atomic_read(&d->dev.kobj.kref.refcount));
+
 	put_device(&d->dev);
 }
 
 static inline struct hci_dev *hci_dev_hold(struct hci_dev *d)
 {
+	BT_DBG("%s orig refcnt %d", d->name,
+	       atomic_read(&d->dev.kobj.kref.refcount));
+
 	get_device(&d->dev);
 	return d;
 }
-- 
1.7.9.5

[PATCHv1 02/26] Bluetooth: trivial: Remove empty line

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/hci_core.c |    1 -
 1 file changed, 1 deletion(-)

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 28bab9d..42c00d18 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -268,7 +268,6 @@ static void hci_init_req(struct hci_dev *hdev, unsigned long opt)
 		BT_ERR("Unknown device type %d", hdev->dev_type);
 		break;
 	}
-
 }
 
 static void hci_le_init_req(struct hci_dev *hdev, unsigned long opt)
-- 
1.7.9.5

[PATCHv1 03/26] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv1 04/26] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add general HCI callback implementation. Can be used for executing
HCI commands from A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |   25 +++++++++
 net/bluetooth/hci_core.c         |  106 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 131 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 5354e7b..3b98640 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -236,6 +247,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1096,5 +1110,16 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
 u8 bdaddr_to_le(u8 bdaddr_type);
+struct hci_cb_cmd *hci_callback_find(struct hci_dev *hdev, __u16 opcode);
+int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+			  void *param,
+			  void (*cb)(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd),
+			  void *opt,
+			  void (*destructor)(struct hci_dev *hdev,
+					     struct hci_cb_cmd *cmd),
+			  gfp_t flags);
+void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 42c00d18..a70f933 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,6 +36,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_callback_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1644,6 +1645,7 @@ struct hci_dev *hci_alloc_dev(void)
 
 	mutex_init(&hdev->lock);
 	mutex_init(&hdev->req_lock);
+	mutex_init(&hdev->cb_list_lock);
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 	INIT_LIST_HEAD(&hdev->blacklist);
@@ -1651,6 +1653,7 @@ struct hci_dev *hci_alloc_dev(void)
 	INIT_LIST_HEAD(&hdev->link_keys);
 	INIT_LIST_HEAD(&hdev->long_term_keys);
 	INIT_LIST_HEAD(&hdev->remote_oob_data);
+	INIT_LIST_HEAD(&hdev->cb_list);
 
 	INIT_WORK(&hdev->rx_work, hci_rx_work);
 	INIT_WORK(&hdev->cmd_work, hci_cmd_work);
@@ -1816,6 +1819,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_link_keys_clear(hdev);
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
+	hci_callback_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2117,6 +2121,108 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_callback_add(struct hci_dev *hdev, __u16 opcode,
+			    void (*cb)(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd),
+			    void *opt,
+			    void (*destructor)(struct hci_dev *hdev,
+					       struct hci_cb_cmd *cmd),
+			    gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_callback_find(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s remove cmd %p", hdev->name, cmd);
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(hdev, cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_callback_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_callback_remove(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+			  void *param,
+			  void (*cb)(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd),
+			  void *opt,
+			  void (*destructor)(struct hci_dev *hdev,
+					     struct hci_cb_cmd *cmd),
+			  gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_callback_add(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
+void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = hci_callback_find(hdev, opcode);
+	if (!cmd)
+		return;
+
+	cmd->status = status;
+	cmd->cb(hdev, cmd);
+
+	hci_callback_remove(hdev, cmd);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[PATCHv1 05/26] Bluetooth: Add callback clear to ops->teardown

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

ops->teardown takes care about deleting queued callbacks for all AMP
controllers.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   17 ++++++++++++++++-
 net/bluetooth/hci_core.c         |    3 +--
 3 files changed, 18 insertions(+), 3 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 3b98640..54b7aa4 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1121,5 +1121,6 @@ int hci_callback_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
 			  gfp_t flags);
 void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 void hci_callback_process(struct hci_dev *hdev, __u16 opcode, u8 status);
+void hci_callback_clear(struct hci_dev *hdev);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 44ef201..3d872db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -444,16 +444,31 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
+static void a2mp_chan_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	struct hci_dev *hdev, *tmp;
+
+	BT_DBG("chan %p", chan);
+
+	list_for_each_entry_safe(hdev, tmp, &hci_dev_list, list) {
+		hci_dev_hold(hdev);
+		/* Iterate through AMP controllers */
+		if (hdev->amp_type == HCI_AMP)
+			hci_callback_clear(hdev);
+		hci_dev_put(hdev);
+	}
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
+	.teardown = a2mp_chan_teardown_cb,
 
 	/* Not implemented for A2MP */
 	.new_connection = l2cap_chan_no_new_connection,
-	.teardown = l2cap_chan_no_teardown,
 	.ready = l2cap_chan_no_ready,
 };
 
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index a70f933..003f6ac 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -36,7 +36,6 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
-static void hci_callback_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -2179,7 +2178,7 @@ void hci_callback_remove(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 	}
 }
 
-static void hci_callback_clear(struct hci_dev *hdev)
+void hci_callback_clear(struct hci_dev *hdev)
 {
 	struct hci_cb_cmd *cmd, *tmp;
 
-- 
1.7.9.5

[PATCHv1 06/26] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with callback to be executed upon receiving
command complete event. Callback will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    1 +
 include/net/bluetooth/amp.h  |   19 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |   28 +++++++++---------
 net/bluetooth/amp.c          |   64 ++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c    |    6 +++-
 6 files changed, 104 insertions(+), 16 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..ec77ddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,5 +122,6 @@ void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..ec7bea7
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3d872db..3580f3d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -37,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -189,24 +189,24 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
-	}
+	if (!hdev)
+		goto send_err;
 
-	if (hdev)
+	if (hdev->dev_type != HCI_BREDR) {
+		amp_read_loc_info(hdev, mgr);
+		goto done;
+	} else {
 		hci_dev_put(hdev);
+	}
+
+send_err:
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..f26a014
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,64 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete_cb(struct hci_dev *hdev,
+					  struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct a2mp_info_rsp rsp;
+
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+}
+
+static void cb_destructor(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	hci_dev_put(hdev);
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+			      amp_read_loc_info_complete_cb, mgr,
+			      cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 1f49957..337b112 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto process_cb;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+process_cb:
+	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv1 07/26] Bluetooth: AMP: Use HCI callback to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
A2MP Get AMP Assoc Response is run from HCI callback.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 ++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++
 net/bluetooth/a2mp.c             |   13 +++++----
 net/bluetooth/amp.c              |   60 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++
 6 files changed, 120 insertions(+), 6 deletions(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ec7bea7..e861675 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 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);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 54b7aa4..b1a0d43 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -135,6 +135,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -188,6 +194,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3580f3d..3468599 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -230,15 +230,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index f26a014..725a9f0 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -62,3 +62,63 @@ void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      amp_read_loc_info_complete_cb, mgr,
 			      cb_destructor, GFP_KERNEL);
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete_cb(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp),
+			      &cp, amp_read_loc_assoc_complete_cb, mgr,
+			      cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 337b112..974165b 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ process_cb:
 	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto process_cb;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+process_cb:
+	/* Run callback when all fragments received */
+	hci_callback_process(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2301,6 +2338,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv1 08/26] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3468599..a264f22 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -161,6 +169,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -378,8 +435,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv1 09/26] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside amp_mgr structure and represent AMP physical links.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   42 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  141 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 193 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ec77ddc..012f573 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -26,6 +26,9 @@ struct amp_mgr {
 	__u8			ident;
 	__u8			handle;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..201c501
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size);
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void phylink_list_flush(struct amp_mgr *mgr);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a264f22..3df7cb5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -594,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -628,6 +630,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..24fb3aa
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,141 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[PATCHv1 10/26] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 012f573..8ba236c 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -29,6 +29,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 201c501..3223ec2 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3df7cb5..f35d90f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -634,6 +635,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 24fb3aa..335cbc3 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,87 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

[PATCHv1 11/26] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/a2mp.c        |   19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3223ec2..6ce1dfb 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -52,5 +52,6 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f35d90f..3a0e3b8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -309,6 +309,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -326,6 +327,11 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = phylink_add(mgr, rsp.local_id, rsp.remote_id, req->amp_assoc,
+			    le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -345,6 +351,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -361,8 +368,20 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+	phylink_del(mgr, plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv1 12/26] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3a0e3b8..5a238df 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -269,6 +269,35 @@ done:
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -459,8 +488,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv1 13/26] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   59 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 58 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5a238df..6f719d3 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -331,6 +331,60 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+			    ctrl->assoc_len);
+
+	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -492,8 +546,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv1 14/26] Bluetooth: A2MP: Create A2MP workqueue

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Workqueue might be used for A2MP tasks which cannot be run from
L2CAP of HCI code due to unsafe locking scenarios. For example in
l2cap_security_cfm and l2cap_conn_start we need to discover and
create amp manager from the code surrounded with &conn->chan_lock
and &chan->lock.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   38 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |    6 ++++++
 3 files changed, 46 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 8ba236c..345fbc6 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -129,5 +129,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+int a2mp_init(void);
+void a2mp_exit(void);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6f719d3..26a225b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -19,6 +19,8 @@
 #include <net/bluetooth/amp.h>
 #include <net/bluetooth/pal.h>
 
+static struct workqueue_struct *amp_workqueue;
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -767,3 +769,39 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+void l2cap_discover_amp(struct l2cap_chan *chan)
+{
+	struct a2mp_discov_req req;
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+
+	BT_DBG("%p", conn);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
+
+int a2mp_init(void)
+{
+	amp_workqueue = create_singlethread_workqueue("a2mp");
+	if (!amp_workqueue)
+		return -EPERM;
+
+	return 0;
+}
+
+void a2mp_exit(void)
+{
+	flush_workqueue(amp_workqueue);
+	destroy_workqueue(amp_workqueue);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 4dbbb79..d4e99d6 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -5730,11 +5730,17 @@ int __init l2cap_init(void)
 			BT_ERR("Failed to create L2CAP debug file");
 	}
 
+	if (enable_hs)
+		return a2mp_init();
+
 	return 0;
 }
 
 void l2cap_exit(void)
 {
+	if (enable_hs)
+		a2mp_exit();
+
 	debugfs_remove(l2cap_debugfs);
 	l2cap_cleanup_sockets();
 }
-- 
1.7.9.5

[PATCHv1 15/26] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   31 ++++++++++++++++++++++++++++---
 net/bluetooth/l2cap_core.c   |   31 +++++++++++++++++++++++++++----
 3 files changed, 57 insertions(+), 7 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 345fbc6..f3f0d7e 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -22,6 +22,7 @@
 struct amp_mgr {
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -129,6 +130,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_discover_amp(struct l2cap_chan *chan);
 int a2mp_init(void);
 void a2mp_exit(void);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 26a225b..7113c76 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -770,18 +770,25 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 	return mgr->a2mp_chan;
 }
 
-void l2cap_discover_amp(struct l2cap_chan *chan)
+struct amp_work {
+	struct work_struct work;
+	struct l2cap_chan *chan;
+};
+
+static void a2mp_discover_amp_worker(struct work_struct *w)
 {
-	struct a2mp_discov_req req;
+	struct amp_work *work = (struct amp_work *) w;
+	struct l2cap_chan *chan = work->chan;
 	struct l2cap_conn *conn = chan->conn;
 	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
 
 	BT_DBG("%p", conn);
 
 	if (!mgr) {
 		mgr = amp_mgr_create(conn);
 		if (!mgr)
-			return;
+			goto clean;
 	}
 
 	mgr->bredr_chan = chan;
@@ -789,6 +796,24 @@ void l2cap_discover_amp(struct l2cap_chan *chan)
 	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
 	req.ext_feat = 0;
 	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+
+clean:
+	kfree(w);
+}
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct amp_work *work;
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, a2mp_discover_amp_worker);
+	work->chan = chan;
+
+	if (!queue_work(amp_workqueue, &work->work))
+		kfree(work);
 }
 
 int a2mp_init(void)
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index d4e99d6..a4a512e 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1005,6 +1005,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1031,6 +1043,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1045,8 +1067,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1142,7 +1165,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5526,7 +5549,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_choose_conn(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv1 16/26] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 335cbc3..e766a98 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -220,3 +221,37 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[PATCHv1 17/26] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 6ce1dfb..8799285 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -53,5 +53,6 @@ int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
 void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index e766a98..b4eeb6a 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -255,3 +255,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[PATCHv1 18/26] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define callback which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   32 ++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |   11 +++++++++++
 5 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 8799285..d9eb87e 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7113c76..0a440d0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -378,9 +378,13 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 			    ctrl->assoc_len);
+	if (!plink)
+		goto done;
 
 	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
 
+	amp_create_phylink(hdev, mgr, plink);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 725a9f0..f0192cf 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 static void amp_read_loc_info_complete_cb(struct hci_dev *hdev,
 					  struct hci_cb_cmd *cmd)
@@ -122,3 +123,34 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      &cp, amp_read_loc_assoc_complete_cb, mgr,
 			      cb_destructor, GFP_KERNEL);
 }
+
+static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
+				     struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("mgr %p", mgr);
+
+	/* Write Remote AMP Assoc */
+}
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_dev_hold(hdev);
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp,
+			      amp_create_phylink_cs_cb, mgr, cb_destructor,
+			      GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 974165b..8fdd0e0 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1686,6 +1686,13 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
+{
+	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	hci_callback_process(hdev, HCI_OP_CREATE_PHY_LINK, status);
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2495,6 +2502,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv1 19/26] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   81 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   15 +++++++
 3 files changed, 98 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b1a0d43..75e17d8 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -138,6 +138,8 @@ struct hci_cb_cmd {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index f0192cf..652108f 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -124,14 +124,95 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      cb_destructor, GFP_KERNEL);
 }
 
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink);
+
+static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
+				      struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
+
+	BT_DBG("mgr %p status 0x%2.2x", mgr, cmd->status);
+
+	if (cmd->status)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, mgr, plink);
+
+	phylink_put(plink);
+}
+
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
+	if (!ctrl)
+		return;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       plink, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_dev_hold(hdev);
+	amp_mgr_get(mgr);
+
+	hci_callback_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp,
+			      amp_write_rem_assoc_cs_cb, mgr, cb_destructor,
+			      GFP_KERNEL);
+
+	kfree(cp);
+}
+
 static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
 				     struct hci_cb_cmd *cmd)
 {
 	struct amp_mgr *mgr = cmd->opt;
+	struct phy_link *plink;
 
 	BT_DBG("mgr %p", mgr);
 
 	/* Write Remote AMP Assoc */
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, mgr, plink);
+		phylink_put(plink);
+	}
 }
 
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 8fdd0e0..a03b385 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,17 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	hci_callback_process(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, rp->status);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -2421,6 +2432,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv1 20/26] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 26f1163..a5d6d16 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -775,5 +775,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0a440d0..ec5afd1 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -179,6 +179,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -209,6 +210,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -218,6 +220,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a4a512e..c34fcd0 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1017,7 +1017,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv1 21/26] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list which is used for query for phy_link by
phy_link handler.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    4 ++++
 include/net/bluetooth/pal.h  |    1 +
 net/bluetooth/a2mp.c         |   12 ++++++++++++
 net/bluetooth/pal.c          |   28 ++++++++++++++++++++++++++++
 4 files changed, 45 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f3f0d7e..eaaa3e4 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,6 +20,7 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct l2cap_chan	*bredr_chan;
@@ -125,6 +126,9 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index d9eb87e..55f4d5b 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -48,6 +48,7 @@ void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+struct phy_link *phylink_lookup_by_handle(u8 handle);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ec5afd1..40e3c94 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -21,6 +21,10 @@
 
 static struct workqueue_struct *amp_workqueue;
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -737,6 +741,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
@@ -781,6 +789,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->amp_ctrls);
 	mutex_init(&mgr->amp_ctrls_lock);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index b4eeb6a..f8bb66c 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -222,6 +222,34 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 	return found;
 }
 
+/* Returns phy_link referenced if found */
+struct phy_link *phylink_lookup_by_handle(u8 handle)
+{
+	struct amp_mgr *mgr;
+	struct phy_link *found = NULL;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		struct phy_link *plink;
+
+		mutex_lock(&mgr->phy_links_lock);
+		list_for_each_entry(plink, &mgr->phy_links, list) {
+			if (plink->handle == handle) {
+				phylink_get(plink);
+				found = plink;
+				break;
+			}
+		}
+		mutex_unlock(&mgr->phy_links_lock);
+
+		if (found)
+			break;
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return found;
+}
+
 int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 {
 	int ret = 0;
-- 
1.7.9.5

[PATCHv1 22/26] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h |    1 +
 include/net/bluetooth/amp.h  |    2 ++
 net/bluetooth/a2mp.c         |    2 +-
 net/bluetooth/amp.c          |   71 ++++++++++++++++++++++++++++++++++++++----
 net/bluetooth/hci_event.c    |   23 ++++++++++++++
 5 files changed, 92 insertions(+), 7 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index eaaa3e4..c8ecbe5 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -131,6 +131,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);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b376cc3..ab4e195 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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 phy_link *plink);
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 40e3c94..a57d99e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -70,7 +70,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;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 652108f..07afb21 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -124,7 +124,60 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 			      cb_destructor, GFP_KERNEL);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Read Local AMP Assoc final link information data callback */
+static void amp_read_loc_assoc_complete_final_cb(struct hci_dev *hdev,
+						 struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_physlink_req *req;
+	struct phy_link *plink;
+	size_t len;
+
+	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)
+		return;
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		goto clean;
+
+	req->local_id = plink->local_id;
+	req->remote_id = plink->remote_id;
+	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+	phylink_put(plink);
+
+	a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
+
+clean:
+	kfree(req);
+}
+
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+				   struct phy_link *plink)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_mgr *mgr = plink->mgr;
+
+	cp.phy_handle = plink->handle;
+	cp.len_so_far = cpu_to_le16(0);
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	hci_dev_hold(hdev);
+	amp_mgr_get(mgr);
+
+	/* Read Local AMP Assoc final link information data */
+	hci_callback_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp),
+			      &cp, amp_read_loc_assoc_complete_final_cb,
+			      mgr, cb_destructor, GFP_KERNEL);
+}
+
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink);
 
 static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
@@ -142,12 +195,16 @@ static void amp_write_rem_assoc_cs_cb(struct hci_dev *hdev,
 	if (!plink)
 		return;
 
-	amp_write_rem_assoc_frag(hdev, mgr, plink);
+	/* All fragments are written */
+	if (amp_write_rem_assoc_frag(hdev, mgr, plink)) {
+		/* Expect Channel Select event */
+	}
 
 	phylink_put(plink);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 				     struct phy_link *plink)
 {
 	struct hci_cp_write_remote_amp_assoc *cp;
@@ -156,7 +213,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
 	if (!ctrl)
-		return;
+		return false;
 
 	if (!ctrl->assoc_rem_len) {
 		BT_DBG("all fragments are written");
@@ -164,7 +221,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 		ctrl->assoc_len_so_far = 0;
 
 		amp_ctrl_put(ctrl);
-		return;
+		return true;
 	}
 
 	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
@@ -173,7 +230,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 	cp = kzalloc(len, GFP_KERNEL);
 	if (!cp) {
 		amp_ctrl_put(ctrl);
-		return;
+		return false;
 	}
 
 	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
@@ -197,6 +254,8 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 			      GFP_KERNEL);
 
 	kfree(cp);
+
+	return false;
 }
 
 static void amp_create_phylink_cs_cb(struct hci_dev *hdev,
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index a03b385..15c7ce6 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3553,6 +3553,25 @@ 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;
+	__u8 phy_handle = ev->phy_handle;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x", hdev->name, phy_handle);
+
+	skb_pull(skb, sizeof(*ev));
+
+	plink = phylink_lookup_by_handle(phy_handle);
+	if (!plink)
+		return;
+
+	amp_read_loc_assoc_final_data(hdev, plink);
+
+	phylink_put(plink);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3709,6 +3728,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;
-- 
1.7.9.5

[PATCHv1 23/26] Bluetooth: AMP: Process physical link complete event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new hci_conn for representing AMP physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/hci_event.c        |   47 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 75e17d8..ba9a05f 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -353,6 +353,7 @@ struct hci_conn {
 	struct amp_mgr	*amp_mgr;
 
 	struct hci_conn	*link;
+	struct phy_link	*phy_link;
 
 	void (*connect_cfm_cb)	(struct hci_conn *conn, u8 status);
 	void (*security_cfm_cb)	(struct hci_conn *conn, u8 status);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 15c7ce6..313bbad 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3414,6 +3414,49 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+				      struct sk_buff *skb)
+{
+	struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+	struct hci_conn *conn;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+	       ev->status);
+
+	if (ev->status)
+		return;
+
+	plink = phylink_lookup_by_handle(ev->phy_handle);
+	if (!plink)
+		return;
+
+	hci_dev_lock(hdev);
+
+	conn = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (conn) {
+		conn->handle = ev->phy_handle;
+		conn->state = BT_CONNECTED;
+
+		hci_conn_hold(conn);
+		conn->disc_timeout = HCI_DISCONN_TIMEOUT/2;
+		hci_conn_put(conn);
+
+		bacpy(&conn->dst, &plink->mgr->l2cap_conn->hcon->dst);
+		conn->out = true;
+		conn->phy_link = plink;
+
+		hci_conn_hold_device(conn);
+		hci_conn_add_sysfs(conn);
+	} else {
+		BT_ERR("Cannot add connection");
+	}
+
+	hci_dev_unlock(hdev);
+
+	phylink_put(plink);
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3736,6 +3779,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_remote_oob_data_request_evt(hdev, skb);
 		break;
 
+	case HCI_EV_PHY_LINK_COMPLETE:
+		hci_phy_link_complete_evt(hdev, skb);
+		break;
+
 	case HCI_EV_NUM_COMP_BLOCKS:
 		hci_num_comp_blocks_evt(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv1 24/26] Bluetooth: AMP: Send Create Chan Req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Send L2CAP Create Channel Request when receiving HCI Physical
Link Complete event.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    5 +++++
 net/bluetooth/hci_event.c        |   11 +++++++++++
 net/bluetooth/l2cap_core.c       |   22 ++++++++++++++++++++++
 3 files changed, 38 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index ba9a05f..1be2c81 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -381,6 +381,7 @@ extern void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason);
 extern int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt);
 extern int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb,
 			      u16 flags);
+extern void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 status);
 
 extern int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr);
 extern void sco_connect_cfm(struct hci_conn *hcon, __u8 status);
@@ -791,6 +792,10 @@ static inline void hci_proto_connect_cfm(struct hci_conn *conn, __u8 status)
 		sco_connect_cfm(conn, status);
 		break;
 
+	case AMP_LINK:
+		l2cap_chan_create_cfm(conn, status);
+		break;
+
 	default:
 		BT_ERR("unknown link type %d", conn->type);
 		break;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 313bbad..cdb32da 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3454,6 +3454,17 @@ static void hci_phy_link_complete_evt(struct hci_dev *hdev,
 
 	hci_dev_unlock(hdev);
 
+	if (conn) {
+		struct hci_conn *bredr_conn = plink->mgr->l2cap_conn->hcon;
+		struct hci_dev *bredr_hdev = bredr_conn->hdev;
+
+		if (bredr_hdev) {
+			hci_dev_hold(bredr_hdev);
+			l2cap_chan_create_cfm(bredr_conn, plink->remote_id);
+			hci_dev_put(bredr_hdev);
+		}
+	}
+
 	phylink_put(plink);
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index c34fcd0..12da90b 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/pal.h>
 
 bool disable_ertm;
 
@@ -1032,6 +1033,21 @@ void l2cap_send_conn_req(struct l2cap_chan *chan)
 	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 }
 
+static void l2cap_send_chan_create_req(struct l2cap_chan *chan, u8 remote_id)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct l2cap_create_chan_req req;
+
+	req.scid = cpu_to_le16(chan->scid);
+	req.psm = chan->psm;
+	req.amp_id = remote_id;
+
+	chan->ident = l2cap_get_ident(conn);
+
+	l2cap_send_cmd(conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
+		       sizeof(req), &req);
+}
+
 static void l2cap_chan_ready(struct l2cap_chan *chan)
 {
 	/* This clears all conf flags, including CONF_NOT_COMPLETE */
@@ -5447,7 +5463,13 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 
 		l2cap_conn_put(conn);
 	}
+}
+
+void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 remote_id)
+{
+	struct amp_mgr *mgr = hcon->amp_mgr;
 
+	l2cap_send_chan_create_req(mgr->bredr_chan, remote_id);
 }
 
 int l2cap_disconn_ind(struct hci_conn *hcon)
-- 
1.7.9.5

[PATCHv1 25/26] Bluetooth: Set Exended Flowspec flag for HS chan

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

For AMP we always assume EFS L2CAP option.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    2 ++
 net/bluetooth/l2cap_core.c    |    7 ++++++-
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index a5d6d16..2cf7229 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -509,6 +509,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/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 12da90b..d99eaf9 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -5468,8 +5468,13 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 remote_id)
 {
 	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct l2cap_chan *chan = mgr->bredr_chan;
 
-	l2cap_send_chan_create_req(mgr->bredr_chan, remote_id);
+	/* Set Extended Flow Spec for AMP */
+	set_bit(FLAG_EFS_ENABLE, &chan->flags);
+	chan->ctrl_id = remote_id;
+
+	l2cap_send_chan_create_req(chan, remote_id);
 }
 
 int l2cap_disconn_ind(struct hci_conn *hcon)
-- 
1.7.9.5

[PATCHv1 26/26] Bluetooth: Add logical link create function

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

After physical link is created logical link needs to be created
above physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    1 +
 net/bluetooth/amp.c         |   43 +++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c   |   12 ++++++++++++
 net/bluetooth/l2cap_core.c  |    9 +++++++++
 4 files changed, 65 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index ab4e195..dcc05a0 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -23,5 +23,6 @@ void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
 				   struct phy_link *plink);
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink);
+void amp_create_logical_link(struct l2cap_chan *chan);
 
 #endif /* __AMP_H */
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 07afb21..1693ea9 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -294,3 +294,46 @@ void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			      amp_create_phylink_cs_cb, mgr, cb_destructor,
 			      GFP_KERNEL);
 }
+
+void amp_create_logical_link(struct l2cap_chan *chan)
+{
+	struct hci_chan *hchan = chan->conn->hchan;
+	struct hci_cp_create_accept_logical_link cp;
+	struct phy_link *plink;
+	struct hci_conn *hcon;
+	struct hci_dev *hdev;
+
+	hdev = hci_dev_get(chan->ctrl_id);
+	if (!hdev)
+		return;
+
+	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, chan->conn->dst);
+	if (!hcon)
+		goto done;
+
+	cp.phy_handle = hcon->phy_link->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 (chan->conn->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 cdb32da..8acef1b 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1704,6 +1704,14 @@ static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 	hci_callback_process(hdev, HCI_OP_CREATE_PHY_LINK, status);
 }
 
+static void hci_cs_create_logical_link(struct hci_dev *hdev, u8 status)
+{
+	struct hci_cp_create_logical_link *cp;
+
+	BT_DBG("%s status 0x%x", hdev->name, status);
+
+}
+
 static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	__u8 status = *((__u8 *) skb->data);
@@ -2521,6 +2529,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_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 d99eaf9..84bb4f8 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -39,6 +39,7 @@
 #include <net/bluetooth/smp.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/pal.h>
+#include <net/bluetooth/amp.h>
 
 bool disable_ertm;
 
@@ -1018,6 +1019,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;
@@ -3800,6 +3807,8 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 			}
 
 			/* check compatibility */
+			if (l2cap_check_efs(chan))
+				amp_create_logical_link(chan);
 
 			clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state);
 			set_bit(CONF_OUTPUT_DONE, &chan->conf_state);
-- 
1.7.9.5

[PATCHv2 00/22] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (21):
  Bluetooth: debug: Print refcnt for hci_dev
  Bluetooth: trivial: Remove empty line
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct definitions
  Bluetooth: Add phylink lookup helper function
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Use phylink in create/disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: Close A2MP chan when deleting corr L2CAP chan
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Process physical link complete event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   23 +++
 include/net/bluetooth/amp.h      |   29 +++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   17 ++
 include/net/bluetooth/l2cap.h    |    2 +
 include/net/bluetooth/pal.h      |   58 ++++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  421 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  156 ++++++++++++++
 net/bluetooth/hci_core.c         |    1 -
 net/bluetooth/hci_event.c        |  156 +++++++++++++-
 net/bluetooth/l2cap_core.c       |   44 +++-
 net/bluetooth/pal.c              |  330 ++++++++++++++++++++++++++++++
 14 files changed, 1241 insertions(+), 38 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[PATCHv2 01/22] Bluetooth: debug: Print refcnt for hci_dev

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1bbc109..5354e7b 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -612,11 +612,17 @@ static inline void hci_conn_put(struct hci_conn *conn)
 /* ----- HCI Devices ----- */
 static inline void hci_dev_put(struct hci_dev *d)
 {
+	BT_DBG("%s orig refcnt %d", d->name,
+	       atomic_read(&d->dev.kobj.kref.refcount));
+
 	put_device(&d->dev);
 }
 
 static inline struct hci_dev *hci_dev_hold(struct hci_dev *d)
 {
+	BT_DBG("%s orig refcnt %d", d->name,
+	       atomic_read(&d->dev.kobj.kref.refcount));
+
 	get_device(&d->dev);
 	return d;
 }
-- 
1.7.9.5

[PATCHv2 02/22] Bluetooth: trivial: Remove empty line

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/hci_core.c |    1 -
 1 file changed, 1 deletion(-)

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 28bab9d..42c00d18 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -268,7 +268,6 @@ static void hci_init_req(struct hci_dev *hdev, unsigned long opt)
 		BT_ERR("Unknown device type %d", hdev->dev_type);
 		break;
 	}
-
 }
 
 static void hci_le_init_req(struct hci_dev *hdev, unsigned long opt)
-- 
1.7.9.5

[PATCHv2 03/22] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv2 04/22] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    8 ++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..e56d656 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,11 +20,15 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum {
+		READ_LOC_AMP_INFO,
+	} state;
 	unsigned long		flags;
 };
 
@@ -118,9 +122,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 44ef201..81d72be 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -518,6 +522,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -552,6 +560,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
@@ -572,3 +584,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

[PATCHv2 05/22] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e56d656..c21268a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -130,5 +130,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 81d72be..0a46bd6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -601,3 +599,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 1f49957..e95e1e5 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv2 06/22] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 +++++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   48 +++++++++++++++++++++++++++++++++-----
 net/bluetooth/amp.c              |   45 +++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++++++
 8 files changed, 162 insertions(+), 7 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c21268a..f9010c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	__u8			handle;
 	enum {
 		READ_LOC_AMP_INFO,
+		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
 };
@@ -132,5 +133,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 5354e7b..0676d90 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0a46bd6..a44a569 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -232,15 +233,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
@@ -626,3 +628,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index e95e1e5..85dad7f 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2301,6 +2338,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv2 07/22] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a44a569..386dd9b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -381,8 +438,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv2 08/22] Bluetooth: AMP: Physical link struct definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structure. Physical links are managed by AMP
manager inside amp_mgr structure and represent AMP physical links.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 +
 include/net/bluetooth/pal.h  |   42 +++++++++++++
 net/bluetooth/Makefile       |    2 +-
 net/bluetooth/a2mp.c         |    6 ++
 net/bluetooth/pal.c          |  141 ++++++++++++++++++++++++++++++++++++++++++
 5 files changed, 193 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f9010c0..f53c1b1 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,9 @@ struct amp_mgr {
 		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
+
+	struct list_head	phy_links;
+	struct mutex		phy_links_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..201c501
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct phy_link {
+	struct list_head	list;
+	__u8			local_id;
+	__u8			remote_id;
+	__u8			state;
+	__u8			amp_role;
+	__u8			handle;
+	struct amp_mgr		*mgr;
+	struct amp_assoc	rem_assoc;
+	struct kref		kref;
+};
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size);
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+int phylink_put(struct phy_link *plink);
+void phylink_get(struct phy_link *plink);
+void phylink_list_flush(struct amp_mgr *mgr);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 386dd9b..3ceabc9 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -586,6 +587,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	phylink_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -624,6 +626,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	/* Phylink initialization */
+	INIT_LIST_HEAD(&mgr->phy_links);
+	mutex_init(&mgr->phy_links_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..24fb3aa
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,141 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+enum pal_states {
+	DISCONNECTED,
+	STARTING,
+	CONNECTING,
+	AUTHENTICATING,
+	CONNECTED,
+	DISCONNECTING
+};
+
+/* Physical Link interface */
+void phylink_get(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	kref_get(&plink->kref);
+}
+
+static void phylink_destroy(struct kref *kref)
+{
+	struct phy_link *plink = container_of(kref, struct phy_link, kref);
+
+	BT_DBG("plink %p", plink);
+
+	kfree(plink);
+}
+
+int phylink_put(struct phy_link *plink)
+{
+	BT_DBG("plink %p orig refcnt %d", plink,
+	       atomic_read(&plink->kref.refcount));
+
+	return kref_put(&plink->kref, &phylink_destroy);
+}
+
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
+			     u8 *rem_assoc, u16 assoc_size)
+{
+	struct phy_link *plink;
+
+	plink = kzalloc(sizeof(*plink), GFP_KERNEL);
+	if (!plink)
+		return NULL;
+
+	plink->local_id = local_id;
+	plink->remote_id = remote_id;
+	plink->mgr = mgr;
+	plink->handle = __next_handle(mgr);
+
+	plink->rem_assoc.len = min_t(u16, assoc_size, HCI_MAX_AMP_ASSOC_SIZE);
+	memcpy(plink->rem_assoc.data, rem_assoc, plink->rem_assoc.len);
+
+	plink->state = STARTING;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_add(&plink->list, &mgr->phy_links);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	kref_init(&plink->kref);
+
+	BT_DBG("Physical link %p created", plink);
+
+	return plink;
+}
+
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink)
+{
+	BT_DBG("phylink %p", plink);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_del(&plink->list);
+	mutex_unlock(&mgr->phy_links_lock);
+
+	phylink_put(plink);
+}
+
+void phylink_list_flush(struct amp_mgr *mgr)
+{
+	struct phy_link *plink, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry_safe(plink, n, &mgr->phy_links, list) {
+		list_del(&plink->list);
+		phylink_put(plink);
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+}
+
+struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
+{
+	struct phy_link *plink, *found = NULL;
+
+	mutex_lock(&mgr->phy_links_lock);
+	list_for_each_entry(plink, &mgr->phy_links, list) {
+		/* Closest match */
+		if (!remote_id && plink->local_id == local_id) {
+			found = plink;
+			break;
+		}
+		/* Exact match */
+		if (plink->local_id == local_id &&
+		    plink->remote_id == remote_id) {
+			found = plink;
+			break;
+		}
+	}
+	mutex_unlock(&mgr->phy_links_lock);
+
+	BT_DBG("local_id %d remote_id %d plink %p", local_id, remote_id,
+	       plink);
+
+	if (found)
+		phylink_get(plink);
+
+	return found;
+}
-- 
1.7.9.5

[PATCHv2 09/22] Bluetooth: Add phylink lookup helper function

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/pal.c         |   28 ++++++++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 201c501..31216cf 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -35,6 +35,7 @@ struct phy_link {
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
+struct phy_link *phylink_lookup_by_handle(u8 handle);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 24fb3aa..e781a20 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -139,3 +139,31 @@ struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id)
 
 	return found;
 }
+
+/* Returns phy_link referenced if found */
+struct phy_link *phylink_lookup_by_handle(u8 handle)
+{
+	struct amp_mgr *mgr;
+	struct phy_link *found = NULL;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		struct phy_link *plink;
+
+		mutex_lock(&mgr->phy_links_lock);
+		list_for_each_entry(plink, &mgr->phy_links, list) {
+			if (plink->handle == handle) {
+				phylink_get(plink);
+				found = plink;
+				break;
+			}
+		}
+		mutex_unlock(&mgr->phy_links_lock);
+
+		if (found)
+			break;
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return found;
+}
-- 
1.7.9.5

[PATCHv2 10/22] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f53c1b1..1b98c63 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -34,6 +34,9 @@ struct amp_mgr {
 
 	struct list_head	phy_links;
 	struct mutex		phy_links_lock;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 31216cf..f99994a 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -32,6 +32,20 @@ struct phy_link {
 	struct kref		kref;
 };
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct phy_link *phylink_add(struct amp_mgr *mgr, u8 local_id, u8 remote_id,
 			     u8 *rem_assoc, u16 assoc_size);
 struct phy_link *phylink_lookup(struct amp_mgr *mgr, u8 local_id, u8 remote_id);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3ceabc9..40147e6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -587,6 +587,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	phylink_list_flush(mgr);
 	kfree(mgr);
 }
@@ -630,6 +631,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	INIT_LIST_HEAD(&mgr->phy_links);
 	mutex_init(&mgr->phy_links_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index e781a20..3fa785d 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -22,6 +22,87 @@ enum pal_states {
 	DISCONNECTING
 };
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 void phylink_get(struct phy_link *plink)
 {
-- 
1.7.9.5

[PATCHv2 11/22] Bluetooth: AMP: Use phylink in create/disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use phy_link structure to keep track about physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/a2mp.c        |   19 +++++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index f99994a..4a5eeed 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -53,5 +53,6 @@ struct phy_link *phylink_lookup_by_handle(u8 handle);
 int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
+void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 40147e6..d1f6864 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -312,6 +312,7 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -329,6 +330,11 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
+	plink = phylink_add(mgr, rsp.local_id, rsp.remote_id, req->amp_assoc,
+			    le16_to_cpu(hdr->len) - sizeof(*req));
+
+	BT_DBG("plink %p", plink);
+
 	rsp.status = A2MP_STATUS_SUCCESS;
 
 send_rsp:
@@ -348,6 +354,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct phy_link *plink;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -364,8 +371,20 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
+	plink = phylink_lookup(mgr, rsp.local_id, rsp.remote_id);
+	if (!plink) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+	phylink_put(plink);
+
+	phylink_del(mgr, plink);
+
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv2 12/22] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d1f6864..7d6f015 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -272,6 +272,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -462,8 +491,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv2 13/22] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   59 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 58 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7d6f015..5cf7a78 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -334,6 +334,60 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct phy_link *plink;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
+			    ctrl->assoc_len);
+
+	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -495,8 +549,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv2 14/22] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 1b98c63..3d7ee3b 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -23,6 +23,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -138,6 +139,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 4e188dc..0952311 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -769,6 +769,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5cf7a78..d284d62 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -630,7 +630,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -667,7 +667,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -707,7 +710,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -720,7 +723,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -753,7 +756,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -841,3 +844,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 7ec0f042..45c7c82 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -517,7 +517,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -1006,6 +1006,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1032,6 +1044,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1046,8 +1068,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1143,7 +1166,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5527,7 +5550,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_choose_conn(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv2 15/22] Bluetooth: Close A2MP chan when deleting corr L2CAP chan

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When we delete L2CAP channel (e.g. when closing socket) we shall
also delete corresponding A2MP channel.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/l2cap_core.c |    9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 45c7c82..b6d901f 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -589,6 +589,7 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 	BT_DBG("chan %p, conn %p, err %d", chan, conn, err);
 
 	if (conn) {
+		struct amp_mgr *mgr = conn->hcon->amp_mgr;
 		/* Delete from channel list */
 		list_del(&chan->list);
 
@@ -596,6 +597,14 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
 		chan->conn = NULL;
 		hci_conn_put(conn->hcon);
+
+		/* BREDR chan corresponding to A2MP chan */
+		if (mgr && chan == mgr->bredr_chan) {
+			BT_DBG("Remove amp_mgr %p", mgr);
+			l2cap_chan_close(mgr->a2mp_chan, 0);
+			amp_mgr_put(mgr);
+			conn->hcon->amp_mgr = NULL;
+		}
 	}
 
 	if (chan->ops->teardown)
-- 
1.7.9.5

[PATCHv2 16/22] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 3fa785d..6087295 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 enum pal_states {
 	DISCONNECTED,
@@ -248,3 +249,37 @@ struct phy_link *phylink_lookup_by_handle(u8 handle)
 
 	return found;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[PATCHv2 17/22] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 4a5eeed..ee9a489 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -54,5 +54,6 @@ int phylink_put(struct phy_link *plink);
 void phylink_get(struct phy_link *plink);
 void phylink_list_flush(struct amp_mgr *mgr);
 void phylink_del(struct amp_mgr *mgr, struct phy_link *plink);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 6087295..f8bb66c 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -283,3 +283,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[PATCHv2 18/22] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   16 ++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 5 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b376cc3 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index ee9a489..55f4d5b 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct phy_link {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d284d62..7355b6f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -379,9 +379,13 @@ static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	plink = phylink_add(mgr, hdev->id, rsp->id, ctrl->assoc,
 			    ctrl->assoc_len);
+	if (!plink)
+		goto done;
 
 	BT_DBG("Created plink %p: loc:%d -> rem:%d", plink, hdev->id, rsp->id);
 
+	amp_create_phylink(hdev, mgr, plink);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..71af56c 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -43,3 +43,19 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct phy_link *plink)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = plink->handle;
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 85dad7f..f151ed2 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1686,6 +1686,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2495,6 +2500,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv2 19/22] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   76 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 +++++++++++++++
 4 files changed, 109 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b376cc3..fdf9d03 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -21,5 +21,7 @@ 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink);
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 0676d90..28b2f98 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 71af56c..7839699 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,82 @@ 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);
 }
 
+static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+				     struct phy_link *plink)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
+	if (!ctrl)
+		return;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return;
+	}
+
+	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       plink, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = plink->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct phy_link *plink;
+
+	BT_DBG("handle 0x%2.2x", handle);
+
+	plink = phylink_lookup_by_handle(handle);
+	if (!plink)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, plink->mgr, plink);
+
+	phylink_put(plink);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct phy_link *plink;
+
+	BT_DBG("hdev %s", hdev->name);
+
+	/* Write Remote AMP Assoc */
+	plink = phylink_lookup_by_handle(handle);
+	if (plink) {
+		amp_write_rem_assoc_frag(hdev, plink->mgr, plink);
+		phylink_put(plink);
+	}
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index f151ed2..71833fd 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1688,7 +1702,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_DISCONNECT);
+	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)
@@ -2419,6 +2444,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv2 20/22] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 0952311..3262dc7 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -775,5 +775,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7355b6f..ed458d1 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b6d901f..e1007af 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1027,7 +1027,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv2 21/22] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h |    3 +++
 include/net/bluetooth/amp.h  |    2 ++
 net/bluetooth/a2mp.c         |   42 +++++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c          |   27 +++++++++++++++++++++++----
 net/bluetooth/hci_event.c    |   24 ++++++++++++++++++++++++
 5 files changed, 93 insertions(+), 5 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 3d7ee3b..02c9ec9 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -30,6 +30,7 @@ struct amp_mgr {
 	enum {
 		READ_LOC_AMP_INFO,
 		READ_LOC_AMP_ASSOC,
+		READ_LOC_AMP_ASSOC_FINAL,
 	} state;
 	unsigned long		flags;
 
@@ -135,6 +136,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);
@@ -142,5 +144,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 fdf9d03..cf64ba4 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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 phy_link *plink);
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct phy_link *plink);
 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ed458d1..ad5d10c 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -68,7 +68,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;
@@ -877,6 +877,46 @@ 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 phy_link *plink;
+	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;
+	}
+
+	plink = phylink_lookup(mgr, hdev->id, 0);
+	if (!plink)
+		goto clean;
+
+	req->local_id = plink->local_id;
+	req->remote_id = plink->remote_id;
+	memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+	phylink_put(plink);
+
+	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 7839699..3bb79e8 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,7 +44,24 @@ 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);
 }
 
-static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
+void amp_read_loc_assoc_final_data(struct hci_dev *hdev,
+				   struct phy_link *plink)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_mgr *mgr = plink->mgr;
+
+	cp.phy_handle = plink->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, struct amp_mgr *mgr,
 				     struct phy_link *plink)
 {
 	struct hci_cp_write_remote_amp_assoc *cp;
@@ -53,7 +70,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	ctrl = amp_ctrl_lookup(mgr, plink->remote_id);
 	if (!ctrl)
-		return;
+		return false;
 
 	if (!ctrl->assoc_rem_len) {
 		BT_DBG("all fragments are written");
@@ -61,7 +78,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 		ctrl->assoc_len_so_far = 0;
 
 		amp_ctrl_put(ctrl);
-		return;
+		return true;
 	}
 
 	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
@@ -70,7 +87,7 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 	cp = kzalloc(len, GFP_KERNEL);
 	if (!cp) {
 		amp_ctrl_put(ctrl);
-		return;
+		return false;
 	}
 
 	BT_DBG("plink %p ctrl %p frag_len %u assoc_len %u rem_len %u",
@@ -89,6 +106,8 @@ static void amp_write_rem_assoc_frag(struct hci_dev *hdev, struct amp_mgr *mgr,
 	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
 
 	kfree(cp);
+
+	return false;
 }
 
 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 71833fd..029ec04 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,
@@ -3565,6 +3566,25 @@ 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;
+	__u8 phy_handle = ev->phy_handle;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x", hdev->name, phy_handle);
+
+	skb_pull(skb, sizeof(*ev));
+
+	plink = phylink_lookup_by_handle(phy_handle);
+	if (!plink)
+		return;
+
+	amp_read_loc_assoc_final_data(hdev, plink);
+
+	phylink_put(plink);
+}
+
 void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_event_hdr *hdr = (void *) skb->data;
@@ -3721,6 +3741,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;
-- 
1.7.9.5

[PATCHv2 22/22] Bluetooth: AMP: Process physical link complete event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new hci_conn for representing AMP physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/hci_event.c        |   47 ++++++++++++++++++++++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 28b2f98..09ec48d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -339,6 +339,7 @@ struct hci_conn {
 	struct amp_mgr	*amp_mgr;
 
 	struct hci_conn	*link;
+	struct phy_link	*phy_link;
 
 	void (*connect_cfm_cb)	(struct hci_conn *conn, u8 status);
 	void (*security_cfm_cb)	(struct hci_conn *conn, u8 status);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 029ec04..5d75d47 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3427,6 +3427,49 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+				      struct sk_buff *skb)
+{
+	struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+	struct hci_conn *conn;
+	struct phy_link *plink;
+
+	BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+	       ev->status);
+
+	if (ev->status)
+		return;
+
+	plink = phylink_lookup_by_handle(ev->phy_handle);
+	if (!plink)
+		return;
+
+	hci_dev_lock(hdev);
+
+	conn = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (conn) {
+		conn->handle = ev->phy_handle;
+		conn->state = BT_CONNECTED;
+
+		hci_conn_hold(conn);
+		conn->disc_timeout = HCI_DISCONN_TIMEOUT/2;
+		hci_conn_put(conn);
+
+		bacpy(&conn->dst, &plink->mgr->l2cap_conn->hcon->dst);
+		conn->out = true;
+		conn->phy_link = plink;
+
+		hci_conn_hold_device(conn);
+		hci_conn_add_sysfs(conn);
+	} else {
+		BT_ERR("Cannot add connection");
+	}
+
+	hci_dev_unlock(hdev);
+
+	phylink_put(plink);
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3749,6 +3792,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_remote_oob_data_request_evt(hdev, skb);
 		break;
 
+	case HCI_EV_PHY_LINK_COMPLETE:
+		hci_phy_link_complete_evt(hdev, skb);
+		break;
+
 	case HCI_EV_NUM_COMP_BLOCKS:
 		hci_num_comp_blocks_evt(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv3 00/19] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
	struct phy_link, refactoring.
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (18):
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct and heplers
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Handle create / disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Send Create Chan Req
  Bluetooth: AMP: Process physical link complete event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   20 ++
 include/net/bluetooth/amp.h      |   29 +++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   35 ++++
 include/net/bluetooth/l2cap.h    |    2 +
 include/net/bluetooth/pal.h      |   40 ++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  414 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  159 +++++++++++++++
 net/bluetooth/hci_event.c        |  147 +++++++++++++-
 net/bluetooth/l2cap_core.c       |   57 +++++-
 net/bluetooth/pal.c              |  202 +++++++++++++++++++
 13 files changed, 1108 insertions(+), 39 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[PATCHv3 01/19] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv3 02/19] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    8 ++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..e56d656 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,11 +20,15 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum {
+		READ_LOC_AMP_INFO,
+	} state;
 	unsigned long		flags;
 };
 
@@ -118,9 +122,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1f..6b7a4b4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -550,6 +558,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
@@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

[PATCHv3 03/19] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e56d656..c21268a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -130,5 +130,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6b7a4b4..7b98250 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -599,3 +597,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 1f49957..e95e1e5 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv3 04/19] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 +++++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   48 +++++++++++++++++++++++++++++++++-----
 net/bluetooth/amp.c              |   45 +++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++++++
 8 files changed, 162 insertions(+), 7 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c21268a..f9010c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	__u8			handle;
 	enum {
 		READ_LOC_AMP_INFO,
+		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
 };
@@ -132,5 +133,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a3337e..1174218 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7b98250..27ebf31 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -232,15 +233,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
@@ -624,3 +626,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index e95e1e5..85dad7f 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2301,6 +2338,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv3 05/19] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 27ebf31..c876997 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -381,8 +438,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv3 06/19] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structures. Physical links are represented by
hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
we use AMP_LINK.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |   20 +++++++++++++++++++
 include/net/bluetooth/pal.h      |   26 ++++++++++++++++++++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |    1 +
 net/bluetooth/pal.c              |   41 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 90 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1174218..4ae5293 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -316,6 +316,7 @@ struct hci_conn {
 
 	__u8		remote_cap;
 	__u8		remote_auth;
+	__u8		remote_id;
 	bool		flush_key;
 
 	unsigned int	sent;
@@ -510,6 +511,25 @@ static inline struct hci_conn *hci_conn_hash_lookup_handle(struct hci_dev *hdev,
 	return NULL;
 }
 
+static inline struct hci_conn *hci_conn_hash_lookup_id(struct hci_dev *hdev,
+						       __u8 remote_id)
+{
+	struct hci_conn_hash *h = &hdev->conn_hash;
+	struct hci_conn  *c;
+
+	rcu_read_lock();
+
+	list_for_each_entry_rcu(c, &h->list, list) {
+		if (c->remote_id == remote_id) {
+			rcu_read_unlock();
+			return c;
+		}
+	}
+	rcu_read_unlock();
+
+	return NULL;
+}
+
 static inline struct hci_conn *hci_conn_hash_lookup_ba(struct hci_dev *hdev,
 							__u8 type, bdaddr_t *ba)
 {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..a0f441b
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,26 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c876997..8f236db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..c71ef16
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,41 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id)
+{
+	struct hci_conn *hcon;
+
+	hcon = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (!hcon)
+		return NULL;
+
+	hcon->state = BT_CONNECT;
+	hcon->out = true;
+	hcon->attempt++;
+	hcon->handle = __next_handle(mgr);
+	hcon->remote_id = remote_id;
+
+	return hcon;
+}
-- 
1.7.9.5

[PATCHv3 07/19] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f9010c0..93967f1 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,9 @@ struct amp_mgr {
 		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index a0f441b..918a4be 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -20,6 +20,20 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 8f236db..36bf0f4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -585,6 +585,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -623,6 +624,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index c71ef16..cb1fa86 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -13,6 +13,87 @@
 
 #include <net/bluetooth/pal.h>
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 static u8 __next_handle(struct amp_mgr *mgr)
 {
-- 
1.7.9.5

[PATCHv3 08/19] Bluetooth: AMP: Handle create / disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use hci_conn structure to keep track about AMP physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 36bf0f4..8c6cda4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -312,6 +312,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;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -329,7 +330,14 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
-	rsp.status = A2MP_STATUS_SUCCESS;
+	hcon = phylink_add(hdev, mgr, req->local_id);
+	if (hcon) {
+		BT_DBG("hcon %p", hcon);
+
+		rsp.status = A2MP_STATUS_SUCCESS;
+	} else {
+		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+	}
 
 send_rsp:
 	if (hdev)
@@ -348,6 +356,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct hci_conn *hcon;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -358,14 +367,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	rsp.remote_id = req->local_id;
 	rsp.status = A2MP_STATUS_SUCCESS;
 
-	hdev = hci_dev_get(req->local_id);
+	hdev = hci_dev_get(req->remote_id);
 	if (!hdev) {
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 		goto send_rsp;
 	}
 
+	hcon = hci_conn_hash_lookup_id(hdev, rsp.local_id);
+	if (!hcon) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv3 09/19] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 8c6cda4..e012855 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -272,6 +272,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -460,8 +489,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv3 10/19] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   60 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 59 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e012855..42bce4a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -334,6 +334,61 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct hci_conn *hcon;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	hcon = phylink_add(hdev, mgr, rsp->id);
+	if (!hcon)
+		goto done;
+
+	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -493,8 +548,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv3 11/19] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 93967f1..6e88a80 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -23,6 +23,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -135,6 +136,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 0330894..3e2d4e6 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -768,6 +768,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 42bce4a..c550589 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -629,7 +629,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -664,7 +664,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -703,7 +706,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -716,7 +719,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -745,7 +748,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -833,3 +836,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b47c325..b33ce3c 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -517,7 +517,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -1008,6 +1008,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1034,6 +1046,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -1048,8 +1070,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1145,7 +1168,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5497,7 +5520,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_choose_conn(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv3 12/19] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index cb1fa86..2526891 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 /* Remote AMP Controllers handling */
 static void amp_ctrl_get(struct amp_ctrl *ctrl)
@@ -120,3 +121,37 @@ struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	return hcon;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[PATCHv3 13/19] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 918a4be..3b6213c 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -36,5 +36,6 @@ struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id);
 void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 2526891..ea3eefa 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -155,3 +155,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[PATCHv3 14/19] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    2 ++
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 5 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b6c08ce 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3b6213c..35f1765 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct amp_ctrl {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c550589..39e0f95 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -383,6 +383,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);
 
+	amp_create_phylink(hdev, mgr, hcon);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..9676393 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -43,3 +43,22 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 85dad7f..f151ed2 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1686,6 +1686,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2495,6 +2500,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv3 15/19] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 ++++++++++++++
 4 files changed, 113 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b6c08ce..74fcf98 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -21,5 +21,7 @@ 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_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);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4ae5293..dc7a32e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 9676393..6f03fb3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,86 @@ 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);
 }
 
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+				     struct hci_conn *hcon)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+	if (!ctrl)
+		return false;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return true;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return false;
+	}
+
+	BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = hcon->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+
+	return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct hci_conn *hcon)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index f151ed2..8cfaf19 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1688,7 +1702,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_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)
@@ -2419,6 +2444,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv3 16/19] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 3e2d4e6..161be83 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -774,5 +774,6 @@ void l2cap_conn_set_timer(struct l2cap_conn *conn, struct delayed_work *work,
 			  long timeout);
 bool l2cap_conn_clear_timer(struct l2cap_conn *conn,
 			    struct delayed_work *work);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 39e0f95..05522ea 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b33ce3c..a7a62f6 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1020,7 +1020,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv3 17/19] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h |    3 +++
 include/net/bluetooth/amp.h  |    2 ++
 net/bluetooth/a2mp.c         |   39 ++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c          |   15 +++++++++++++++
 net/bluetooth/hci_event.c    |   21 +++++++++++++++++++++
 5 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6e88a80..c955f1f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -30,6 +30,7 @@ struct amp_mgr {
 	enum {
 		READ_LOC_AMP_INFO,
 		READ_LOC_AMP_ASSOC,
+		READ_LOC_AMP_ASSOC_FINAL,
 	} state;
 	unsigned long		flags;
 
@@ -132,6 +133,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);
@@ -139,5 +141,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 74fcf98..70d33d4 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 05522ea..c9a3bc8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -68,7 +68,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;
@@ -867,6 +867,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 hci_conn *hcon;
+	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;
+	}
+
+	hcon = hci_conn_hash_lookup_id(hdev, hdev->id);
+	if (!hcon)
+		goto clean;
+
+	req->local_id = hdev->id;
+	req->remote_id = hcon->remote_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 6f03fb3..9e2e639 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,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 8cfaf19..0841b25 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,
@@ -3565,6 +3566,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;
@@ -3721,6 +3738,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;
-- 
1.7.9.5

[PATCHv3 18/19] Bluetooth: AMP: Send Create Chan Req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Send L2CAP Create Channel Request when receiving HCI Physical
Link Complete event.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci_core.h |    5 +++++
 net/bluetooth/l2cap_core.c       |   22 ++++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index dc7a32e..81fd19c 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -367,6 +367,7 @@ extern void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason);
 extern int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt);
 extern int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb,
 			      u16 flags);
+extern void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 status);
 
 extern int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr);
 extern void sco_connect_cfm(struct hci_conn *hcon, __u8 status);
@@ -787,6 +788,10 @@ static inline void hci_proto_connect_cfm(struct hci_conn *conn, __u8 status)
 		sco_connect_cfm(conn, status);
 		break;
 
+	case AMP_LINK:
+		l2cap_chan_create_cfm(conn, status);
+		break;
+
 	default:
 		BT_ERR("unknown link type %d", conn->type);
 		break;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a7a62f6..6487705 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/pal.h>
 
 bool disable_ertm;
 
@@ -1035,6 +1036,21 @@ void l2cap_send_conn_req(struct l2cap_chan *chan)
 	l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req);
 }
 
+static void l2cap_send_chan_create_req(struct l2cap_chan *chan, u8 remote_id)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct l2cap_create_chan_req req;
+
+	req.scid = cpu_to_le16(chan->scid);
+	req.psm = chan->psm;
+	req.amp_id = remote_id;
+
+	chan->ident = l2cap_get_ident(conn);
+
+	l2cap_send_cmd(conn, chan->ident, L2CAP_CREATE_CHAN_REQ,
+		       sizeof(req), &req);
+}
+
 static void l2cap_chan_ready(struct l2cap_chan *chan)
 {
 	/* This clears all conf flags, including CONF_NOT_COMPLETE */
@@ -5418,7 +5434,13 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 
 		l2cap_conn_put(conn);
 	}
+}
+
+void l2cap_chan_create_cfm(struct hci_conn *hcon, u8 remote_id)
+{
+	struct amp_mgr *mgr = hcon->amp_mgr;
 
+	l2cap_send_chan_create_req(mgr->bredr_chan, remote_id);
 }
 
 int l2cap_disconn_ind(struct hci_conn *hcon)
-- 
1.7.9.5

[PATCHv3 19/19] Bluetooth: AMP: Process physical link complete event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Add new hci_conn for representing AMP physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/hci_event.c |   41 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 41 insertions(+)

diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0841b25..3630044 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3427,6 +3427,43 @@ unlock:
 	hci_dev_unlock(hdev);
 }
 
+static void hci_phy_link_complete_evt(struct hci_dev *hdev,
+				      struct sk_buff *skb)
+{
+	struct hci_ev_phy_link_complete *ev = (void *) skb->data;
+	struct hci_conn *hcon, *bredr_hcon;
+	struct hci_dev *bredr_hdev;
+
+	BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle,
+	       ev->status);
+
+	if (ev->status)
+		return;
+
+	hci_dev_lock(hdev);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle);
+	if (!hcon)
+		return;
+
+	bredr_hcon = hcon->amp_mgr->l2cap_conn->hcon;
+
+	hcon->state = BT_CONNECTED;
+	bacpy(&hcon->dst, &bredr_hcon->dst);
+
+	hci_conn_hold_device(hcon);
+	hci_conn_add_sysfs(hcon);
+
+	hci_dev_unlock(hdev);
+
+	bredr_hdev = bredr_hcon->hdev;
+	if (bredr_hdev) {
+		hci_dev_hold(bredr_hdev);
+		l2cap_chan_create_cfm(bredr_hcon, hcon->remote_id);
+		hci_dev_put(bredr_hdev);
+	}
+}
+
 static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct hci_ev_le_conn_complete *ev = (void *) skb->data;
@@ -3746,6 +3783,10 @@ void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_remote_oob_data_request_evt(hdev, skb);
 		break;
 
+	case HCI_EV_PHY_LINK_COMPLETE:
+		hci_phy_link_complete_evt(hdev, skb);
+		break;
+
 	case HCI_EV_NUM_COMP_BLOCKS:
 		hci_num_comp_blocks_evt(hdev, skb);
 		break;
-- 
1.7.9.5

Re: [PATCHv3 00/19] Bluetooth: Create AMP physical link

[Mat Martineau]

Andrei -

On Thu, 6 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> This set of patches enhances A2MP protocol and creates physical
> link between AMP controllers. This is further iteration towards
> Bluetooth High Speed.
>
> Changes:
> 	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
> 	struct phy_link, refactoring.
> 	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
> 	* p1: Fixed locking issues, added basic logical link preparation.
> 	* v3: Remove workqueue from callback processing; change callback functions
> 	names according to reviewers recommendations; create global amp_mgr_list to
> 	have lookup to amp manager, physical and logical links so for those HCI events
> 	which might be identified by __handler__ we have lookup; remove extensive
> 	hexdump from gen_amp_key.
> 	* v2: Fix typos and bugs, add functionality: now physical connection
> 	might be established.
> 	* v1: Fix typos, change debug prints, refactor code for better
> 	splitting functionality.
>

This doesn't apply to the current bluetooth-next HEAD, could you 
rebase?

Patch 18 (AMP: Send Create Chan Req) also conflicts with my signaling 
patch set, so it would help if you dropped it for now and added that 
functionality after the signaling code is in.

Thanks,

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

[PATCHv4 00/17] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p4: Rebased against recent bluetooth-next, minor fixes
	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
	struct phy_link, refactoring.
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (16):
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct and heplers
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Handle create / disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   20 ++
 include/net/bluetooth/amp.h      |   29 +++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   30 +++
 include/net/bluetooth/l2cap.h    |    2 +
 include/net/bluetooth/pal.h      |   40 ++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  414 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  159 +++++++++++++++
 net/bluetooth/hci_event.c        |  106 +++++++++-
 net/bluetooth/l2cap_core.c       |   35 +++-
 net/bluetooth/pal.c              |  203 +++++++++++++++++++
 13 files changed, 1041 insertions(+), 39 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[PATCHv4 01/17] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv4 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    8 ++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..e56d656 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,11 +20,15 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum {
+		READ_LOC_AMP_INFO,
+	} state;
 	unsigned long		flags;
 };
 
@@ -118,9 +122,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1f..6b7a4b4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -550,6 +558,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	conn->hcon->amp_mgr = mgr;
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
@@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

Re: [PATCHv4 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Mat Martineau]

Andrei -

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> Create amp_mgr_list global list which will be used by different
> hci devices to find amp_mgr.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/a2mp.h |    8 ++++++++
> net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
> 2 files changed, 37 insertions(+)
>
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 6a76e0a..e56d656 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -20,11 +20,15 @@
> #define A2MP_FEAT_EXT	0x8000
>
> struct amp_mgr {
> +	struct list_head	list;
> 	struct l2cap_conn	*l2cap_conn;
> 	struct l2cap_chan	*a2mp_chan;
> 	struct kref		kref;
> 	__u8			ident;
> 	__u8			handle;
> +	enum {
> +		READ_LOC_AMP_INFO,
> +	} state;
> 	unsigned long		flags;
> };
>
> @@ -118,9 +122,13 @@ struct a2mp_physlink_rsp {
> #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
> #define A2MP_STATUS_SECURITY_VIOLATION		0x06
>
> +extern struct list_head amp_mgr_list;
> +extern struct mutex amp_mgr_list_lock;
> +
> void amp_mgr_get(struct amp_mgr *mgr);
> int amp_mgr_put(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);
>
> #endif /* __A2MP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 0760d1f..6b7a4b4 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -17,6 +17,10 @@
> #include <net/bluetooth/l2cap.h>
> #include <net/bluetooth/a2mp.h>
>
> +/* Global AMP Manager list */
> +LIST_HEAD(amp_mgr_list);
> +DEFINE_MUTEX(amp_mgr_list_lock);
> +
> /* A2MP build & send command helper functions */
> static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
> {
> @@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
>
> 	BT_DBG("mgr %p", mgr);
>
> +	mutex_lock(&amp_mgr_list_lock);
> +	list_del(&mgr->list);
> +	mutex_unlock(&amp_mgr_list_lock);
> +
> 	kfree(mgr);
> }
>
> @@ -550,6 +558,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
>
> 	conn->hcon->amp_mgr = mgr;
>
> +	mutex_lock(&amp_mgr_list_lock);
> +	list_add(&mgr->list, &amp_mgr_list);
> +	mutex_unlock(&amp_mgr_list_lock);
> +
> 	kref_init(&mgr->kref);

Should the reference count be initialized before adding to the list?

>
> 	return mgr;
> @@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
>
> 	return mgr->a2mp_chan;
> }
> +
> +struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
> +{
> +	struct amp_mgr *mgr;
> +
> +	mutex_lock(&amp_mgr_list_lock);
> +	list_for_each_entry(mgr, &amp_mgr_list, list) {
> +		if (mgr->state == state) {
> +			amp_mgr_get(mgr);
> +			mutex_unlock(&amp_mgr_list_lock);
> +			return mgr;
> +		}
> +	}
> +	mutex_unlock(&amp_mgr_list_lock);
> +
> +	return NULL;
> +}
> -- 
> 1.7.9.5

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 08:02:58AM -0700, Mat Martineau wrote:
...

> >@@ -550,6 +558,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
> >
> >	conn->hcon->amp_mgr = mgr;
> >
> >+	mutex_lock(&amp_mgr_list_lock);
> >+	list_add(&mgr->list, &amp_mgr_list);
> >+	mutex_unlock(&amp_mgr_list_lock);
> >+
> >	kref_init(&mgr->kref);
> 
> Should the reference count be initialized before adding to the list?

will change this.

Best regards 
Andrei Emeltchenko 

[PATCHv4 03/17] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e56d656..c21268a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -130,5 +130,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6b7a4b4..7b98250 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -599,3 +597,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 48d7302..5ae5121 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv4 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 +++++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 +++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   48 +++++++++++++++++++++++++++++++++-----
 net/bluetooth/amp.c              |   45 +++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++++++
 8 files changed, 162 insertions(+), 7 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c21268a..f9010c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	__u8			handle;
 	enum {
 		READ_LOC_AMP_INFO,
+		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
 };
@@ -132,5 +133,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a3337e..1174218 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7b98250..27ebf31 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -232,15 +233,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
-	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+		if (hdev)
+			hci_dev_put(hdev);
 
-clean:
-	if (hdev)
-		hci_dev_put(hdev);
+		goto done;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
 
+done:
 	skb_pull(skb, sizeof(*req));
 	return 0;
 }
@@ -624,3 +626,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 5ae5121..6cc44cf 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2302,6 +2339,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

Re: [PATCHv4 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Mat Martineau]

Andrei -

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
> HCI command to AMP controller. If the AMP Assoc data is larger then it
> can fit to HCI event only fragment is read. When all fragments are read
> send A2MP Get AMP Assoc Response.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/a2mp.h     |    2 ++
> include/net/bluetooth/amp.h      |   21 +++++++++++++++++
> include/net/bluetooth/hci.h      |    2 ++
> include/net/bluetooth/hci_core.h |    8 +++++++
> net/bluetooth/Makefile           |    2 +-
> net/bluetooth/a2mp.c             |   48 +++++++++++++++++++++++++++++++++-----
> net/bluetooth/amp.c              |   45 +++++++++++++++++++++++++++++++++++
> net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++++++
> 8 files changed, 162 insertions(+), 7 deletions(-)
> create mode 100644 include/net/bluetooth/amp.h
> create mode 100644 net/bluetooth/amp.c
>
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index c21268a..f9010c0 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -28,6 +28,7 @@ struct amp_mgr {
> 	__u8			handle;
> 	enum {
> 		READ_LOC_AMP_INFO,
> +		READ_LOC_AMP_ASSOC,
> 	} state;
> 	unsigned long		flags;
> };
> @@ -132,5 +133,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
> struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
> void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
> void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
> +void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
>
> #endif /* __A2MP_H */
> diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
> new file mode 100644
> index 0000000..e861675
> --- /dev/null
> +++ b/include/net/bluetooth/amp.h
> @@ -0,0 +1,21 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#ifndef __AMP_H
> +#define __AMP_H
> +
> +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);
> +
> +#endif /* __AMP_H */
> diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> index 42aae18..1cb8b55 100644
> --- a/include/net/bluetooth/hci.h
> +++ b/include/net/bluetooth/hci.h
> @@ -33,6 +33,8 @@
> #define HCI_LINK_KEY_SIZE	16
> #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
>
> +#define HCI_MAX_AMP_ASSOC_SIZE	672
> +
> /* HCI dev events */
> #define HCI_DEV_REG			1
> #define HCI_DEV_UNREG			2
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 6a3337e..1174218 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -124,6 +124,12 @@ struct le_scan_params {
>
> #define HCI_MAX_SHORT_NAME_LENGTH	10
>
> +struct amp_assoc {
> +	__u16	len;
> +	__u16	offset;
> +	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
> +};
> +
> #define NUM_REASSEMBLY 4
> struct hci_dev {
> 	struct list_head list;
> @@ -177,6 +183,8 @@ struct hci_dev {
> 	__u32		amp_max_flush_to;
> 	__u32		amp_be_flush_to;
>
> +	struct amp_assoc	loc_assoc;
> +
> 	__u8		flow_ctl_mode;
>
> 	unsigned int	auto_accept_delay;
> diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
> index fa6d94a..dea6a28 100644
> --- a/net/bluetooth/Makefile
> +++ b/net/bluetooth/Makefile
> @@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
>
> bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
> 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
> -	a2mp.o
> +	a2mp.o amp.o
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 7b98250..27ebf31 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -16,6 +16,7 @@
> #include <net/bluetooth/hci_core.h>
> #include <net/bluetooth/l2cap.h>
> #include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
>
> /* Global AMP Manager list */
> LIST_HEAD(amp_mgr_list);
> @@ -232,15 +233,16 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
>
> 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
> 			  &rsp);
> -		goto clean;
> -	}
>
> -	/* Placeholder for HCI Read AMP Assoc */
> +		if (hdev)
> +			hci_dev_put(hdev);
>
> -clean:
> -	if (hdev)
> -		hci_dev_put(hdev);
> +		goto done;
> +	}
> +
> +	amp_read_loc_assoc(hdev, mgr);
>
> +done:
> 	skb_pull(skb, sizeof(*req));
> 	return 0;
> }
> @@ -624,3 +626,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
> 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
> 	amp_mgr_put(mgr);
> }
> +
> +void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
> +{
> +	struct amp_mgr *mgr;
> +	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
> +	struct a2mp_amp_assoc_rsp *rsp;
> +	size_t len;
> +
> +	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);

What if multiple amp managers are in the READ_LOC_AMP_ASSOC state? 
Is that possible if multiple remote devices send GETAMPASSOC at the 
same time?

> +	if (!mgr)
> +		return;
> +
> +	BT_DBG("%s mgr %p", hdev->name, mgr);
> +
> +	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
> +	rsp = kzalloc(len, GFP_KERNEL);
> +	if (!rsp) {
> +		amp_mgr_put(mgr);
> +		return;
> +	}
> +
> +	rsp->id = hdev->id;
> +
> +	if (status) {
> +		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
> +	} else {
> +		rsp->status = A2MP_STATUS_SUCCESS;
> +		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
> +	}
> +
> +	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
> +	amp_mgr_put(mgr);
> +	kfree(rsp);
> +}
> diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
> new file mode 100644
> index 0000000..2d4e79e
> --- /dev/null
> +++ b/net/bluetooth/amp.c
> @@ -0,0 +1,45 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#include <net/bluetooth/bluetooth.h>
> +#include <net/bluetooth/hci.h>
> +#include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
> +
> +void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
> +{
> +	struct hci_cp_read_local_amp_assoc cp;
> +	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
> +
> +	BT_DBG("%s handle %d", hdev->name, phy_handle);
> +
> +	cp.phy_handle = phy_handle;
> +	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
> +	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
> +
> +	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
> +}
> +
> +void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
> +{
> +	struct hci_cp_read_local_amp_assoc cp;
> +
> +	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
> +	memset(&cp, 0, sizeof(cp));
> +
> +	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
> +
> +	mgr->state = READ_LOC_AMP_ASSOC;
> +	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
> +}
> diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
> index 5ae5121..6cc44cf 100644
> --- a/net/bluetooth/hci_event.c
> +++ b/net/bluetooth/hci_event.c
> @@ -31,6 +31,7 @@
> #include <net/bluetooth/hci_core.h>
> #include <net/bluetooth/mgmt.h>
> #include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
>
> /* Handle HCI Event packets */
>
> @@ -866,6 +867,42 @@ a2mp_rsp:
> 	a2mp_send_getinfo_rsp(hdev);
> }
>
> +static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
> +					struct sk_buff *skb)
> +{
> +	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
> +	struct amp_assoc *assoc = &hdev->loc_assoc;
> +	size_t rem_len, frag_len;
> +
> +	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
> +
> +	if (rp->status)
> +		goto a2mp_rsp;
> +
> +	frag_len = skb->len - sizeof(*rp);
> +	rem_len = __le16_to_cpu(rp->rem_len);
> +
> +	if (rem_len > frag_len) {
> +		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
> +
> +		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
> +		assoc->offset += frag_len;
> +
> +		/* Read other fragments */
> +		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
> +
> +		return;
> +	}
> +
> +	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
> +	assoc->len = assoc->offset + rem_len;
> +	assoc->offset = 0;
> +
> +a2mp_rsp:
> +	/* Send A2MP Rsp when all fragments are received */
> +	a2mp_send_getampassoc_rsp(hdev, rp->status);
> +}
> +
> static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
> 					  struct sk_buff *skb)
> {
> @@ -2302,6 +2339,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
> 		hci_cc_read_local_amp_info(hdev, skb);
> 		break;
>
> +	case HCI_OP_READ_LOCAL_AMP_ASSOC:
> +		hci_cc_read_local_amp_assoc(hdev, skb);
> +		break;
> +
> 	case HCI_OP_DELETE_STORED_LINK_KEY:
> 		hci_cc_delete_stored_link_key(hdev, skb);
> 		break;
> -- 
> 1.7.9.5

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 08:28:06AM -0700, Mat Martineau wrote:
> >When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
> >HCI command to AMP controller. If the AMP Assoc data is larger then it
> >can fit to HCI event only fragment is read. When all fragments are read
> >send A2MP Get AMP Assoc Response.
> >
> >Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

...

> >+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
> >+{
> >+	struct amp_mgr *mgr;
> >+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
> >+	struct a2mp_amp_assoc_rsp *rsp;
> >+	size_t len;
> >+
> >+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
> 
> What if multiple amp managers are in the READ_LOC_AMP_ASSOC state?
> Is that possible if multiple remote devices send GETAMPASSOC at the
> same time?

In this very unrealistic case I still do not see a problem. All those HCI
requests would be serialized and response would be sent for all AMP
managers.

Best regards 
Andrei Emeltchenko 

Re: [PATCHv4 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Mat Martineau]

Hi Andrei -

On Fri, 14 Sep 2012, Andrei Emeltchenko wrote:

> Hi Mat,
>
> On Thu, Sep 13, 2012 at 08:28:06AM -0700, Mat Martineau wrote:
>>> When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
>>> HCI command to AMP controller. If the AMP Assoc data is larger then it
>>> can fit to HCI event only fragment is read. When all fragments are read
>>> send A2MP Get AMP Assoc Response.
>>>
>>> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> ...
>
>>> +void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
>>> +{
>>> +	struct amp_mgr *mgr;
>>> +	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
>>> +	struct a2mp_amp_assoc_rsp *rsp;
>>> +	size_t len;
>>> +
>>> +	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
>>
>> What if multiple amp managers are in the READ_LOC_AMP_ASSOC state?
>> Is that possible if multiple remote devices send GETAMPASSOC at the
>> same time?
>
> In this very unrealistic case I still do not see a problem. All those HCI
> requests would be serialized and response would be sent for all AMP
> managers.

I agree that it won't be a common problem - but with millions of 
instances of the Linux kernel out there, if simultaneous requests 
*can* happen, they *will* happen.

It's mostly the reading of fragments I'm concerned about.  If multiple 
HCI_OP_READ_LOCAL_AMP_ASSOC commands are queued up to send to the AMP 
controller, and any of those involve reading fragments, the 
hdev->loc_assoc would be corrupted or you could get a buffer overrun 
from the memcpy's in hci_cc_read_local_amp_assoc.

An easy fix is to respond with an error status to any GETAMPASSOC 
commands that arrive while an earlier GETAMPASSOC is being processed.

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

[PATCHv4 05/17] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 27ebf31..c876997 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -381,8 +438,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv4 06/17] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structures. Physical links are represented by
hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
we use AMP_LINK.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |   20 ++++++++++++++++++
 include/net/bluetooth/pal.h      |   26 +++++++++++++++++++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |    1 +
 net/bluetooth/pal.c              |   42 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 91 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1174218..4ae5293 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -316,6 +316,7 @@ struct hci_conn {
 
 	__u8		remote_cap;
 	__u8		remote_auth;
+	__u8		remote_id;
 	bool		flush_key;
 
 	unsigned int	sent;
@@ -510,6 +511,25 @@ static inline struct hci_conn *hci_conn_hash_lookup_handle(struct hci_dev *hdev,
 	return NULL;
 }
 
+static inline struct hci_conn *hci_conn_hash_lookup_id(struct hci_dev *hdev,
+						       __u8 remote_id)
+{
+	struct hci_conn_hash *h = &hdev->conn_hash;
+	struct hci_conn  *c;
+
+	rcu_read_lock();
+
+	list_for_each_entry_rcu(c, &h->list, list) {
+		if (c->remote_id == remote_id) {
+			rcu_read_unlock();
+			return c;
+		}
+	}
+	rcu_read_unlock();
+
+	return NULL;
+}
+
 static inline struct hci_conn *hci_conn_hash_lookup_ba(struct hci_dev *hdev,
 							__u8 type, bdaddr_t *ba)
 {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..a0f441b
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,26 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c876997..8f236db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..3377ad1
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,42 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id)
+{
+	struct hci_conn *hcon;
+
+	hcon = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
+	if (!hcon)
+		return NULL;
+
+	hcon->state = BT_CONNECT;
+	hcon->out = true;
+	hcon->attempt++;
+	hcon->handle = __next_handle(mgr);
+	hcon->remote_id = remote_id;
+	hcon->amp_mgr = mgr;
+
+	return hcon;
+}
-- 
1.7.9.5

Re: [PATCHv4 06/17] Bluetooth: AMP: Physical link struct and heplers

[Mat Martineau]

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> Define physical link structures. Physical links are represented by
> hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
> we use AMP_LINK.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/hci.h      |    1 +
> include/net/bluetooth/hci_core.h |   20 ++++++++++++++++++
> include/net/bluetooth/pal.h      |   26 +++++++++++++++++++++++
> net/bluetooth/Makefile           |    2 +-
> net/bluetooth/a2mp.c             |    1 +
> net/bluetooth/pal.c              |   42 ++++++++++++++++++++++++++++++++++++++
> 6 files changed, 91 insertions(+), 1 deletion(-)
> create mode 100644 include/net/bluetooth/pal.h
> create mode 100644 net/bluetooth/pal.c
>
> diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> index 1cb8b55..4c41b8c 100644
> --- a/include/net/bluetooth/hci.h
> +++ b/include/net/bluetooth/hci.h
> @@ -207,6 +207,7 @@ enum {
> #define ESCO_LINK	0x02
> /* Low Energy links do not have defined link type. Use invented one */
> #define LE_LINK		0x80
> +#define AMP_LINK	0x81
>
> /* LMP features */
> #define LMP_3SLOT	0x01
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index 1174218..4ae5293 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -316,6 +316,7 @@ struct hci_conn {
>
> 	__u8		remote_cap;
> 	__u8		remote_auth;
> +	__u8		remote_id;
> 	bool		flush_key;
>
> 	unsigned int	sent;
> @@ -510,6 +511,25 @@ static inline struct hci_conn *hci_conn_hash_lookup_handle(struct hci_dev *hdev,
> 	return NULL;
> }
>
> +static inline struct hci_conn *hci_conn_hash_lookup_id(struct hci_dev *hdev,
> +						       __u8 remote_id)
> +{
> +	struct hci_conn_hash *h = &hdev->conn_hash;
> +	struct hci_conn  *c;
> +
> +	rcu_read_lock();
> +
> +	list_for_each_entry_rcu(c, &h->list, list) {
> +		if (c->remote_id == remote_id) {
> +			rcu_read_unlock();
> +			return c;
> +		}
> +	}
> +	rcu_read_unlock();
> +
> +	return NULL;
> +}
> +

If there are multiple AMP physical links to different devices, the 
remote_id probably isn't enough information to look up a physical 
link.  Most of the time the remote_id will be "1", so there will 
probably be multiple hci_conns with the same remote_id.

l2cap_conn + remote_id should be unique, though.

> static inline struct hci_conn *hci_conn_hash_lookup_ba(struct hci_dev *hdev,
> 							__u8 type, bdaddr_t *ba)
> {
> diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
> new file mode 100644
> index 0000000..a0f441b
> --- /dev/null
> +++ b/include/net/bluetooth/pal.h
> @@ -0,0 +1,26 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#ifndef __PAL_H
> +#define __PAL_H
> +
> +#include <net/bluetooth/bluetooth.h>
> +#include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/l2cap.h>
> +#include <net/bluetooth/a2mp.h>
> +#include <net/bluetooth/amp.h>
> +
> +struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
> +			     u8 remote_id);
> +
> +#endif /* __PAL_H */
> diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
> index dea6a28..3f76fc2 100644
> --- a/net/bluetooth/Makefile
> +++ b/net/bluetooth/Makefile
> @@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
>
> bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
> 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
> -	a2mp.o amp.o
> +	a2mp.o amp.o pal.o
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index c876997..8f236db 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -17,6 +17,7 @@
> #include <net/bluetooth/l2cap.h>
> #include <net/bluetooth/a2mp.h>
> #include <net/bluetooth/amp.h>
> +#include <net/bluetooth/pal.h>
>
> /* Global AMP Manager list */
> LIST_HEAD(amp_mgr_list);
> diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
> new file mode 100644
> index 0000000..3377ad1
> --- /dev/null
> +++ b/net/bluetooth/pal.c
> @@ -0,0 +1,42 @@
> +/*
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#include <net/bluetooth/pal.h>
> +
> +/* Physical Link interface */
> +static u8 __next_handle(struct amp_mgr *mgr)
> +{
> +	if (++mgr->handle == 0)
> +		mgr->handle = 1;
> +
> +	return mgr->handle;
> +}
> +
> +struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
> +			     u8 remote_id)
> +{
> +	struct hci_conn *hcon;
> +
> +	hcon = hci_conn_add(hdev, AMP_LINK, BDADDR_ANY);
> +	if (!hcon)
> +		return NULL;
> +
> +	hcon->state = BT_CONNECT;
> +	hcon->out = true;
> +	hcon->attempt++;
> +	hcon->handle = __next_handle(mgr);
> +	hcon->remote_id = remote_id;
> +	hcon->amp_mgr = mgr;
> +
> +	return hcon;
> +}
> -- 
> 1.7.9.5

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 06/17] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 08:40:59AM -0700, Mat Martineau wrote:
> >From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> >
> >Define physical link structures. Physical links are represented by
> >hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
> >we use AMP_LINK.
> >
> >Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> >---
> >include/net/bluetooth/hci.h      |    1 +
> >include/net/bluetooth/hci_core.h |   20 ++++++++++++++++++
> >include/net/bluetooth/pal.h      |   26 +++++++++++++++++++++++
> >net/bluetooth/Makefile           |    2 +-
> >net/bluetooth/a2mp.c             |    1 +
> >net/bluetooth/pal.c              |   42 ++++++++++++++++++++++++++++++++++++++
> >6 files changed, 91 insertions(+), 1 deletion(-)
> >create mode 100644 include/net/bluetooth/pal.h
> >create mode 100644 net/bluetooth/pal.c
> >
> >diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
> >index 1cb8b55..4c41b8c 100644
> >--- a/include/net/bluetooth/hci.h
> >+++ b/include/net/bluetooth/hci.h
> >@@ -207,6 +207,7 @@ enum {
> >#define ESCO_LINK	0x02
> >/* Low Energy links do not have defined link type. Use invented one */
> >#define LE_LINK		0x80
> >+#define AMP_LINK	0x81
> >
> >/* LMP features */
> >#define LMP_3SLOT	0x01
> >diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> >index 1174218..4ae5293 100644
> >--- a/include/net/bluetooth/hci_core.h
> >+++ b/include/net/bluetooth/hci_core.h
> >@@ -316,6 +316,7 @@ struct hci_conn {
> >
> >	__u8		remote_cap;
> >	__u8		remote_auth;
> >+	__u8		remote_id;
> >	bool		flush_key;
> >
> >	unsigned int	sent;
> >@@ -510,6 +511,25 @@ static inline struct hci_conn *hci_conn_hash_lookup_handle(struct hci_dev *hdev,
> >	return NULL;
> >}
> >
> >+static inline struct hci_conn *hci_conn_hash_lookup_id(struct hci_dev *hdev,
> >+						       __u8 remote_id)
> >+{
> >+	struct hci_conn_hash *h = &hdev->conn_hash;
> >+	struct hci_conn  *c;
> >+
> >+	rcu_read_lock();
> >+
> >+	list_for_each_entry_rcu(c, &h->list, list) {
> >+		if (c->remote_id == remote_id) {
> >+			rcu_read_unlock();
> >+			return c;
> >+		}
> >+	}
> >+	rcu_read_unlock();
> >+
> >+	return NULL;
> >+}
> >+
> 
> If there are multiple AMP physical links to different devices, the
> remote_id probably isn't enough information to look up a physical
> link.  Most of the time the remote_id will be "1", so there will
> probably be multiple hci_conns with the same remote_id.

I am thinking then we might use dst address and use function below.

> 
> l2cap_conn + remote_id should be unique, though.
> 
> >static inline struct hci_conn *hci_conn_hash_lookup_ba(struct hci_dev *hdev,
> >							__u8 type, bdaddr_t *ba)
> >{

like

hci_conn_hash_lookup_ba(hdev, AMP_LINK, dst)


Best regards 
Andrei Emeltchenko 

[PATCHv4 07/17] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f9010c0..93967f1 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,9 @@ struct amp_mgr {
 		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index a0f441b..918a4be 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -20,6 +20,20 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 8f236db..36bf0f4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -585,6 +585,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -623,6 +624,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	kref_init(&mgr->kref);
 
 	return mgr;
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 3377ad1..7548644 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -13,6 +13,87 @@
 
 #include <net/bluetooth/pal.h>
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 static u8 __next_handle(struct amp_mgr *mgr)
 {
-- 
1.7.9.5

Re: [PATCHv4 07/17] Bluetooth: AMP: Remote AMP ctrl definitions

[Mat Martineau]

Andrei -

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> Create remote AMP controllers structure. It is used to keep information
> about discovered remote AMP controllers by A2MP protocol.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/a2mp.h |    3 ++
> include/net/bluetooth/pal.h  |   14 ++++++++
> net/bluetooth/a2mp.c         |    5 +++
> net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
> 4 files changed, 103 insertions(+)
>
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index f9010c0..93967f1 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -31,6 +31,9 @@ struct amp_mgr {
> 		READ_LOC_AMP_ASSOC,
> 	} state;
> 	unsigned long		flags;
> +
> +	struct list_head	amp_ctrls;
> +	struct mutex		amp_ctrls_lock;
> };
>
> struct a2mp_cmd {
> diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
> index a0f441b..918a4be 100644
> --- a/include/net/bluetooth/pal.h
> +++ b/include/net/bluetooth/pal.h
> @@ -20,6 +20,20 @@
> #include <net/bluetooth/a2mp.h>
> #include <net/bluetooth/amp.h>
>
> +struct amp_ctrl {
> +	struct list_head	list;
> +	struct kref		kref;
> +	__u8			id;
> +	__u16			assoc_len_so_far;
> +	__u16			assoc_rem_len;
> +	__u16			assoc_len;
> +	__u8			*assoc;
> +};

I think you have the necessary information here.  There should be no 
need to also track the remote device in the amp_ctrl struct because 
the amp_mgr that owns the list of controllers is only referring to one 
remote device, right?

> +
> +int amp_ctrl_put(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);
> struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
> 			     u8 remote_id);
>
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 8f236db..36bf0f4 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -585,6 +585,7 @@ static void amp_mgr_destroy(struct kref *kref)
> 	list_del(&mgr->list);
> 	mutex_unlock(&amp_mgr_list_lock);
>
> +	amp_ctrl_list_flush(mgr);
> 	kfree(mgr);
> }
>
> @@ -623,6 +624,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
> 	list_add(&mgr->list, &amp_mgr_list);
> 	mutex_unlock(&amp_mgr_list_lock);
>
> +	/* Remote AMP ctrl list initialization */
> +	INIT_LIST_HEAD(&mgr->amp_ctrls);
> +	mutex_init(&mgr->amp_ctrls_lock);
> +
> 	kref_init(&mgr->kref);
>
> 	return mgr;
> diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
> index 3377ad1..7548644 100644
> --- a/net/bluetooth/pal.c
> +++ b/net/bluetooth/pal.c
> @@ -13,6 +13,87 @@
>
> #include <net/bluetooth/pal.h>
>
> +/* Remote AMP Controllers handling */
> +static void amp_ctrl_get(struct amp_ctrl *ctrl)
> +{
> +	BT_DBG("ctrl %p orig refcnt %d", ctrl,
> +	       atomic_read(&ctrl->kref.refcount));
> +
> +	kref_get(&ctrl->kref);
> +}
> +
> +static void amp_ctrl_destroy(struct kref *kref)
> +{
> +	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
> +
> +	BT_DBG("ctrl %p", ctrl);
> +
> +	if (ctrl->assoc)
> +		kfree(ctrl->assoc);
> +
> +	kfree(ctrl);
> +}
> +
> +int amp_ctrl_put(struct amp_ctrl *ctrl)
> +{
> +	BT_DBG("ctrl %p orig refcnt %d", ctrl,
> +	       atomic_read(&ctrl->kref.refcount));
> +
> +	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
> +}
> +
> +struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
> +{
> +	struct amp_ctrl *ctrl;
> +
> +	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
> +	if (!ctrl)
> +		return NULL;
> +
> +	mutex_lock(&mgr->amp_ctrls_lock);
> +	list_add(&ctrl->list, &mgr->amp_ctrls);
> +	mutex_unlock(&mgr->amp_ctrls_lock);
> +
> +	kref_init(&ctrl->kref);
> +
> +	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
> +
> +	return ctrl;
> +}
> +
> +void amp_ctrl_list_flush(struct amp_mgr *mgr)
> +{
> +	struct amp_ctrl *ctrl, *n;
> +
> +	BT_DBG("mgr %p", mgr);
> +
> +	mutex_lock(&mgr->amp_ctrls_lock);
> +	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
> +		list_del(&ctrl->list);
> +		amp_ctrl_put(ctrl);
> +	}
> +	mutex_unlock(&mgr->amp_ctrls_lock);
> +}
> +
> +struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
> +{
> +	struct amp_ctrl *ctrl = NULL;
> +
> +	mutex_lock(&mgr->amp_ctrls_lock);
> +	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
> +		if (ctrl->id == id)
> +			break;
> +	}
> +	mutex_unlock(&mgr->amp_ctrls_lock);
> +
> +	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
> +
> +	if (ctrl)
> +		amp_ctrl_get(ctrl);
> +
> +	return ctrl;
> +}
> +
> /* Physical Link interface */
> static u8 __next_handle(struct amp_mgr *mgr)
> {
> -- 
> 1.7.9.5


--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 07/17] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 08:50:24AM -0700, Mat Martineau wrote:

...

> >+struct amp_ctrl {
> >+	struct list_head	list;
> >+	struct kref		kref;
> >+	__u8			id;
> >+	__u16			assoc_len_so_far;
> >+	__u16			assoc_rem_len;
> >+	__u16			assoc_len;
> >+	__u8			*assoc;
> >+};
> 
> I think you have the necessary information here.  There should be no
> need to also track the remote device in the amp_ctrl struct because
> the amp_mgr that owns the list of controllers is only referring to
> one remote device, right?

amp_mgr tracks the list of remote AMP controllers found in the A2MP
Discover Response. It is not limited to one remote controller.

Though it is usually only one AMP controller but Spec says that:

<------8<-------------------------------------------------------------------------
|  Controller List (3 or more octets)
|  Controller List is variable in length. It consists of 1 or more entry.
|  Each entry is comprised of a Controller ID, a Controller Type and a Controller
|  status.
<------8<-------------------------------------------------------------------------

Then for each controller we send A2MP Getinfo Request.

Best regards 
Andrei Emeltchenko 

[PATCHv4 08/17] Bluetooth: AMP: Handle create / disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use hci_conn structure to keep track about AMP physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 36bf0f4..8c6cda4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -312,6 +312,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;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -329,7 +330,14 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
-	rsp.status = A2MP_STATUS_SUCCESS;
+	hcon = phylink_add(hdev, mgr, req->local_id);
+	if (hcon) {
+		BT_DBG("hcon %p", hcon);
+
+		rsp.status = A2MP_STATUS_SUCCESS;
+	} else {
+		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+	}
 
 send_rsp:
 	if (hdev)
@@ -348,6 +356,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct hci_conn *hcon;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -358,14 +367,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	rsp.remote_id = req->local_id;
 	rsp.status = A2MP_STATUS_SUCCESS;
 
-	hdev = hci_dev_get(req->local_id);
+	hdev = hci_dev_get(req->remote_id);
 	if (!hdev) {
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 		goto send_rsp;
 	}
 
+	hcon = hci_conn_hash_lookup_id(hdev, rsp.local_id);
+	if (!hcon) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv4 09/17] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 8c6cda4..e012855 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -272,6 +272,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -460,8 +489,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv4 10/17] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   60 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 59 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e012855..42bce4a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -334,6 +334,61 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct hci_conn *hcon;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	hcon = phylink_add(hdev, mgr, rsp->id);
+	if (!hcon)
+		goto done;
+
+	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -493,8 +548,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv4 11/17] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 93967f1..6e88a80 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -23,6 +23,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -135,6 +136,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7ed8e35..aba830f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -767,6 +767,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 42bce4a..c550589 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -629,7 +629,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -664,7 +664,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -703,7 +706,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -716,7 +719,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -745,7 +748,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -833,3 +836,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 9732f03..cfe047f 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -455,7 +455,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -946,6 +946,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -972,6 +984,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_choose_conn(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1008,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_choose_conn(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1082,7 +1105,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_choose_conn(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5454,7 +5477,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_choose_conn(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

Re: [PATCHv4 11/17] Bluetooth: Choose connection based on capabilities

[Mat Martineau]

Andrei -

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> Choose which L2CAP connection to establish by checking support
> for HS and remote side supported features.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/a2mp.h  |    2 ++
> include/net/bluetooth/l2cap.h |    1 +
> net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
> net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
> 4 files changed, 60 insertions(+), 10 deletions(-)
>
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 93967f1..6e88a80 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -23,6 +23,7 @@ struct amp_mgr {
> 	struct list_head	list;
> 	struct l2cap_conn	*l2cap_conn;
> 	struct l2cap_chan	*a2mp_chan;
> +	struct l2cap_chan	*bredr_chan;
> 	struct kref		kref;
> 	__u8			ident;
> 	__u8			handle;
> @@ -135,6 +136,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
> 				       struct sk_buff *skb);
> struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
> 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);
>
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index 7ed8e35..aba830f 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -767,6 +767,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
> void l2cap_chan_set_defaults(struct l2cap_chan *chan);
> int l2cap_ertm_init(struct l2cap_chan *chan);
> void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> +void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> void l2cap_chan_del(struct l2cap_chan *chan, int err);
>
> #endif /* __L2CAP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 42bce4a..c550589 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -629,7 +629,7 @@ static struct l2cap_ops a2mp_chan_ops = {
> 	.ready = l2cap_chan_no_ready,
> };
>
> -static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
> +static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
> {
> 	struct l2cap_chan *chan;
> 	int err;
> @@ -664,7 +664,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
>
> 	chan->conf_state = 0;
>
> -	l2cap_chan_add(conn, chan);
> +	if (locked)
> +		__l2cap_chan_add(conn, chan);
> +	else
> +		l2cap_chan_add(conn, chan);
>
> 	chan->remote_mps = chan->omtu;
> 	chan->mps = chan->omtu;
> @@ -703,7 +706,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
> 	return kref_put(&mgr->kref, &amp_mgr_destroy);
> }
>
> -static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
> +static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
> {
> 	struct amp_mgr *mgr;
> 	struct l2cap_chan *chan;
> @@ -716,7 +719,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
>
> 	mgr->l2cap_conn = conn;
>
> -	chan = a2mp_chan_open(conn);
> +	chan = a2mp_chan_open(conn, locked);
> 	if (!chan) {
> 		kfree(mgr);
> 		return NULL;
> @@ -745,7 +748,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
> {
> 	struct amp_mgr *mgr;
>
> -	mgr = amp_mgr_create(conn);
> +	mgr = amp_mgr_create(conn, false);
> 	if (!mgr) {
> 		BT_ERR("Could not create AMP manager");
> 		return NULL;
> @@ -833,3 +836,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
> 	amp_mgr_put(mgr);
> 	kfree(rsp);
> }
> +
> +void a2mp_discover_amp(struct l2cap_chan *chan)
> +{
> +	struct l2cap_conn *conn = chan->conn;
> +	struct amp_mgr *mgr = conn->hcon->amp_mgr;
> +	struct a2mp_discov_req req;
> +
> +	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
> +
> +	if (!mgr) {
> +		mgr = amp_mgr_create(conn, true);
> +		if (!mgr)
> +			return;
> +	}
> +
> +	mgr->bredr_chan = chan;
> +
> +	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
> +	req.ext_feat = 0;
> +	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
> +}
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index 9732f03..cfe047f 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -455,7 +455,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
> 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
> }
>
> -static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
> +void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
> {
> 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
> 	       __le16_to_cpu(chan->psm), chan->dcid);
> @@ -946,6 +946,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
> 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
> }
>
> +static bool __amp_capable(struct l2cap_chan *chan)
> +{
> +	struct l2cap_conn *conn = chan->conn;
> +
> +	if (enable_hs &&
> +	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
> +	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
> +		return true;
> +	else
> +		return false;
> +}
> +
> static void l2cap_send_conn_req(struct l2cap_chan *chan)
> {
> 	struct l2cap_conn *conn = chan->conn;
> @@ -972,6 +984,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
> 	chan->ops->ready(chan);
> }
>
> +static void l2cap_choose_conn(struct l2cap_chan *chan)

I find this function name a little confusing - "choose" sounds like it 
is just making a decision.  This function also takes the action of 
starting physical link setup or sending a connection request, which is 
more than just "choosing".

l2cap_start_connection, maybe?

> +{
> +	if (__amp_capable(chan)) {
> +		BT_DBG("chan %p AMP capable: discover AMPs", chan);
> +		a2mp_discover_amp(chan);
> +	} else {
> +		l2cap_send_conn_req(chan);
> +	}
> +}
> +
> static void l2cap_do_start(struct l2cap_chan *chan)
> {
> 	struct l2cap_conn *conn = chan->conn;
> @@ -986,8 +1008,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
> 			return;
>
> 		if (l2cap_chan_check_security(chan) &&
> -				__l2cap_no_conn_pending(chan))
> -			l2cap_send_conn_req(chan);
> +				__l2cap_no_conn_pending(chan)) {
> +			l2cap_choose_conn(chan);
> +		}
> 	} else {
> 		struct l2cap_info_req req;
> 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
> @@ -1082,7 +1105,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
> 				continue;
> 			}
>
> -			l2cap_send_conn_req(chan);
> +			l2cap_choose_conn(chan);
>
> 		} else if (chan->state == BT_CONNECT2) {
> 			struct l2cap_conn_rsp rsp;
> @@ -5454,7 +5477,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
>
> 		if (chan->state == BT_CONNECT) {
> 			if (!status) {
> -				l2cap_send_conn_req(chan);
> +				l2cap_choose_conn(chan);
> 			} else {
> 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
> 			}
> -- 
> 1.7.9.5



--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 11/17] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 09:26:27AM -0700, Mat Martineau wrote:

...

> >+static void l2cap_choose_conn(struct l2cap_chan *chan)
> 
> I find this function name a little confusing - "choose" sounds like
> it is just making a decision.  This function also takes the action
> of starting physical link setup or sending a connection request,
> which is more than just "choosing".
> 
> l2cap_start_connection, maybe?

Sounds good to me. Will change.

Best regards 
Andrei Emeltchenko 

[PATCHv4 12/17] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 7548644..5f44ebb 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 /* Remote AMP Controllers handling */
 static void amp_ctrl_get(struct amp_ctrl *ctrl)
@@ -121,3 +122,37 @@ struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	return hcon;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[PATCHv4 13/17] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 918a4be..3b6213c 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -36,5 +36,6 @@ struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id);
 void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 5f44ebb..0354648 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -156,3 +156,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[PATCHv4 14/17] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    2 ++
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 5 files changed, 34 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b6c08ce 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3b6213c..35f1765 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct amp_ctrl {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c550589..39e0f95 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -383,6 +383,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);
 
+	amp_create_phylink(hdev, mgr, hcon);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..9676393 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -43,3 +43,22 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6cc44cf..4b093f8 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1687,6 +1687,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2496,6 +2501,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv4 15/17] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 ++++++++++++++
 4 files changed, 113 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b6c08ce..74fcf98 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -21,5 +21,7 @@ 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_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);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 4ae5293..dc7a32e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 9676393..6f03fb3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,86 @@ 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);
 }
 
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+				     struct hci_conn *hcon)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+	if (!ctrl)
+		return false;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return true;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return false;
+	}
+
+	BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = hcon->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+
+	return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct hci_conn *hcon)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 4b093f8..57a6a16 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1689,7 +1703,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_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)
@@ -2420,6 +2445,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv4 16/17] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index aba830f..0967f9e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 39e0f95..05522ea 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index cfe047f..d2ab82c 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

Re: [PATCHv4 16/17] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Mat Martineau]

Andrei -

On Wed, 12 Sep 2012, Andrei Emeltchenko wrote:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> When there is no remote AMP controller found fallback to normal
> L2CAP sequence.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
> include/net/bluetooth/l2cap.h |    1 +
> net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
> net/bluetooth/l2cap_core.c    |    2 +-
> 3 files changed, 30 insertions(+), 1 deletion(-)
>
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index aba830f..0967f9e 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
> void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> void l2cap_chan_del(struct l2cap_chan *chan, int err);
> +void l2cap_send_conn_req(struct l2cap_chan *chan);
>
> #endif /* __L2CAP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 39e0f95..05522ea 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> 	u16 len = le16_to_cpu(hdr->len);
> 	struct a2mp_cl *cl;
> 	u16 ext_feat;
> +	bool found = false;
>
> 	if (len < sizeof(*rsp))
> 		return -EINVAL;
> @@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
> 			struct a2mp_info_req req;
>
> +			found = true;
> 			req.id = cl->id;
> 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
> 				  sizeof(req), &req);
> @@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> 		cl = (void *) skb_pull(skb, sizeof(*cl));
> 	}
>
> +	/* Fall back to L2CAP init sequence */
> +	if (!found) {
> +		struct l2cap_conn *conn = mgr->l2cap_conn;
> +		struct l2cap_chan *chan;
> +
> +		mutex_lock(&conn->chan_lock);
> +
> +		list_for_each_entry(chan, &conn->chan_l, list) {
> +
> +			BT_DBG("chan %p state %s", chan,
> +			       state_to_string(chan->state));
> +
> +			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
> +				continue;
> +
> +			l2cap_chan_lock(chan);
> +
> +			if (chan->state == BT_CONNECT)
> +				l2cap_send_conn_req(chan);
> +
> +			l2cap_chan_unlock(chan);
> +		}
> +
> +		mutex_unlock(&conn->chan_lock);
> +	}
> +
> 	return 0;
> }
>
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index cfe047f..d2ab82c 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
> 		return false;
> }
>
> -static void l2cap_send_conn_req(struct l2cap_chan *chan)
> +void l2cap_send_conn_req(struct l2cap_chan *chan)
> {
> 	struct l2cap_conn *conn = chan->conn;
> 	struct l2cap_conn_req req;
> -- 
> 1.7.9.5
>
>

Here's another place where we have duplicated functionality.  This 
patch has BR/EDR fallback too:

[RFCv1 13/20] Bluetooth: Handle physical link completion

You can call l2cap_physical_cfm() with a failure result instead of 
calling l2cap_send_conn_req() directly.

--
Mat Martineau

The Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum,
hosted by The Linux Foundation

Re: [PATCHv4 16/17] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

Hi Mat,

On Thu, Sep 13, 2012 at 09:39:57AM -0700, Mat Martineau wrote:
> >From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> >
> >When there is no remote AMP controller found fallback to normal
> >L2CAP sequence.
> >
> >Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> >---
> >include/net/bluetooth/l2cap.h |    1 +
> >net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
> >net/bluetooth/l2cap_core.c    |    2 +-
> >3 files changed, 30 insertions(+), 1 deletion(-)
> >
> >diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> >index aba830f..0967f9e 100644
> >--- a/include/net/bluetooth/l2cap.h
> >+++ b/include/net/bluetooth/l2cap.h
> >@@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
> >void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> >void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
> >void l2cap_chan_del(struct l2cap_chan *chan, int err);
> >+void l2cap_send_conn_req(struct l2cap_chan *chan);
> >
> >#endif /* __L2CAP_H */
> >diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> >index 39e0f95..05522ea 100644
> >--- a/net/bluetooth/a2mp.c
> >+++ b/net/bluetooth/a2mp.c
> >@@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> >	u16 len = le16_to_cpu(hdr->len);
> >	struct a2mp_cl *cl;
> >	u16 ext_feat;
> >+	bool found = false;
> >
> >	if (len < sizeof(*rsp))
> >		return -EINVAL;
> >@@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> >		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
> >			struct a2mp_info_req req;
> >
> >+			found = true;
> >			req.id = cl->id;
> >			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
> >				  sizeof(req), &req);
> >@@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
> >		cl = (void *) skb_pull(skb, sizeof(*cl));
> >	}
> >
> >+	/* Fall back to L2CAP init sequence */
> >+	if (!found) {
> >+		struct l2cap_conn *conn = mgr->l2cap_conn;
> >+		struct l2cap_chan *chan;
> >+
> >+		mutex_lock(&conn->chan_lock);
> >+
> >+		list_for_each_entry(chan, &conn->chan_l, list) {
> >+
> >+			BT_DBG("chan %p state %s", chan,
> >+			       state_to_string(chan->state));
> >+
> >+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
> >+				continue;
> >+
> >+			l2cap_chan_lock(chan);
> >+
> >+			if (chan->state == BT_CONNECT)
> >+				l2cap_send_conn_req(chan);
> >+
> >+			l2cap_chan_unlock(chan);
> >+		}
> >+
> >+		mutex_unlock(&conn->chan_lock);
> >+	}
> >+
> >	return 0;
> >}
> >
> >diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> >index cfe047f..d2ab82c 100644
> >--- a/net/bluetooth/l2cap_core.c
> >+++ b/net/bluetooth/l2cap_core.c
> >@@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
> >		return false;
> >}
> >
> >-static void l2cap_send_conn_req(struct l2cap_chan *chan)
> >+void l2cap_send_conn_req(struct l2cap_chan *chan)
> >{
> >	struct l2cap_conn *conn = chan->conn;
> >	struct l2cap_conn_req req;
> >-- 
> >1.7.9.5
> >
> >
> 
> Here's another place where we have duplicated functionality.  This
> patch has BR/EDR fallback too:
> 
> [RFCv1 13/20] Bluetooth: Handle physical link completion
> 
> You can call l2cap_physical_cfm() with a failure result instead of
> calling l2cap_send_conn_req() directly.

I suppose there is no such function yet, otherwise this can be done.

Best regards 
Andrei Emeltchenko 

[PATCHv4 17/17] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h |    3 +++
 include/net/bluetooth/amp.h  |    2 ++
 net/bluetooth/a2mp.c         |   39 ++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c          |   15 +++++++++++++++
 net/bluetooth/hci_event.c    |   21 +++++++++++++++++++++
 5 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6e88a80..c955f1f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -30,6 +30,7 @@ struct amp_mgr {
 	enum {
 		READ_LOC_AMP_INFO,
 		READ_LOC_AMP_ASSOC,
+		READ_LOC_AMP_ASSOC_FINAL,
 	} state;
 	unsigned long		flags;
 
@@ -132,6 +133,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);
@@ -139,5 +141,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 74fcf98..70d33d4 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 05522ea..c9a3bc8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -68,7 +68,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;
@@ -867,6 +867,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 hci_conn *hcon;
+	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;
+	}
+
+	hcon = hci_conn_hash_lookup_id(hdev, hdev->id);
+	if (!hcon)
+		goto clean;
+
+	req->local_id = hdev->id;
+	req->remote_id = hcon->remote_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 6f03fb3..9e2e639 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,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 57a6a16..c5214e9 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,
@@ -3566,6 +3567,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;
@@ -3722,6 +3739,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;
-- 
1.7.9.5

[PATCHv5 00/17] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p5: Fix issues reported by Mat in mailing list review
	* p4: Rebased against recent bluetooth-next, minor fixes
	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
	struct phy_link, refactoring.
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (16):
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct and heplers
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Handle create / disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   20 ++
 include/net/bluetooth/amp.h      |   29 +++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   11 +
 include/net/bluetooth/l2cap.h    |    2 +
 include/net/bluetooth/pal.h      |   40 ++++
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  424 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  159 ++++++++++++++
 net/bluetooth/hci_event.c        |  106 +++++++++-
 net/bluetooth/l2cap_core.c       |   35 +++-
 net/bluetooth/pal.c              |  204 ++++++++++++++++++
 13 files changed, 1034 insertions(+), 38 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/amp.c
 create mode 100644 net/bluetooth/pal.c

-- 
1.7.9.5

[PATCHv5 01/17] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv5 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    8 ++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..e56d656 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,11 +20,15 @@
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum {
+		READ_LOC_AMP_INFO,
+	} state;
 	unsigned long		flags;
 };
 
@@ -118,9 +122,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1f..3f93060 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -552,6 +560,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	return mgr;
 }
 
@@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

Re: [PATCHv5 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Marcel Holtmann]

Hi Andrei,

> Create amp_mgr_list global list which will be used by different
> hci devices to find amp_mgr.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/a2mp.h |    8 ++++++++
>  net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
>  2 files changed, 37 insertions(+)
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 6a76e0a..e56d656 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -20,11 +20,15 @@
>  #define A2MP_FEAT_EXT	0x8000
>  
>  struct amp_mgr {
> +	struct list_head	list;
>  	struct l2cap_conn	*l2cap_conn;
>  	struct l2cap_chan	*a2mp_chan;
>  	struct kref		kref;
>  	__u8			ident;
>  	__u8			handle;
> +	enum {
> +		READ_LOC_AMP_INFO,
> +	} state;

I am not a huge fan of these things. Wouldn't it be better to define the
states externally properly.

Regards

Marcel

Re: [PATCHv5 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

Hi Marcel,

On Tue, Sep 18, 2012 at 09:43:51AM +0200, Marcel Holtmann wrote:
> Hi Andrei,
> 
> > Create amp_mgr_list global list which will be used by different
> > hci devices to find amp_mgr.
> > 
> > Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> > ---
> >  include/net/bluetooth/a2mp.h |    8 ++++++++
> >  net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
> >  2 files changed, 37 insertions(+)
> > 
> > diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> > index 6a76e0a..e56d656 100644
> > --- a/include/net/bluetooth/a2mp.h
> > +++ b/include/net/bluetooth/a2mp.h
> > @@ -20,11 +20,15 @@
> >  #define A2MP_FEAT_EXT	0x8000
> >  
> >  struct amp_mgr {
> > +	struct list_head	list;
> >  	struct l2cap_conn	*l2cap_conn;
> >  	struct l2cap_chan	*a2mp_chan;
> >  	struct kref		kref;
> >  	__u8			ident;
> >  	__u8			handle;
> > +	enum {
> > +		READ_LOC_AMP_INFO,
> > +	} state;
> 
> I am not a huge fan of these things. Wouldn't it be better to define the
> states externally properly.

Yes, this might be done.

Best regards 
Andrei Emeltchenko 

[PATCHv5 03/17] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e56d656..c21268a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -130,5 +130,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3f93060..0e97b3b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -599,3 +597,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 48d7302..5ae5121 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv5 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 ++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 ++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   56 ++++++++++++++++++++++++++++++++++----
 net/bluetooth/amp.c              |   45 ++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++
 8 files changed, 171 insertions(+), 6 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c21268a..f9010c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	__u8			handle;
 	enum {
 		READ_LOC_AMP_INFO,
+		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
 };
@@ -132,5 +133,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a3337e..1174218 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0e97b3b..7140061 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -218,26 +219,37 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 {
 	struct a2mp_amp_assoc_req *req = (void *) skb->data;
 	struct hci_dev *hdev;
+	struct amp_mgr *tmp;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
 
 	BT_DBG("id %d", req->id);
 
+	/* Make sure that other request is not processed */
+	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
 	hdev = hci_dev_get(req->id);
-	if (!hdev || hdev->amp_type == HCI_BREDR) {
+	if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
 		struct a2mp_amp_assoc_rsp rsp;
 		rsp.id = req->id;
-		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		if (tmp) {
+			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+			amp_mgr_put(tmp);
+		} else {
+			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		}
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
+
+		goto done;
 	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+	amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
 	if (hdev)
 		hci_dev_put(hdev);
 
@@ -624,3 +636,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 5ae5121..6cc44cf 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2302,6 +2339,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv5 05/17] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7140061..f04c441 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -391,8 +448,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv5 06/17] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structures. Physical links are represented by
hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
we use AMP_LINK.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |    1 +
 include/net/bluetooth/pal.h      |   26 +++++++++++++++++++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |    1 +
 net/bluetooth/pal.c              |   43 ++++++++++++++++++++++++++++++++++++++
 6 files changed, 73 insertions(+), 1 deletion(-)
 create mode 100644 include/net/bluetooth/pal.h
 create mode 100644 net/bluetooth/pal.c

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1174218..a1cda4d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -316,6 +316,7 @@ struct hci_conn {
 
 	__u8		remote_cap;
 	__u8		remote_auth;
+	__u8		remote_id;
 	bool		flush_key;
 
 	unsigned int	sent;
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
new file mode 100644
index 0000000..a0f441b
--- /dev/null
+++ b/include/net/bluetooth/pal.h
@@ -0,0 +1,26 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __PAL_H
+#define __PAL_H
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id);
+
+#endif /* __PAL_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index dea6a28..3f76fc2 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o pal.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f04c441..ec0e9c3 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,7 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <net/bluetooth/pal.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
new file mode 100644
index 0000000..9e4ea85
--- /dev/null
+++ b/net/bluetooth/pal.c
@@ -0,0 +1,43 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/pal.h>
+
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id)
+{
+	bdaddr_t *dst = mgr->l2cap_conn->dst;
+	struct hci_conn *hcon;
+
+	hcon = hci_conn_add(hdev, AMP_LINK, dst);
+	if (!hcon)
+		return NULL;
+
+	hcon->state = BT_CONNECT;
+	hcon->out = true;
+	hcon->attempt++;
+	hcon->handle = __next_handle(mgr);
+	hcon->remote_id = remote_id;
+	hcon->amp_mgr = mgr;
+
+	return hcon;
+}
-- 
1.7.9.5

[PATCHv5 07/17] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/pal.h  |   14 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/pal.c          |   81 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index f9010c0..93967f1 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,9 @@ struct amp_mgr {
 		READ_LOC_AMP_ASSOC,
 	} state;
 	unsigned long		flags;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index a0f441b..918a4be 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -20,6 +20,20 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ec0e9c3..dc5c01b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -595,6 +595,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -631,6 +632,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	mutex_lock(&amp_mgr_list_lock);
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 9e4ea85..bc2426a 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -13,6 +13,87 @@
 
 #include <net/bluetooth/pal.h>
 
+/* Remote AMP Controllers handling */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	if (ctrl->assoc)
+		kfree(ctrl->assoc);
+
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 static u8 __next_handle(struct amp_mgr *mgr)
 {
-- 
1.7.9.5

[PATCHv5 08/17] Bluetooth: AMP: Handle create / disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use hci_conn structure to keep track about AMP physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index dc5c01b..df50c4d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -322,6 +322,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;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -339,7 +340,14 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
-	rsp.status = A2MP_STATUS_SUCCESS;
+	hcon = phylink_add(hdev, mgr, req->local_id);
+	if (hcon) {
+		BT_DBG("hcon %p", hcon);
+
+		rsp.status = A2MP_STATUS_SUCCESS;
+	} else {
+		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+	}
 
 send_rsp:
 	if (hdev)
@@ -358,6 +366,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct hci_conn *hcon;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -368,14 +377,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	rsp.remote_id = req->local_id;
 	rsp.status = A2MP_STATUS_SUCCESS;
 
-	hdev = hci_dev_get(req->local_id);
+	hdev = hci_dev_get(req->remote_id);
 	if (!hdev) {
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 		goto send_rsp;
 	}
 
+	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+	if (!hcon) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv5 09/17] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index df50c4d..d5e4003 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -272,6 +272,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -470,8 +499,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv5 10/17] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   60 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 59 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d5e4003..6969925 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -344,6 +344,61 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct hci_conn *hcon;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	hcon = phylink_add(hdev, mgr, rsp->id);
+	if (!hcon)
+		goto done;
+
+	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -503,8 +558,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv5 11/17] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 93967f1..6e88a80 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -23,6 +23,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -135,6 +136,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7ed8e35..aba830f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -767,6 +767,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6969925..eb85727 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -639,7 +639,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -674,7 +674,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -713,7 +716,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -726,7 +729,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -755,7 +758,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -843,3 +846,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 9732f03..df27198 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -455,7 +455,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -946,6 +946,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -972,6 +984,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_start_connection(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1008,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_start_connection(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1082,7 +1105,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_start_connection(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5454,7 +5477,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_start_connection(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv5 12/17] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/pal.c |   35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index bc2426a..8e28986 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -12,6 +12,7 @@
 */
 
 #include <net/bluetooth/pal.h>
+#include <crypto/hash.h>
 
 /* Remote AMP Controllers handling */
 static void amp_ctrl_get(struct amp_ctrl *ctrl)
@@ -122,3 +123,37 @@ struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	return hcon;
 }
+
+int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
-- 
1.7.9.5

[PATCHv5 13/17] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/pal.h |    1 +
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/pal.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 47 insertions(+)

diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 918a4be..3b6213c 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -36,5 +36,6 @@ struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id);
 void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type);
 
 #endif /* __PAL_H */
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/pal.c b/net/bluetooth/pal.c
index 8e28986..99817ce 100644
--- a/net/bluetooth/pal.c
+++ b/net/bluetooth/pal.c
@@ -157,3 +157,48 @@ int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	crypto_free_shash(tfm);
 	return ret;
 }
+
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
-- 
1.7.9.5

[PATCHv5 14/17] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    4 ++++
 include/net/bluetooth/pal.h |    1 -
 net/bluetooth/a2mp.c        |    4 ++++
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 5 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..b6c08ce 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,8 +14,12 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+#include <net/bluetooth/pal.h>
+
 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/pal.h b/include/net/bluetooth/pal.h
index 3b6213c..35f1765 100644
--- a/include/net/bluetooth/pal.h
+++ b/include/net/bluetooth/pal.h
@@ -18,7 +18,6 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
-#include <net/bluetooth/amp.h>
 
 struct amp_ctrl {
 	struct list_head	list;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index eb85727..bc29246 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -393,6 +393,10 @@ 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:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..9676393 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -43,3 +43,22 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6cc44cf..4b093f8 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1687,6 +1687,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2496,6 +2501,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv5 15/17] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 ++++++++++++++
 4 files changed, 113 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index b6c08ce..74fcf98 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -21,5 +21,7 @@ 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_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);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index a1cda4d..eebfc61 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 9676393..6f03fb3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,86 @@ 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);
 }
 
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+				     struct hci_conn *hcon)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+	if (!ctrl)
+		return false;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return true;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return false;
+	}
+
+	BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = hcon->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+
+	return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct hci_conn *hcon)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 4b093f8..57a6a16 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1689,7 +1703,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_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)
@@ -2420,6 +2445,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv5 16/17] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index aba830f..0967f9e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index bc29246..0904c58 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -181,6 +181,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -211,6 +212,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -220,6 +222,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index df27198..8717678 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv5 17/17] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h |    3 +++
 include/net/bluetooth/amp.h  |    2 ++
 net/bluetooth/a2mp.c         |   39 ++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c          |   15 +++++++++++++++
 net/bluetooth/hci_event.c    |   21 +++++++++++++++++++++
 5 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6e88a80..c955f1f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -30,6 +30,7 @@ struct amp_mgr {
 	enum {
 		READ_LOC_AMP_INFO,
 		READ_LOC_AMP_ASSOC,
+		READ_LOC_AMP_ASSOC_FINAL,
 	} state;
 	unsigned long		flags;
 
@@ -132,6 +133,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);
@@ -139,5 +141,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 74fcf98..70d33d4 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -19,6 +19,8 @@
 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/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0904c58..03facef 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -68,7 +68,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;
@@ -879,6 +879,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 6f03fb3..9e2e639 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -44,6 +44,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 57a6a16..c5214e9 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,
@@ -3566,6 +3567,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;
@@ -3722,6 +3739,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;
-- 
1.7.9.5

[PATCHv6 00/17] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p6: Refactoring: moving code from pal.[ch] to amp.[ch]
	* p5: Fix issues reported by Mat in mailing list review
	* p4: Rebased against recent bluetooth-next, minor fixes
	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
	struct phy_link, refactoring.
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (16):
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct and heplers
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Handle create / disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   22 ++
 include/net/bluetooth/amp.h      |   47 +++++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   11 +
 include/net/bluetooth/l2cap.h    |    4 +
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  423 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  348 +++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |  106 +++++++++-
 net/bluetooth/l2cap_core.c       |   35 +++-
 11 files changed, 1000 insertions(+), 38 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.9.5

[PATCHv6 01/17] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 0f28f70..42aae18 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv6 02/17] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |   10 ++++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 39 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..316c1c8 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -19,12 +19,18 @@
 
 #define A2MP_FEAT_EXT	0x8000
 
+enum amp_mgr_state {
+	READ_LOC_AMP_INFO,
+};
+
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum amp_mgr_state	state;
 	unsigned long		flags;
 };
 
@@ -118,9 +124,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1f..3f93060 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -552,6 +560,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	return mgr;
 }
 
@@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

[PATCHv6 03/17] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 316c1c8..af59468 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -132,5 +132,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3f93060..0e97b3b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -599,3 +597,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 48d7302..5ae5121 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv6 04/17] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 ++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 ++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   56 ++++++++++++++++++++++++++++++++++----
 net/bluetooth/amp.c              |   45 ++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++
 8 files changed, 171 insertions(+), 6 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index af59468..d547671 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -21,6 +21,7 @@
 
 enum amp_mgr_state {
 	READ_LOC_AMP_INFO,
+	READ_LOC_AMP_ASSOC,
 };
 
 struct amp_mgr {
@@ -134,5 +135,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 42aae18..1cb8b55 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a3337e..1174218 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0e97b3b..7140061 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -218,26 +219,37 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 {
 	struct a2mp_amp_assoc_req *req = (void *) skb->data;
 	struct hci_dev *hdev;
+	struct amp_mgr *tmp;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
 
 	BT_DBG("id %d", req->id);
 
+	/* Make sure that other request is not processed */
+	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
 	hdev = hci_dev_get(req->id);
-	if (!hdev || hdev->amp_type == HCI_BREDR) {
+	if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
 		struct a2mp_amp_assoc_rsp rsp;
 		rsp.id = req->id;
-		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		if (tmp) {
+			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+			amp_mgr_put(tmp);
+		} else {
+			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		}
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
+
+		goto done;
 	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+	amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
 	if (hdev)
 		hci_dev_put(hdev);
 
@@ -624,3 +636,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 5ae5121..6cc44cf 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2302,6 +2339,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv6 05/17] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7140061..f04c441 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -391,8 +448,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv6 06/17] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structures. Physical links are represented by
hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
we use AMP_LINK.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    3 +++
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/amp.c              |   29 +++++++++++++++++++++++++++++
 4 files changed, 34 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..3414dfd 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,6 +14,9 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id);
+
 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);
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1cb8b55..4c41b8c 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 1174218..a1cda4d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -316,6 +316,7 @@ struct hci_conn {
 
 	__u8		remote_cap;
 	__u8		remote_auth;
+	__u8		remote_id;
 	bool		flush_key;
 
 	unsigned int	sent;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..50a7b2f 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,35 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id)
+{
+	bdaddr_t *dst = mgr->l2cap_conn->dst;
+	struct hci_conn *hcon;
+
+	hcon = hci_conn_add(hdev, AMP_LINK, dst);
+	if (!hcon)
+		return NULL;
+
+	hcon->state = BT_CONNECT;
+	hcon->out = true;
+	hcon->attempt++;
+	hcon->handle = __next_handle(mgr);
+	hcon->remote_id = remote_id;
+	hcon->amp_mgr = mgr;
+
+	return hcon;
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv6 07/17] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/amp.h  |   15 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/amp.c          |   79 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 102 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index d547671..99c7389 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -33,6 +33,9 @@ struct amp_mgr {
 	__u8			handle;
 	enum amp_mgr_state	state;
 	unsigned long		flags;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 3414dfd..f57854f 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,6 +14,21 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
+
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f04c441..35e188c 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -594,6 +594,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -630,6 +631,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	mutex_lock(&amp_mgr_list_lock);
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 50a7b2f..b26ca7c 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,85 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+/* Remote AMP Controllers interface */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	kfree(ctrl->assoc);
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl = NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id)
+			break;
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	BT_DBG("mgr %p id %d ctrl %p", mgr, id, ctrl);
+
+	if (ctrl)
+		amp_ctrl_get(ctrl);
+
+	return ctrl;
+}
+
 /* Physical Link interface */
 static u8 __next_handle(struct amp_mgr *mgr)
 {
-- 
1.7.9.5

[PATCHv6 08/17] Bluetooth: AMP: Handle create / disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use hci_conn structure to keep track about AMP physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 35e188c..0125417 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -321,6 +321,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;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -338,7 +339,14 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
-	rsp.status = A2MP_STATUS_SUCCESS;
+	hcon = phylink_add(hdev, mgr, req->local_id);
+	if (hcon) {
+		BT_DBG("hcon %p", hcon);
+
+		rsp.status = A2MP_STATUS_SUCCESS;
+	} else {
+		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+	}
 
 send_rsp:
 	if (hdev)
@@ -357,6 +365,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct hci_conn *hcon;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -367,14 +376,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	rsp.remote_id = req->local_id;
 	rsp.status = A2MP_STATUS_SUCCESS;
 
-	hdev = hci_dev_get(req->local_id);
+	hdev = hci_dev_get(req->remote_id);
 	if (!hdev) {
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 		goto send_rsp;
 	}
 
+	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+	if (!hcon) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv6 09/17] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0125417..594df96 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -271,6 +271,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -469,8 +498,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv6 10/17] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   60 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 59 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 594df96..d0fde05 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -343,6 +343,61 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct hci_conn *hcon;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	hcon = phylink_add(hdev, mgr, rsp->id);
+	if (!hcon)
+		goto done;
+
+	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -502,8 +557,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv6 11/17] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 99c7389..9fda7c9 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -137,6 +138,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7ed8e35..aba830f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -767,6 +767,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d0fde05..93adaad 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -638,7 +638,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -673,7 +673,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -712,7 +715,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -725,7 +728,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -754,7 +757,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -842,3 +845,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 9732f03..df27198 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -455,7 +455,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -946,6 +946,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -972,6 +984,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_start_connection(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1008,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_start_connection(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1082,7 +1105,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_start_connection(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5454,7 +5477,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_start_connection(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv6 12/17] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/amp.c |   36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index b26ca7c..c9636ae 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <crypto/hash.h>
 
 /* Remote AMP Controllers interface */
 static void amp_ctrl_get(struct amp_ctrl *ctrl)
@@ -125,6 +126,41 @@ struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 	return hcon;
 }
 
+/* AMP crypto key generation interface */
+static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv6 13/17] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    2 ++
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/amp.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index f57854f..763b463 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -32,6 +32,8 @@ void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
+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);
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index c9636ae..de45696 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -161,6 +161,51 @@ static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	return ret;
 }
 
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv6 14/17] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    2 ++
 net/bluetooth/a2mp.c        |    2 ++
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 4 files changed, 32 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 763b463..cadb3d0 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -37,5 +37,7 @@ 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 
 #endif /* __AMP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 93adaad..773e8fc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -392,6 +392,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);
 
+	amp_create_phylink(hdev, mgr, hcon);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index de45696..c95e455 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -232,3 +232,22 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6cc44cf..4b093f8 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1687,6 +1687,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2496,6 +2501,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv6 15/17] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 ++++++++++++++
 4 files changed, 113 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index cadb3d0..8f80329 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -39,5 +39,7 @@ 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_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);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index a1cda4d..eebfc61 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index c95e455..da8ae2d 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -233,6 +233,86 @@ 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);
 }
 
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+				     struct hci_conn *hcon)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+	if (!ctrl)
+		return false;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return true;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return false;
+	}
+
+	BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = hcon->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+
+	return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct hci_conn *hcon)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 4b093f8..57a6a16 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1689,7 +1703,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_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)
@@ -2420,6 +2445,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv6 16/17] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index aba830f..0967f9e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 773e8fc..28d1246 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -180,6 +180,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -210,6 +211,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -219,6 +221,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index df27198..8717678 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv6 17/17] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h  |    3 +++
 include/net/bluetooth/amp.h   |    2 ++
 include/net/bluetooth/l2cap.h |    2 ++
 net/bluetooth/a2mp.c          |   41 ++++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c           |   15 +++++++++++++++
 net/bluetooth/hci_event.c     |   21 +++++++++++++++++++++
 6 files changed, 83 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 9fda7c9..e776ab2 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 8f80329..70496c0 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 0967f9e..ab58b81 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 28d1246..375a67f 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 da8ae2d..05e2636 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 57a6a16..c5214e9 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,
@@ -3566,6 +3567,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;
@@ -3722,6 +3739,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;
-- 
1.7.9.5

Re: [PATCHv6 00/17] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

ping

On Tue, Sep 18, 2012 at 05:28:12PM +0300, Andrei Emeltchenko wrote:
> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> This set of patches enhances A2MP protocol and creates physical
> link between AMP controllers. This is further iteration towards
> Bluetooth High Speed.
> 
> Changes:
> 	* p6: Refactoring: moving code from pal.[ch] to amp.[ch]
> 	* p5: Fix issues reported by Mat in mailing list review
> 	* p4: Rebased against recent bluetooth-next, minor fixes
> 	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
> 	struct phy_link, refactoring.
> 	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
> 	* p1: Fixed locking issues, added basic logical link preparation.
> 	* v3: Remove workqueue from callback processing; change callback functions
> 	names according to reviewers recommendations; create global amp_mgr_list to
> 	have lookup to amp manager, physical and logical links so for those HCI events
> 	which might be identified by __handler__ we have lookup; remove extensive
> 	hexdump from gen_amp_key.
> 	* v2: Fix typos and bugs, add functionality: now physical connection
> 	might be established.
> 	* v1: Fix typos, change debug prints, refactor code for better
> 	splitting functionality.
> 
> Andrei Emeltchenko (16):
>   Bluetooth: Add HCI logical link cmds definitions
>   Bluetooth: A2MP: Create amp_mgr global list
>   Bluetooth: AMP: Use HCI cmd to Read AMP Info
>   Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
>   Bluetooth: A2MP: Process Discover Response
>   Bluetooth: AMP: Physical link struct and heplers
>   Bluetooth: AMP: Remote AMP ctrl definitions
>   Bluetooth: AMP: Handle create / disc phylink req
>   Bluetooth: A2MP: Process A2MP Getinfo Rsp
>   Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
>   Bluetooth: Choose connection based on capabilities
>   Bluetooth: AMP: Add AMP key calculation
>   Bluetooth: AMP: Create Physical Link
>   Bluetooth: AMP: Write remote AMP Assoc
>   Bluetooth: A2MP: Add fallback to normal l2cap init sequence
>   Bluetooth: AMP: Process Chan Selected event
> 
> Dmitry Kasatkin (1):
>   Bluetooth: Add function to derive AMP key using hmac
> 
>  include/net/bluetooth/a2mp.h     |   22 ++
>  include/net/bluetooth/amp.h      |   47 +++++
>  include/net/bluetooth/hci.h      |   39 +++-
>  include/net/bluetooth/hci_core.h |   11 +
>  include/net/bluetooth/l2cap.h    |    4 +
>  net/bluetooth/Kconfig            |    1 +
>  net/bluetooth/Makefile           |    2 +-
>  net/bluetooth/a2mp.c             |  423 +++++++++++++++++++++++++++++++++++---
>  net/bluetooth/amp.c              |  348 +++++++++++++++++++++++++++++++
>  net/bluetooth/hci_event.c        |  106 +++++++++-
>  net/bluetooth/l2cap_core.c       |   35 +++-
>  11 files changed, 1000 insertions(+), 38 deletions(-)
>  create mode 100644 include/net/bluetooth/amp.h
>  create mode 100644 net/bluetooth/amp.c
> 
> -- 
> 1.7.9.5
> 
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html

[PATCHv7 00/19] Bluetooth: Create AMP physical link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

This set of patches enhances A2MP protocol and creates physical
link between AMP controllers. This is further iteration towards
Bluetooth High Speed.

Changes:
	* p7: Fix ctrl_lookup, add phylink accept functions.
	* p6: Refactoring: moving code from pal.[ch] to amp.[ch]
	* p5: Fix issues reported by Mat in mailing list review
	* p4: Rebased against recent bluetooth-next, minor fixes
	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
	struct phy_link, refactoring.
	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
	* p1: Fixed locking issues, added basic logical link preparation.
	* v3: Remove workqueue from callback processing; change callback functions
	names according to reviewers recommendations; create global amp_mgr_list to
	have lookup to amp manager, physical and logical links so for those HCI events
	which might be identified by __handler__ we have lookup; remove extensive
	hexdump from gen_amp_key.
	* v2: Fix typos and bugs, add functionality: now physical connection
	might be established.
	* v1: Fix typos, change debug prints, refactor code for better
	splitting functionality.

Andrei Emeltchenko (18):
  Bluetooth: Add HCI logical link cmds definitions
  Bluetooth: A2MP: Create amp_mgr global list
  Bluetooth: AMP: Use HCI cmd to Read AMP Info
  Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
  Bluetooth: A2MP: Process Discover Response
  Bluetooth: AMP: Physical link struct and heplers
  Bluetooth: AMP: Remote AMP ctrl definitions
  Bluetooth: AMP: Handle create / disc phylink req
  Bluetooth: A2MP: Process A2MP Getinfo Rsp
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
  Bluetooth: Choose connection based on capabilities
  Bluetooth: AMP: Add AMP key calculation
  Bluetooth: AMP: Create Physical Link
  Bluetooth: AMP: Write remote AMP Assoc
  Bluetooth: A2MP: Add fallback to normal l2cap init sequence
  Bluetooth: AMP: Process Chan Selected event
  Bluetooth: AMP: Accept Physical Link
  Bluetooth: AMP: Handle Accept phylink command status evt

Dmitry Kasatkin (1):
  Bluetooth: Add function to derive AMP key using hmac

 include/net/bluetooth/a2mp.h     |   22 ++
 include/net/bluetooth/amp.h      |   50 +++++
 include/net/bluetooth/hci.h      |   39 +++-
 include/net/bluetooth/hci_core.h |   11 +
 include/net/bluetooth/l2cap.h    |    4 +
 net/bluetooth/Kconfig            |    1 +
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |  454 +++++++++++++++++++++++++++++++++++---
 net/bluetooth/amp.c              |  367 ++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |  126 ++++++++++-
 net/bluetooth/l2cap_core.c       |   35 ++-
 11 files changed, 1072 insertions(+), 39 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.9.5

[PATCHv7 01/19] Bluetooth: Add HCI logical link cmds definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/hci.h |   36 +++++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 76b2b6b..4be26abf6d 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -556,12 +556,46 @@ struct hci_cp_accept_phy_link {
 	__u8     key[HCI_AMP_LINK_KEY_SIZE];
 } __packed;
 
-#define HCI_OP_DISCONN_PHY_LINK	0x0437
+#define HCI_OP_DISCONN_PHY_LINK		0x0437
 struct hci_cp_disconn_phy_link {
 	__u8     phy_handle;
 	__u8     reason;
 } __packed;
 
+struct ext_flow_spec {
+	__u8       id;
+	__u8       stype;
+	__le16     msdu;
+	__le32     sdu_itime;
+	__le32     acc_lat;
+	__le32     flush_to;
+} __packed;
+
+#define HCI_OP_CREATE_LOGICAL_LINK	0x0438
+#define HCI_OP_ACCEPT_LOGICAL_LINK	0x0439
+struct hci_cp_create_accept_logical_link {
+	__u8                  phy_handle;
+	struct ext_flow_spec  tx_flow_spec;
+	struct ext_flow_spec  rx_flow_spec;
+} __packed;
+
+#define HCI_OP_DISCONN_LOGICAL_LINK	0x043a
+struct hci_cp_disconn_logical_link {
+	__le16   log_handle;
+} __packed;
+
+#define HCI_OP_LOGICAL_LINK_CANCEL	0x043b
+struct hci_cp_logical_link_cancel {
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
+struct hci_rp_logical_link_cancel {
+	__u8     status;
+	__u8     phy_handle;
+	__u8     flow_spec_id;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
-- 
1.7.9.5

[PATCHv7 02/19] Bluetooth: A2MP: Create amp_mgr global list

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create amp_mgr_list global list which will be used by different
hci devices to find amp_mgr.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |   10 ++++++++++
 net/bluetooth/a2mp.c         |   29 +++++++++++++++++++++++++++++
 2 files changed, 39 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6a76e0a..316c1c8 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -19,12 +19,18 @@
 
 #define A2MP_FEAT_EXT	0x8000
 
+enum amp_mgr_state {
+	READ_LOC_AMP_INFO,
+};
+
 struct amp_mgr {
+	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
+	enum amp_mgr_state	state;
 	unsigned long		flags;
 };
 
@@ -118,9 +124,13 @@ struct a2mp_physlink_rsp {
 #define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
 #define A2MP_STATUS_SECURITY_VIOLATION		0x06
 
+extern struct list_head amp_mgr_list;
+extern struct mutex amp_mgr_list_lock;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(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);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0760d1f..3f93060 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,10 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* Global AMP Manager list */
+LIST_HEAD(amp_mgr_list);
+DEFINE_MUTEX(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -516,6 +520,10 @@ static void amp_mgr_destroy(struct kref *kref)
 
 	BT_DBG("mgr %p", mgr);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	kfree(mgr);
 }
 
@@ -552,6 +560,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	mutex_lock(&amp_mgr_list_lock);
+	list_add(&mgr->list, &amp_mgr_list);
+	mutex_unlock(&amp_mgr_list_lock);
+
 	return mgr;
 }
 
@@ -570,3 +582,20 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 
 	return mgr->a2mp_chan;
 }
+
+struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
+{
+	struct amp_mgr *mgr;
+
+	mutex_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if (mgr->state == state) {
+			amp_mgr_get(mgr);
+			mutex_unlock(&amp_mgr_list_lock);
+			return mgr;
+		}
+	}
+	mutex_unlock(&amp_mgr_list_lock);
+
+	return NULL;
+}
-- 
1.7.9.5

[PATCHv7 03/19] Bluetooth: AMP: Use HCI cmd to Read AMP Info

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get Info Request execute Read Local AMP Info HCI
command to AMP controller with function to be executed upon receiving
command complete event. Function will handle A2MP Get Info Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    2 ++
 net/bluetooth/a2mp.c         |   57 ++++++++++++++++++++++++++++++------------
 net/bluetooth/hci_event.c    |    6 ++++-
 3 files changed, 48 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 316c1c8..af59468 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -132,5 +132,7 @@ int amp_mgr_put(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);
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3f93060..0e97b3b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -41,8 +41,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
@@ -185,7 +184,6 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
 {
 	struct a2mp_info_req *req  = (void *) skb->data;
-	struct a2mp_info_rsp rsp;
 	struct hci_dev *hdev;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
@@ -193,23 +191,23 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	BT_DBG("id %d", req->id);
 
-	rsp.id = req->id;
-	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
-
 	hdev = hci_dev_get(req->id);
-	if (hdev && hdev->amp_type != HCI_BREDR) {
-		rsp.status = 0;
-		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
-		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
-		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
-		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
-		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	if (!hdev) {
+		struct a2mp_info_rsp rsp;
+
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
+			  &rsp);
 	}
 
-	if (hdev)
-		hci_dev_put(hdev);
+	if (hdev->dev_type != HCI_BREDR) {
+		mgr->state = READ_LOC_AMP_INFO;
+		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
+	}
 
-	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+	hci_dev_put(hdev);
 
 	skb_pull(skb, sizeof(*req));
 	return 0;
@@ -599,3 +597,30 @@ struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
 
 	return NULL;
 }
+
+void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
+{
+	struct amp_mgr *mgr;
+	struct a2mp_info_rsp rsp;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	rsp.id = hdev->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	if (hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
+	amp_mgr_put(mgr);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 6f2b519..d05b8fe 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -30,6 +30,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -846,7 +847,7 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto a2mp_rsp;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -860,6 +861,9 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+a2mp_rsp:
+	a2mp_send_getinfo_rsp(hdev);
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[PATCHv7 04/19] Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Request execute Read Local AMP Assoc
HCI command to AMP controller. If the AMP Assoc data is larger then it
can fit to HCI event only fragment is read. When all fragments are read
send A2MP Get AMP Assoc Response.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h     |    2 ++
 include/net/bluetooth/amp.h      |   21 ++++++++++++++
 include/net/bluetooth/hci.h      |    2 ++
 include/net/bluetooth/hci_core.h |    8 ++++++
 net/bluetooth/Makefile           |    2 +-
 net/bluetooth/a2mp.c             |   56 ++++++++++++++++++++++++++++++++++----
 net/bluetooth/amp.c              |   45 ++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   41 ++++++++++++++++++++++++++++
 8 files changed, 171 insertions(+), 6 deletions(-)
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/amp.c

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index af59468..d547671 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -21,6 +21,7 @@
 
 enum amp_mgr_state {
 	READ_LOC_AMP_INFO,
+	READ_LOC_AMP_ASSOC,
 };
 
 struct amp_mgr {
@@ -134,5 +135,6 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
 void a2mp_send_getinfo_rsp(struct hci_dev *hdev);
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status);
 
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..e861675
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,21 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+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);
+
+#endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 4be26abf6d..ccc08e5 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -33,6 +33,8 @@
 #define HCI_LINK_KEY_SIZE	16
 #define HCI_AMP_LINK_KEY_SIZE	(2 * HCI_LINK_KEY_SIZE)
 
+#define HCI_MAX_AMP_ASSOC_SIZE	672
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index e7d4546..b1c7d06 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -124,6 +124,12 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -177,6 +183,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0e97b3b..7140061 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Global AMP Manager list */
 LIST_HEAD(amp_mgr_list);
@@ -218,26 +219,37 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 {
 	struct a2mp_amp_assoc_req *req = (void *) skb->data;
 	struct hci_dev *hdev;
+	struct amp_mgr *tmp;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
 
 	BT_DBG("id %d", req->id);
 
+	/* Make sure that other request is not processed */
+	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+
 	hdev = hci_dev_get(req->id);
-	if (!hdev || hdev->amp_type == HCI_BREDR) {
+	if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
 		struct a2mp_amp_assoc_rsp rsp;
 		rsp.id = req->id;
-		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		if (tmp) {
+			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
+			amp_mgr_put(tmp);
+		} else {
+			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		}
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
 			  &rsp);
-		goto clean;
+
+		goto done;
 	}
 
-	/* Placeholder for HCI Read AMP Assoc */
+	amp_read_loc_assoc(hdev, mgr);
 
-clean:
+done:
 	if (hdev)
 		hci_dev_put(hdev);
 
@@ -624,3 +636,37 @@ void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
 	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
 	amp_mgr_put(mgr);
 }
+
+void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
+{
+	struct amp_mgr *mgr;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
+	if (!mgr)
+		return;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp) {
+		amp_mgr_put(mgr);
+		return;
+	}
+
+	rsp->id = hdev->id;
+
+	if (status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+	} else {
+		rsp->status = A2MP_STATUS_SUCCESS;
+		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+	}
+
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	amp_mgr_put(mgr);
+	kfree(rsp);
+}
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..2d4e79e
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,45 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s handle %d", hdev->name, phy_handle);
+
+	cp.phy_handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	mgr->state = READ_LOC_AMP_ASSOC;
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index d05b8fe..0a9e720 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -31,6 +31,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/mgmt.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -866,6 +867,42 @@ a2mp_rsp:
 	a2mp_send_getinfo_rsp(hdev);
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto a2mp_rsp;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		BT_DBG("frag_len %d rem_len %d", frag_len, rem_len);
+
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->phy_handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+a2mp_rsp:
+	/* Send A2MP Rsp when all fragments are received */
+	a2mp_send_getampassoc_rsp(hdev, rp->status);
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 					  struct sk_buff *skb)
 {
@@ -2317,6 +2354,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

[PATCHv7 05/19] Bluetooth: A2MP: Process Discover Response

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Discover Response send A2MP Get Info Request
for each AMP controller in the discovery list.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   62 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 61 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7140061..f04c441 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -67,6 +67,14 @@ 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)
+{
+	if (++mgr->ident == 0)
+		mgr->ident = 1;
+
+	return mgr->ident;
+}
+
 static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
 {
 	cl->id = 0;
@@ -165,6 +173,55 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_cl *cl;
+	u16 ext_feat;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	len -= sizeof(*rsp);
+	skb_pull(skb, sizeof(*rsp));
+
+	ext_feat = le16_to_cpu(rsp->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	cl = (void *) skb->data;
+	while (len >= sizeof(*cl)) {
+		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
+		       cl->status);
+
+		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
+			struct a2mp_info_req req;
+
+			req.id = cl->id;
+			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
+				  sizeof(req), &req);
+		}
+
+		len -= sizeof(*cl);
+		cl = (void *) skb_pull(skb, sizeof(*cl));
+	}
+
+	return 0;
+}
+
 static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 			      struct a2mp_cmd *hdr)
 {
@@ -391,8 +448,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discphyslink_req(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
+			err = a2mp_discover_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv7 06/19] Bluetooth: AMP: Physical link struct and heplers

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Define physical link structures. Physical links are represented by
hci_conn structure. For BR/EDR we use type ACL_LINK and for AMP
we use AMP_LINK.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    3 +++
 include/net/bluetooth/hci.h      |    1 +
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/amp.c              |   29 +++++++++++++++++++++++++++++
 4 files changed, 34 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index e861675..3414dfd 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,6 +14,9 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id);
+
 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);
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index ccc08e5..77b6a19 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -207,6 +207,7 @@ enum {
 #define ESCO_LINK	0x02
 /* Low Energy links do not have defined link type. Use invented one */
 #define LE_LINK		0x80
+#define AMP_LINK	0x81
 
 /* LMP features */
 #define LMP_3SLOT	0x01
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b1c7d06..464eae3 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -318,6 +318,7 @@ struct hci_conn {
 
 	__u8		remote_cap;
 	__u8		remote_auth;
+	__u8		remote_id;
 	bool		flush_key;
 
 	unsigned int	sent;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 2d4e79e..50a7b2f 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,35 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+/* Physical Link interface */
+static u8 __next_handle(struct amp_mgr *mgr)
+{
+	if (++mgr->handle == 0)
+		mgr->handle = 1;
+
+	return mgr->handle;
+}
+
+struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
+			     u8 remote_id)
+{
+	bdaddr_t *dst = mgr->l2cap_conn->dst;
+	struct hci_conn *hcon;
+
+	hcon = hci_conn_add(hdev, AMP_LINK, dst);
+	if (!hcon)
+		return NULL;
+
+	hcon->state = BT_CONNECT;
+	hcon->out = true;
+	hcon->attempt++;
+	hcon->handle = __next_handle(mgr);
+	hcon->remote_id = remote_id;
+	hcon->amp_mgr = mgr;
+
+	return hcon;
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv7 07/19] Bluetooth: AMP: Remote AMP ctrl definitions

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Create remote AMP controllers structure. It is used to keep information
about discovered remote AMP controllers by A2MP protocol.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    3 ++
 include/net/bluetooth/amp.h  |   15 ++++++++
 net/bluetooth/a2mp.c         |    5 +++
 net/bluetooth/amp.c          |   79 ++++++++++++++++++++++++++++++++++++++++++
 4 files changed, 102 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index d547671..99c7389 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -33,6 +33,9 @@ struct amp_mgr {
 	__u8			handle;
 	enum amp_mgr_state	state;
 	unsigned long		flags;
+
+	struct list_head	amp_ctrls;
+	struct mutex		amp_ctrls_lock;
 };
 
 struct a2mp_cmd {
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 3414dfd..f57854f 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -14,6 +14,21 @@
 #ifndef __AMP_H
 #define __AMP_H
 
+struct amp_ctrl {
+	struct list_head	list;
+	struct kref		kref;
+	__u8			id;
+	__u16			assoc_len_so_far;
+	__u16			assoc_rem_len;
+	__u16			assoc_len;
+	__u8			*assoc;
+};
+
+int amp_ctrl_put(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);
+
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f04c441..35e188c 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -594,6 +594,7 @@ static void amp_mgr_destroy(struct kref *kref)
 	list_del(&mgr->list);
 	mutex_unlock(&amp_mgr_list_lock);
 
+	amp_ctrl_list_flush(mgr);
 	kfree(mgr);
 }
 
@@ -630,6 +631,10 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	kref_init(&mgr->kref);
 
+	/* Remote AMP ctrl list initialization */
+	INIT_LIST_HEAD(&mgr->amp_ctrls);
+	mutex_init(&mgr->amp_ctrls_lock);
+
 	mutex_lock(&amp_mgr_list_lock);
 	list_add(&mgr->list, &amp_mgr_list);
 	mutex_unlock(&amp_mgr_list_lock);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 50a7b2f..8ef912c 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -17,6 +17,85 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
 
+/* Remote AMP Controllers interface */
+static void amp_ctrl_get(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	kref_get(&ctrl->kref);
+}
+
+static void amp_ctrl_destroy(struct kref *kref)
+{
+	struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref);
+
+	BT_DBG("ctrl %p", ctrl);
+
+	kfree(ctrl->assoc);
+	kfree(ctrl);
+}
+
+int amp_ctrl_put(struct amp_ctrl *ctrl)
+{
+	BT_DBG("ctrl %p orig refcnt %d", ctrl,
+	       atomic_read(&ctrl->kref.refcount));
+
+	return kref_put(&ctrl->kref, &amp_ctrl_destroy);
+}
+
+struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (!ctrl)
+		return NULL;
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_add(&ctrl->list, &mgr->amp_ctrls);
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	kref_init(&ctrl->kref);
+
+	BT_DBG("mgr %p ctrl %p", mgr, ctrl);
+
+	return ctrl;
+}
+
+void amp_ctrl_list_flush(struct amp_mgr *mgr)
+{
+	struct amp_ctrl *ctrl, *n;
+
+	BT_DBG("mgr %p", mgr);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) {
+		list_del(&ctrl->list);
+		amp_ctrl_put(ctrl);
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+}
+
+struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id)
+{
+	struct amp_ctrl *ctrl;
+
+	BT_DBG("mgr %p id %d", mgr, id);
+
+	mutex_lock(&mgr->amp_ctrls_lock);
+	list_for_each_entry(ctrl, &mgr->amp_ctrls, list) {
+		if (ctrl->id == id) {
+			amp_ctrl_get(ctrl);
+			mutex_unlock(&mgr->amp_ctrls_lock);
+			return ctrl;
+		}
+	}
+	mutex_unlock(&mgr->amp_ctrls_lock);
+
+	return NULL;
+}
+
 /* Physical Link interface */
 static u8 __next_handle(struct amp_mgr *mgr)
 {
-- 
1.7.9.5

[PATCHv7 08/19] Bluetooth: AMP: Handle create / disc phylink req

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Use hci_conn structure to keep track about AMP physical connections.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   21 +++++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 35e188c..0125417 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -321,6 +321,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;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -338,7 +339,14 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 
 	/* TODO process physlink create */
 
-	rsp.status = A2MP_STATUS_SUCCESS;
+	hcon = phylink_add(hdev, mgr, req->local_id);
+	if (hcon) {
+		BT_DBG("hcon %p", hcon);
+
+		rsp.status = A2MP_STATUS_SUCCESS;
+	} else {
+		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
+	}
 
 send_rsp:
 	if (hdev)
@@ -357,6 +365,7 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	struct a2mp_physlink_req *req = (void *) skb->data;
 	struct a2mp_physlink_rsp rsp;
 	struct hci_dev *hdev;
+	struct hci_conn *hcon;
 
 	if (le16_to_cpu(hdr->len) < sizeof(*req))
 		return -EINVAL;
@@ -367,14 +376,22 @@ static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	rsp.remote_id = req->local_id;
 	rsp.status = A2MP_STATUS_SUCCESS;
 
-	hdev = hci_dev_get(req->local_id);
+	hdev = hci_dev_get(req->remote_id);
 	if (!hdev) {
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 		goto send_rsp;
 	}
 
+	hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst);
+	if (!hcon) {
+		BT_ERR("No phys link exist");
+		rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
+		goto clean;
+	}
+
 	/* TODO Disconnect Phys Link here */
 
+clean:
 	hci_dev_put(hdev);
 
 send_rsp:
-- 
1.7.9.5

[PATCHv7 09/19] Bluetooth: A2MP: Process A2MP Getinfo Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Process A2MP Getinfo Response, send Get AMP Assoc Req.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   34 +++++++++++++++++++++++++++++++++-
 1 file changed, 33 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0125417..594df96 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -271,6 +271,35 @@ static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
+	struct a2mp_amp_assoc_req req;
+	struct amp_ctrl *ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
+
+	if (rsp->status)
+		return -EINVAL;
+
+	ctrl = amp_ctrl_add(mgr);
+	if (!ctrl)
+		return -ENOMEM;
+
+	ctrl->id = rsp->id;
+
+	req.id = rsp->id;
+	a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
+		  &req);
+
+	skb_pull(skb, sizeof(*rsp));
+	return 0;
+}
+
 static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				struct a2mp_cmd *hdr)
 {
@@ -469,8 +498,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_discover_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETINFO_RSP:
+			err = a2mp_getinfo_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
-- 
1.7.9.5

[PATCHv7 10/19] Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response save assoc data to remote
AMP controller list and prepare for creating physical link.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/a2mp.c |   60 +++++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 59 insertions(+), 1 deletion(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 594df96..d0fde05 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -343,6 +343,61 @@ done:
 	return 0;
 }
 
+static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+				struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
+	u16 len = le16_to_cpu(hdr->len);
+	struct hci_dev *hdev;
+	struct amp_ctrl *ctrl;
+	struct hci_conn *hcon;
+
+	if (len < sizeof(*rsp))
+		return -EINVAL;
+
+	BT_DBG("id %d status 0x%2.2x assoc len %u", rsp->id, rsp->status,
+	       len - sizeof(*rsp));
+
+	if (rsp->status)
+		return -EINVAL;
+
+	/* Save remote ASSOC data */
+	ctrl = amp_ctrl_lookup(mgr, rsp->id);
+	if (ctrl) {
+		u8 *assoc, assoc_len = len - sizeof(*rsp);
+
+		assoc = kzalloc(assoc_len, GFP_KERNEL);
+		if (!assoc) {
+			amp_ctrl_put(ctrl);
+			return -ENOMEM;
+		}
+
+		memcpy(assoc, rsp->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);
+	}
+
+	/* Create Phys Link */
+	hdev = hci_dev_get(rsp->id);
+	if (!hdev)
+		return -EINVAL;
+
+	hcon = phylink_add(hdev, mgr, rsp->id);
+	if (!hcon)
+		goto done;
+
+	BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
+
+done:
+	hci_dev_put(hdev);
+	skb_pull(skb, len);
+	return 0;
+}
+
 static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
 {
@@ -502,8 +557,11 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 			err = a2mp_getinfo_rsp(mgr, skb, hdr);
 			break;
 
-		case A2MP_CHANGE_RSP:
 		case A2MP_GETAMPASSOC_RSP:
+			err = a2mp_getampassoc_rsp(mgr, skb, hdr);
+			break;
+
+		case A2MP_CHANGE_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
 			err = a2mp_cmd_rsp(mgr, skb, hdr);
-- 
1.7.9.5

[PATCHv7 11/19] Bluetooth: Choose connection based on capabilities

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Choose which L2CAP connection to establish by checking support
for HS and remote side supported features.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h  |    2 ++
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   34 +++++++++++++++++++++++++++++-----
 net/bluetooth/l2cap_core.c    |   33 ++++++++++++++++++++++++++++-----
 4 files changed, 60 insertions(+), 10 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 99c7389..9fda7c9 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -28,6 +28,7 @@ struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
+	struct l2cap_chan	*bredr_chan;
 	struct kref		kref;
 	__u8			ident;
 	__u8			handle;
@@ -137,6 +138,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 struct amp_mgr *amp_mgr_lookup_by_state(u8 state);
 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);
 
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 7ed8e35..aba830f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -767,6 +767,7 @@ int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d0fde05..93adaad 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -638,7 +638,7 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.ready = l2cap_chan_no_ready,
 };
 
-static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
 {
 	struct l2cap_chan *chan;
 	int err;
@@ -673,7 +673,10 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	chan->conf_state = 0;
 
-	l2cap_chan_add(conn, chan);
+	if (locked)
+		__l2cap_chan_add(conn, chan);
+	else
+		l2cap_chan_add(conn, chan);
 
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
@@ -712,7 +715,7 @@ int amp_mgr_put(struct amp_mgr *mgr)
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
 
-static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
 {
 	struct amp_mgr *mgr;
 	struct l2cap_chan *chan;
@@ -725,7 +728,7 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	mgr->l2cap_conn = conn;
 
-	chan = a2mp_chan_open(conn);
+	chan = a2mp_chan_open(conn, locked);
 	if (!chan) {
 		kfree(mgr);
 		return NULL;
@@ -754,7 +757,7 @@ struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 {
 	struct amp_mgr *mgr;
 
-	mgr = amp_mgr_create(conn);
+	mgr = amp_mgr_create(conn, false);
 	if (!mgr) {
 		BT_ERR("Could not create AMP manager");
 		return NULL;
@@ -842,3 +845,24 @@ void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
 	amp_mgr_put(mgr);
 	kfree(rsp);
 }
+
+void a2mp_discover_amp(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+	struct amp_mgr *mgr = conn->hcon->amp_mgr;
+	struct a2mp_discov_req req;
+
+	BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
+
+	if (!mgr) {
+		mgr = amp_mgr_create(conn, true);
+		if (!mgr)
+			return;
+	}
+
+	mgr->bredr_chan = chan;
+
+	req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	req.ext_feat = 0;
+	a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 0873345..0dc3a98 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -455,7 +455,7 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan)
 	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
 }
 
-static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
 	       __le16_to_cpu(chan->psm), chan->dcid);
@@ -946,6 +946,18 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan)
 	return !test_bit(CONF_CONNECT_PEND, &chan->conf_state);
 }
 
+static bool __amp_capable(struct l2cap_chan *chan)
+{
+	struct l2cap_conn *conn = chan->conn;
+
+	if (enable_hs &&
+	    chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED &&
+	    conn->fixed_chan_mask & L2CAP_FC_A2MP)
+		return true;
+	else
+		return false;
+}
+
 static void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -972,6 +984,16 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->ops->ready(chan);
 }
 
+static void l2cap_start_connection(struct l2cap_chan *chan)
+{
+	if (__amp_capable(chan)) {
+		BT_DBG("chan %p AMP capable: discover AMPs", chan);
+		a2mp_discover_amp(chan);
+	} else {
+		l2cap_send_conn_req(chan);
+	}
+}
+
 static void l2cap_do_start(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
@@ -986,8 +1008,9 @@ static void l2cap_do_start(struct l2cap_chan *chan)
 			return;
 
 		if (l2cap_chan_check_security(chan) &&
-				__l2cap_no_conn_pending(chan))
-			l2cap_send_conn_req(chan);
+				__l2cap_no_conn_pending(chan)) {
+			l2cap_start_connection(chan);
+		}
 	} else {
 		struct l2cap_info_req req;
 		req.type = __constant_cpu_to_le16(L2CAP_IT_FEAT_MASK);
@@ -1082,7 +1105,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 				continue;
 			}
 
-			l2cap_send_conn_req(chan);
+			l2cap_start_connection(chan);
 
 		} else if (chan->state == BT_CONNECT2) {
 			struct l2cap_conn_rsp rsp;
@@ -5462,7 +5485,7 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 
 		if (chan->state == BT_CONNECT) {
 			if (!status) {
-				l2cap_send_conn_req(chan);
+				l2cap_start_connection(chan);
 			} else {
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 			}
-- 
1.7.9.5

[PATCHv7 12/19] Bluetooth: Add function to derive AMP key using hmac

[Andrei Emeltchenko]

From: Dmitry Kasatkin <dmitry.kasatkin@intel.com>

hmac(sha256) will be used for AMP key generation.

Signed-off-by: Dmitry Kasatkin <dmitry.kasatkin@intel.com>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 net/bluetooth/amp.c |   36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 8ef912c..ea4d5ff 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/amp.h>
+#include <crypto/hash.h>
 
 /* Remote AMP Controllers interface */
 static void amp_ctrl_get(struct amp_ctrl *ctrl)
@@ -125,6 +126,41 @@ struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 	return hcon;
 }
 
+/* AMP crypto key generation interface */
+static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
+{
+	int ret = 0;
+	struct crypto_shash *tfm;
+
+	if (!ksize)
+		return -EINVAL;
+
+	tfm = crypto_alloc_shash("hmac(sha256)", 0, 0);
+	if (IS_ERR(tfm)) {
+		BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm));
+		return PTR_ERR(tfm);
+	}
+
+	ret = crypto_shash_setkey(tfm, key, ksize);
+	if (ret) {
+		BT_DBG("crypto_ahash_setkey failed: err %d", ret);
+	} else {
+		struct {
+			struct shash_desc shash;
+			char ctx[crypto_shash_descsize(tfm)];
+		} desc;
+
+		desc.shash.tfm = tfm;
+		desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+
+		ret = crypto_shash_digest(&desc.shash, plaintext, psize,
+					  output);
+	}
+
+	crypto_free_shash(tfm);
+	return ret;
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv7 13/19] Bluetooth: AMP: Add AMP key calculation

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

Function calculates AMP keys using hmac_sha256 helper. Calculated keys
are Generic AMP Link Key (gamp) and Dedicated AMP Link Key with
keyID "802b" for 802.11 PAL.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    2 ++
 net/bluetooth/Kconfig       |    1 +
 net/bluetooth/amp.c         |   45 +++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 48 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index f57854f..763b463 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -32,6 +32,8 @@ void amp_ctrl_list_flush(struct amp_mgr *mgr);
 struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr,
 			     u8 remote_id);
 
+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);
diff --git a/net/bluetooth/Kconfig b/net/bluetooth/Kconfig
index 3537d38..1c11d0d 100644
--- a/net/bluetooth/Kconfig
+++ b/net/bluetooth/Kconfig
@@ -11,6 +11,7 @@ menuconfig BT
 	select CRYPTO_BLKCIPHER
 	select CRYPTO_AES
 	select CRYPTO_ECB
+	select CRYPTO_SHA256
 	help
 	  Bluetooth is low-cost, low-power, short-range wireless technology.
 	  It was designed as a replacement for cables and other short-range
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index ea4d5ff..67bc2c2f 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -161,6 +161,51 @@ static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output)
 	return ret;
 }
 
+int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type)
+{
+	struct hci_dev *hdev = conn->hdev;
+	struct link_key *key;
+	u8 keybuf[HCI_AMP_LINK_KEY_SIZE];
+	u8 gamp_key[HCI_AMP_LINK_KEY_SIZE];
+	int err;
+
+	if (!hci_conn_check_link_mode(conn))
+		return -EACCES;
+
+	BT_DBG("conn %p key_type %d", conn, conn->key_type);
+
+	/* Legacy key */
+	if (conn->key_type < 3) {
+		BT_ERR("Legacy key type %d", conn->key_type);
+		return -EACCES;
+	}
+
+	*type = conn->key_type;
+	*len = HCI_AMP_LINK_KEY_SIZE;
+
+	key = hci_find_link_key(hdev, &conn->dst);
+
+	/* BR/EDR Link Key concatenated together with itself */
+	memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE);
+	memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE);
+
+	/* Derive Generic AMP Link Key (gamp) */
+	err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key);
+	if (err) {
+		BT_ERR("Could not derive Generic AMP Key: err %d", err);
+		return err;
+	}
+
+	if (conn->key_type == HCI_LK_DEBUG_COMBINATION) {
+		BT_DBG("Use Generic AMP Key (gamp)");
+		memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE);
+		return err;
+	}
+
+	/* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */
+	return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data);
+}
+
 void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
 {
 	struct hci_cp_read_local_amp_assoc cp;
-- 
1.7.9.5

[PATCHv7 14/19] Bluetooth: AMP: Create Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Get AMP Assoc Response execute HCI Create Physical
Link to AMP controller. Define function which will run when receiving
HCI Command Status.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    2 ++
 net/bluetooth/a2mp.c        |    2 ++
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 net/bluetooth/hci_event.c   |    9 +++++++++
 4 files changed, 32 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 763b463..cadb3d0 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -37,5 +37,7 @@ 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_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 
 #endif /* __AMP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 93adaad..773e8fc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -392,6 +392,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);
 
+	amp_create_phylink(hdev, mgr, hcon);
+
 done:
 	hci_dev_put(hdev);
 	skb_pull(skb, len);
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 67bc2c2f..657ec73 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -232,3 +232,22 @@ void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
 	mgr->state = READ_LOC_AMP_ASSOC;
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
 }
+
+void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0a9e720..e7d2db4 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1697,6 +1697,11 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
 }
 
+static void hci_cs_create_phylink(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);
@@ -2511,6 +2516,10 @@ static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cs_le_start_enc(hdev, ev->status);
 		break;
 
+	case HCI_OP_CREATE_PHY_LINK:
+		hci_cs_create_phylink(hdev, ev->status);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv7 15/19] Bluetooth: AMP: Write remote AMP Assoc

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status after HCI Create Physical Link
execute HCI Write Remote AMP Assoc command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h      |    2 +
 include/net/bluetooth/hci_core.h |    2 +
 net/bluetooth/amp.c              |   80 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_event.c        |   29 ++++++++++++++
 4 files changed, 113 insertions(+)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index cadb3d0..8f80329 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -39,5 +39,7 @@ 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_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);
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 464eae3..ea1f934 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -127,6 +127,8 @@ struct le_scan_params {
 struct amp_assoc {
 	__u16	len;
 	__u16	offset;
+	__u16	rem_len;
+	__u16	len_so_far;
 	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
 };
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 657ec73..5895ad0 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -233,6 +233,86 @@ 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);
 }
 
+
+/* Write AMP Assoc data fragments, returns true with last fragment written*/
+static bool amp_write_rem_assoc_frag(struct hci_dev *hdev,
+				     struct hci_conn *hcon)
+{
+	struct hci_cp_write_remote_amp_assoc *cp;
+	struct amp_mgr *mgr = hcon->amp_mgr;
+	struct amp_ctrl *ctrl;
+	u16 frag_len, len;
+
+	ctrl = amp_ctrl_lookup(mgr, hcon->remote_id);
+	if (!ctrl)
+		return false;
+
+	if (!ctrl->assoc_rem_len) {
+		BT_DBG("all fragments are written");
+		ctrl->assoc_rem_len = ctrl->assoc_len;
+		ctrl->assoc_len_so_far = 0;
+
+		amp_ctrl_put(ctrl);
+		return true;
+	}
+
+	frag_len = min_t(u16, 248, ctrl->assoc_rem_len);
+	len = frag_len + sizeof(*cp);
+
+	cp = kzalloc(len, GFP_KERNEL);
+	if (!cp) {
+		amp_ctrl_put(ctrl);
+		return false;
+	}
+
+	BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u",
+	       hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len);
+
+	cp->phy_handle = hcon->handle;
+	cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far);
+	cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len);
+	memcpy(cp->frag, ctrl->assoc, frag_len);
+
+	ctrl->assoc_len_so_far += frag_len;
+	ctrl->assoc_rem_len -= frag_len;
+
+	amp_ctrl_put(ctrl);
+
+	hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp);
+
+	kfree(cp);
+
+	return false;
+}
+
+void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
+void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle)
+{
+	struct hci_conn *hcon;
+
+	BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle);
+
+	hcon = hci_conn_hash_lookup_handle(hdev, handle);
+	if (!hcon)
+		return;
+
+	BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon);
+
+	amp_write_rem_assoc_frag(hdev, hcon);
+}
+
 void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 			struct hci_conn *hcon)
 {
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index e7d2db4..2c7e27a 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1215,6 +1215,20 @@ static void hci_cc_write_le_host_supported(struct hci_dev *hdev,
 	hci_req_complete(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED, status);
 }
 
+static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev,
+					  struct sk_buff *skb)
+{
+	struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data;
+
+	BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x",
+	       hdev->name, rp->status, rp->phy_handle);
+
+	if (rp->status)
+		return;
+
+	amp_write_rem_assoc_continue(hdev, rp->phy_handle);
+}
+
 static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status)
 {
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
@@ -1699,7 +1713,18 @@ static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status)
 
 static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status)
 {
+	struct hci_cp_create_phy_link *cp;
+
 	BT_DBG("%s status 0x%2.2x", hdev->name, status);
+
+	if (status)
+		return;
+
+	cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_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)
@@ -2435,6 +2460,10 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		hci_cc_write_le_host_supported(hdev, skb);
 		break;
 
+	case HCI_OP_WRITE_REMOTE_AMP_ASSOC:
+		hci_cc_write_remote_amp_assoc(hdev, skb);
+		break;
+
 	default:
 		BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode);
 		break;
-- 
1.7.9.5

[PATCHv7 16/19] Bluetooth: A2MP: Add fallback to normal l2cap init sequence

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When there is no remote AMP controller found fallback to normal
L2CAP sequence.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    1 +
 net/bluetooth/a2mp.c          |   28 ++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 30 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index aba830f..0967f9e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -769,5 +769,6 @@ int l2cap_ertm_init(struct l2cap_chan *chan);
 void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void l2cap_chan_del(struct l2cap_chan *chan, int err);
+void l2cap_send_conn_req(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 773e8fc..28d1246 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -180,6 +180,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 	u16 len = le16_to_cpu(hdr->len);
 	struct a2mp_cl *cl;
 	u16 ext_feat;
+	bool found = false;
 
 	if (len < sizeof(*rsp))
 		return -EINVAL;
@@ -210,6 +211,7 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
 			struct a2mp_info_req req;
 
+			found = true;
 			req.id = cl->id;
 			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
 				  sizeof(req), &req);
@@ -219,6 +221,32 @@ static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
 		cl = (void *) skb_pull(skb, sizeof(*cl));
 	}
 
+	/* Fall back to L2CAP init sequence */
+	if (!found) {
+		struct l2cap_conn *conn = mgr->l2cap_conn;
+		struct l2cap_chan *chan;
+
+		mutex_lock(&conn->chan_lock);
+
+		list_for_each_entry(chan, &conn->chan_l, list) {
+
+			BT_DBG("chan %p state %s", chan,
+			       state_to_string(chan->state));
+
+			if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+				continue;
+
+			l2cap_chan_lock(chan);
+
+			if (chan->state == BT_CONNECT)
+				l2cap_send_conn_req(chan);
+
+			l2cap_chan_unlock(chan);
+		}
+
+		mutex_unlock(&conn->chan_lock);
+	}
+
 	return 0;
 }
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 0dc3a98..b4e707b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -958,7 +958,7 @@ static bool __amp_capable(struct l2cap_chan *chan)
 		return false;
 }
 
-static void l2cap_send_conn_req(struct l2cap_chan *chan)
+void l2cap_send_conn_req(struct l2cap_chan *chan)
 {
 	struct l2cap_conn *conn = chan->conn;
 	struct l2cap_conn_req req;
-- 
1.7.9.5

[PATCHv7 17/19] Bluetooth: AMP: Process Chan Selected event

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

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>
---
 include/net/bluetooth/a2mp.h  |    3 +++
 include/net/bluetooth/amp.h   |    2 ++
 include/net/bluetooth/l2cap.h |    2 ++
 net/bluetooth/a2mp.c          |   41 ++++++++++++++++++++++++++++++++++++++++-
 net/bluetooth/amp.c           |   15 +++++++++++++++
 net/bluetooth/hci_event.c     |   21 +++++++++++++++++++++
 6 files changed, 83 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 9fda7c9..e776ab2 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 8f80329..70496c0 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 0967f9e..ab58b81 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 28d1246..375a67f 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 5895ad0..4f7b264 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 2c7e27a..8f1eccf 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,
@@ -3640,6 +3641,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;
@@ -3804,6 +3821,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;
-- 
1.7.9.5

[PATCHv7 18/19] Bluetooth: AMP: Accept Physical Link

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving A2MP Create Physical Link message execute HCI
Accept Physical Link command to AMP controller.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    2 ++
 net/bluetooth/a2mp.c        |    5 +----
 net/bluetooth/amp.c         |   19 +++++++++++++++++++
 3 files changed, 22 insertions(+), 4 deletions(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 70496c0..1b06d7b 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -41,6 +41,8 @@ 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_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon);
 void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle);
 void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 375a67f..dbfdbbb 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -453,12 +453,9 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		goto send_rsp;
 	}
 
-	/* TODO process physlink create */
-
 	hcon = phylink_add(hdev, mgr, req->local_id);
 	if (hcon) {
-		BT_DBG("hcon %p", hcon);
-
+		amp_accept_phylink(hdev, mgr, hcon);
 		rsp.status = A2MP_STATUS_SUCCESS;
 	} else {
 		rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index 4f7b264..845e430 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -346,3 +346,22 @@ void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
 
 	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
 }
+
+void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr,
+			struct hci_conn *hcon)
+{
+	struct hci_cp_accept_phy_link cp;
+
+	cp.phy_handle = hcon->handle;
+
+	BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon,
+	       hcon->handle);
+
+	if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len,
+			    &cp.key_type)) {
+		BT_DBG("Cannot create link key");
+		return;
+	}
+
+	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
-- 
1.7.9.5

[PATCHv7 19/19] Bluetooth: AMP: Handle Accept phylink command status evt

[Andrei Emeltchenko]

From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

When receiving HCI Command Status event for Accept Physical Link
execute HCI Write Remote AMP Assoc with data saved from A2MP Create
Physical Link Request.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/amp.h |    1 +
 net/bluetooth/a2mp.c        |   32 ++++++++++++++++++++++++++++++++
 net/bluetooth/amp.c         |    2 +-
 net/bluetooth/hci_event.c   |   20 ++++++++++++++++++++
 4 files changed, 54 insertions(+), 1 deletion(-)

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 1b06d7b..b1e5490 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 dbfdbbb..47565d2 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 845e430..5dab2d1 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -19,7 +19,7 @@
 #include <crypto/hash.h>
 
 /* 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 8f1eccf..65f66bf 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -1728,6 +1728,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);
@@ -2550,6 +2566,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;
-- 
1.7.9.5

Re: [PATCHv7 00/19] Bluetooth: Create AMP physical link

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-09-27 17:26:05 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> This set of patches enhances A2MP protocol and creates physical
> link between AMP controllers. This is further iteration towards
> Bluetooth High Speed.
> 
> Changes:
> 	* p7: Fix ctrl_lookup, add phylink accept functions.
> 	* p6: Refactoring: moving code from pal.[ch] to amp.[ch]
> 	* p5: Fix issues reported by Mat in mailing list review
> 	* p4: Rebased against recent bluetooth-next, minor fixes
> 	* p3: Use hci_conn for representing physical link(type AMP_LINK) instead of
> 	struct phy_link, refactoring.
> 	* p2: Remove HCI callbacks and use amp_mgr global list, refactor code.
> 	* p1: Fixed locking issues, added basic logical link preparation.
> 	* v3: Remove workqueue from callback processing; change callback functions
> 	names according to reviewers recommendations; create global amp_mgr_list to
> 	have lookup to amp manager, physical and logical links so for those HCI events
> 	which might be identified by __handler__ we have lookup; remove extensive
> 	hexdump from gen_amp_key.
> 	* v2: Fix typos and bugs, add functionality: now physical connection
> 	might be established.
> 	* v1: Fix typos, change debug prints, refactor code for better
> 	splitting functionality.
> 
> Andrei Emeltchenko (18):
>   Bluetooth: Add HCI logical link cmds definitions
>   Bluetooth: A2MP: Create amp_mgr global list
>   Bluetooth: AMP: Use HCI cmd to Read AMP Info
>   Bluetooth: AMP: Use HCI cmd to Read Loc AMP Assoc
>   Bluetooth: A2MP: Process Discover Response
>   Bluetooth: AMP: Physical link struct and heplers
>   Bluetooth: AMP: Remote AMP ctrl definitions
>   Bluetooth: AMP: Handle create / disc phylink req
>   Bluetooth: A2MP: Process A2MP Getinfo Rsp
>   Bluetooth: A2MP: Process A2MP Get AMP Assoc Rsp
>   Bluetooth: Choose connection based on capabilities
>   Bluetooth: AMP: Add AMP key calculation
>   Bluetooth: AMP: Create Physical Link
>   Bluetooth: AMP: Write remote AMP Assoc
>   Bluetooth: A2MP: Add fallback to normal l2cap init sequence
>   Bluetooth: AMP: Process Chan Selected event
>   Bluetooth: AMP: Accept Physical Link
>   Bluetooth: AMP: Handle Accept phylink command status evt
> 
> Dmitry Kasatkin (1):
>   Bluetooth: Add function to derive AMP key using hmac

All 19 patches were applied to bluetooth-next. Thanks.

	Gustavo