[RFCv1 00/21] RFC Bluetooth A2MP implementation

[RFCv1 00/21] RFC Bluetooth A2MP implementation

[Emeltchenko Andrei]

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

Changes:
	* RFCv1 added refcnt to amp_mgr, fixed sleeping in atomic

Code based on Code Aurora (git://codeaurora.org/kernel/msm.git) and Atheros
(search for: [PATCH 2/3] Add a2mp protocol/AMP manager, by Atheros Linux BT3)
implementations (mostly Code Aurora). Main difference to the original code is
- code rebase against recent bluetooth-next
- rewritten way to send A2MP msgs
- handling fixed channel modifications
- I was trying to separate AMP vs A2MP name usage. AMP is used for
"Alternate MAC PHY" controller, A2MP for "AMP Manager Protocol".
- fixes related to PTS beta A2MP tests

Andrei Emeltchenko (21):
  Bluetooth: A2MP: Create A2MP socket
  Bluetooth: A2MP: Add sk_data_ready for A2MP socket
  Bluetooth: A2MP: Add sk_state_change for A2MP socket
  Bluetooth: A2MP: Build and Send msg helpers
  Bluetooth: A2MP: AMP Manager basic functions
  Bluetooth: A2MP: Remove AMP manager in state_change
  Bluetooth: A2MP: AMP manager initialization
  Bluetooth: A2MP: Definitions for A2MP commands
  Bluetooth: A2MP: Define A2MP status codes
  Bluetooth: A2MP: Process A2MP messages
  Bluetooth: A2MP: Process A2MP Command Reject
  Bluetooth: A2MP: Helper functions to count HCI devs
  Bluetooth: A2MP: Process A2MP Discover Request
  Bluetooth: A2MP: Process A2MP Change Notify
  Bluetooth: A2MP: Process A2MP Get Info Request
  Bluetooth: A2MP: Process A2MP Get AMP Assoc Request
  Bluetooth: A2MP: Process A2MP Create Physlink Request
  Bluetooth: A2MP: Process A2MP Disc Physlink Request
  Bluetooth: A2MP: Process A2MP Command Responses
  Bluetooth: A2MP: Handling fixed channel
  Bluetooth: A2MP: Manage incoming connections

 include/net/bluetooth/a2mp.h     |  139 ++++++++
 include/net/bluetooth/hci.h      |    2 +
 include/net/bluetooth/hci_core.h |   14 +
 include/net/bluetooth/l2cap.h    |    5 +
 net/bluetooth/a2mp.c             |  664 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c       |   87 ++++--
 6 files changed, 886 insertions(+), 25 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 net/bluetooth/a2mp.c

-- 
1.7.4.1

[RFCv1 01/21] Bluetooth: A2MP: Create A2MP socket

[Emeltchenko Andrei]

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

Create fixed channel A2MP socket

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    4 ++
 net/bluetooth/a2mp.c          |   94 +++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c    |    2 +-
 3 files changed, 99 insertions(+), 1 deletions(-)
 create mode 100644 net/bluetooth/a2mp.c

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9280bff..c0af77c 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -48,6 +48,8 @@
 #define L2CAP_CONN_TIMEOUT	(40000) /* 40 seconds */
 #define L2CAP_INFO_TIMEOUT	(4000)  /*  4 seconds */
 
+#define L2CAP_A2MP_DEFAULT_MTU		670
+
 /* L2CAP socket address */
 struct sockaddr_l2 {
 	sa_family_t	l2_family;
@@ -221,6 +223,7 @@ struct l2cap_conn_rsp {
 /* channel indentifier */
 #define L2CAP_CID_SIGNALING	0x0001
 #define L2CAP_CID_CONN_LESS	0x0002
+#define L2CAP_CID_A2MP		0x0003
 #define L2CAP_CID_LE_DATA	0x0004
 #define L2CAP_CID_LE_SIGNALING	0x0005
 #define L2CAP_CID_SMP		0x0006
@@ -807,5 +810,6 @@ int l2cap_chan_connect(struct l2cap_chan *chan);
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 								u32 priority);
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
+void l2cap_ertm_init(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..293a765
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,94 @@
+/*
+   Copyright (c) 2010-2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011 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_core.h>
+#include <net/bluetooth/l2cap.h>
+
+static void l2cap_fixed_channel_config(struct sock *sk)
+{
+	struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+
+	lock_sock(sk);
+
+	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
+	chan->fcs = L2CAP_FCS_CRC16;
+
+	chan->max_tx = 0xFF;
+	chan->remote_max_tx = chan->max_tx;
+
+	/* Send 10 packets without ack */
+	chan->tx_win = 10;
+	chan->remote_tx_win = chan->tx_win;
+
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
+	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
+
+	skb_queue_head_init(&chan->tx_q);
+
+	chan->mode = L2CAP_MODE_ERTM;
+	l2cap_ertm_init(chan);
+
+	release_sock(sk);
+}
+
+static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
+{
+	int err;
+	struct socket *sock;
+	struct sockaddr_l2 addr;
+	struct sock *sk;
+
+	err = sock_create_kern(PF_BLUETOOTH, SOCK_SEQPACKET,
+					BTPROTO_L2CAP, &sock);
+	if (err) {
+		BT_ERR("sock_create_kern failed %d", err);
+		return NULL;
+	}
+
+	sk = sock->sk;
+	memset(&addr, 0, sizeof(addr));
+	bacpy(&addr.l2_bdaddr, conn->src);
+	addr.l2_family = AF_BLUETOOTH;
+	addr.l2_cid = L2CAP_CID_A2MP;
+	err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr));
+	if (err) {
+		BT_ERR("kernel_bind failed %d", err);
+		sock_release(sock);
+		return NULL;
+	}
+
+	l2cap_fixed_channel_config(sk);
+
+	memset(&addr, 0, sizeof(addr));
+	bacpy(&addr.l2_bdaddr, conn->dst);
+	addr.l2_family = AF_BLUETOOTH;
+	addr.l2_cid = L2CAP_CID_A2MP;
+	err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr),
+							O_NONBLOCK);
+	if ((err == 0) || (err == -EINPROGRESS))
+		return sock;
+	else {
+		BT_ERR("kernel_connect failed %d", err);
+		sock_release(sock);
+		return NULL;
+	}
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index f850684..56b94c0 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1942,7 +1942,7 @@ static void l2cap_ack_timeout(unsigned long arg)
 	bh_unlock_sock(chan->sk);
 }
 
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+void l2cap_ertm_init(struct l2cap_chan *chan)
 {
 	struct sock *sk = chan->sk;
 
-- 
1.7.4.1

[RFCv1 02/21] Bluetooth: A2MP: Add sk_data_ready for A2MP socket

[Emeltchenko Andrei]

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


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |   24 ++++++++++++++++++++++++
 net/bluetooth/a2mp.c         |   39 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 63 insertions(+), 0 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
new file mode 100644
index 0000000..95dfaeb
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,24 @@
+/*
+   Copyright (c) 2010-2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011 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 __A2MP_H
+#define __A2MP_H
+
+struct a2mp_work_data_ready {
+	struct work_struct work;
+	struct sock *sk;
+	int bytes;
+};
+
+#endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 293a765..4475ec8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,44 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
+
+static struct workqueue_struct *a2mp_workqueue;
+
+static void data_ready_worker(struct work_struct *w)
+{
+	struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
+	struct sock *sk = work->sk;
+	struct sk_buff *skb;
+
+	BT_DBG("sk %p", sk);
+
+	/* skb_dequeue() is thread-safe */
+	while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
+		/* process a2mp here */
+		kfree_skb(skb);
+	}
+
+	sock_put(work->sk);
+	kfree(work);
+}
+
+static void a2mp_data_ready(struct sock *sk, int bytes)
+{
+	struct a2mp_work_data_ready *work;
+
+	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+	if (work) {
+		INIT_WORK(&work->work, data_ready_worker);
+		sock_hold(sk);
+		work->sk = sk;
+		work->bytes = bytes;
+		if (!queue_work(a2mp_workqueue, &work->work)) {
+			kfree(work);
+			sock_put(sk);
+		}
+	}
+}
 
 static void l2cap_fixed_channel_config(struct sock *sk)
 {
@@ -65,6 +103,7 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
 	}
 
 	sk = sock->sk;
+	sk->sk_data_ready = a2mp_data_ready;
 	memset(&addr, 0, sizeof(addr));
 	bacpy(&addr.l2_bdaddr, conn->src);
 	addr.l2_family = AF_BLUETOOTH;
-- 
1.7.4.1

[RFCv1 03/21] Bluetooth: A2MP: Add sk_state_change for A2MP socket

[Emeltchenko Andrei]

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


Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/a2mp.h |    5 +++++
 net/bluetooth/a2mp.c         |   28 ++++++++++++++++++++++++++++
 2 files changed, 33 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 95dfaeb..a3938f9 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -21,4 +21,9 @@ struct a2mp_work_data_ready {
 	int bytes;
 };
 
+struct a2mp_work_state_change {
+	struct work_struct work;
+	struct sock *sk;
+};
+
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4475ec8..df34114 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -56,6 +56,32 @@ static void a2mp_data_ready(struct sock *sk, int bytes)
 	}
 }
 
+static void state_change_worker(struct work_struct *w)
+{
+	struct a2mp_work_state_change *work = (struct a2mp_work_state_change *) w;
+	struct sock *sk = work->sk;
+
+	BT_DBG("sk %p", sk);
+	sock_put(work->sk);
+	kfree(work);
+}
+
+static void a2mp_state_change(struct sock *sk)
+{
+	struct a2mp_work_state_change *work;
+
+	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+	if (work) {
+		INIT_WORK(&work->work, state_change_worker);
+		sock_hold(sk);
+		work->sk = sk;
+		if (!queue_work(a2mp_workqueue, &work->work)) {
+			kfree(work);
+			sock_put(sk);
+		}
+	}
+}
+
 static void l2cap_fixed_channel_config(struct sock *sk)
 {
 	struct l2cap_chan *chan = l2cap_pi(sk)->chan;
@@ -104,6 +130,8 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
 
 	sk = sock->sk;
 	sk->sk_data_ready = a2mp_data_ready;
+	sk->sk_state_change = a2mp_state_change;
+
 	memset(&addr, 0, sizeof(addr));
 	bacpy(&addr.l2_bdaddr, conn->src);
 	addr.l2_family = AF_BLUETOOTH;
-- 
1.7.4.1

[RFCv1 04/21] Bluetooth: A2MP: Build and Send msg helpers

[Emeltchenko Andrei]

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

Helper function to build and send A2MP messages.

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a3938f9..7ead6a3 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,13 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+struct a2mp_cmd {
+	__u8	code;
+	__u8	ident;
+	__le16	len;
+	__u8	data[0];
+} __packed;
+
 struct a2mp_work_data_ready {
 	struct work_struct work;
 	struct sock *sk;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index df34114..1590b21 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -21,6 +21,52 @@
 
 static struct workqueue_struct *a2mp_workqueue;
 
+/* A2MP build & send command helper functions */
+static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
+{
+	struct a2mp_cmd *cmd;
+	int plen;
+
+	plen = sizeof(*cmd) + len;
+	cmd = kzalloc(plen, GFP_KERNEL);
+	if (!cmd)
+		return NULL;
+
+	cmd->code = code;
+	cmd->ident = ident;
+	cmd->len = cpu_to_le16(len);
+
+	memcpy(cmd->data, data, len);
+
+	return cmd;
+}
+
+static inline int __a2mp_send(struct socket *sock, u8 *data, int len)
+{
+	struct kvec iv = { data, len };
+	struct msghdr msg;
+
+	memset(&msg, 0, sizeof(msg));
+
+	return kernel_sendmsg(sock, &msg, &iv, 1, len);
+}
+
+static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+								void *data)
+{
+	struct a2mp_cmd *cmd;
+
+	if (!mgr->a2mp_sock)
+		return;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	__a2mp_send(mgr->a2mp_sock, (u8 *)cmd, len + sizeof(*cmd));
+	kfree(cmd);
+}
+
 static void data_ready_worker(struct work_struct *w)
 {
 	struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
-- 
1.7.4.1

[RFCv1 05/21] Bluetooth: A2MP: AMP Manager basic functions

[Emeltchenko Andrei]

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

Define AMP Manager and some basic functions.

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 7ead6a3..a89c506 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,14 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+struct amp_mgr {
+	struct list_head	list;
+	struct l2cap_conn	*l2cap_conn;
+	struct socket		*a2mp_sock;
+	struct kref		kref;
+	unsigned long		flags;
+};
+
 struct a2mp_cmd {
 	__u8	code;
 	__u8	ident;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 1590b21..e7b7534 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -13,6 +13,7 @@
 */
 
 #include <linux/workqueue.h>
+#include <linux/list.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -21,6 +22,13 @@
 
 static struct workqueue_struct *a2mp_workqueue;
 
+void inline amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
+
+LIST_HEAD(amp_mgr_list);
+DEFINE_RWLOCK(amp_mgr_list_lock);
+
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 {
@@ -205,3 +213,77 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
 		return NULL;
 	}
 }
+
+/* AMP Manager functions */
+void inline amp_mgr_get(struct amp_mgr *mgr)
+{
+	kref_get(&mgr->kref);
+}
+
+static void amp_mgr_destroy(struct kref *kref)
+{
+	struct amp_mgr *mgr;
+	mgr = container_of(kref, struct amp_mgr, kref);
+
+	BT_ERR("Destroy amp_mgr %p", mgr);
+
+	write_lock(&amp_mgr_list_lock);
+	list_del(&mgr->list);
+	write_unlock(&amp_mgr_list_lock);
+
+	sock_release(mgr->a2mp_sock);
+
+	kfree(mgr);
+}
+
+int inline amp_mgr_put(struct amp_mgr *mgr)
+{
+	return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+struct amp_mgr *get_amp_mgr_sk(struct sock *sk)
+{
+	struct amp_mgr *mgr;
+	struct amp_mgr *found = NULL;
+
+	read_lock(&amp_mgr_list_lock);
+	list_for_each_entry(mgr, &amp_mgr_list, list) {
+		if ((mgr->a2mp_sock) && (mgr->a2mp_sock->sk == sk)) {
+			found = mgr;
+			amp_mgr_get(mgr);
+			break;
+		}
+	}
+	read_unlock(&amp_mgr_list_lock);
+
+	return found;
+}
+
+struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+{
+	struct amp_mgr *mgr;
+
+	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+	if (!mgr)
+		goto finished;
+
+	BT_DBG("conn %p mgr %p", conn, mgr);
+
+	mgr->l2cap_conn = conn;
+
+	mgr->a2mp_sock = open_a2mp_sock(conn);
+	if (!mgr->a2mp_sock) {
+		kfree(mgr);
+		mgr = NULL;
+		goto finished;
+	}
+
+	write_lock(&amp_mgr_list_lock);
+	list_add(&(mgr->list), &amp_mgr_list);
+	write_unlock(&amp_mgr_list_lock);
+
+	kref_init(&mgr->kref);
+
+finished:
+	return mgr;
+}
-- 
1.7.4.1

[RFCv1 06/21] Bluetooth: A2MP: Remove AMP manager in state_change

[Emeltchenko Andrei]

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

Handle BT_CLOSED state change by removing AMP manager.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e7b7534..21fae45 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -114,8 +114,23 @@ static void state_change_worker(struct work_struct *w)
 {
 	struct a2mp_work_state_change *work = (struct a2mp_work_state_change *) w;
 	struct sock *sk = work->sk;
+	struct amp_mgr *mgr = NULL;
 
-	BT_DBG("sk %p", sk);
+	mgr = get_amp_mgr_sk(sk);
+	if (!mgr)
+		goto out;
+
+	BT_DBG("sk %p mgr %p state %d", sk, mgr, sk->sk_state);
+
+	switch (sk->sk_state) {
+	case BT_CLOSED:
+		amp_mgr_put(mgr);
+		break;
+	}
+
+	amp_mgr_put(mgr);
+
+out:
 	sock_put(work->sk);
 	kfree(work);
 }
-- 
1.7.4.1

[RFCv1 07/21] Bluetooth: A2MP: AMP manager initialization

[Emeltchenko Andrei]

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


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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a89c506..cba3d0f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -41,4 +41,7 @@ struct a2mp_work_state_change {
 	struct sock *sk;
 };
 
+int a2mp_init(void);
+void a2mp_exit(void);
+
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 21fae45..6ffdd12 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -302,3 +302,17 @@ struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 finished:
 	return mgr;
 }
+
+int a2mp_init(void)
+{
+	a2mp_workqueue = create_singlethread_workqueue("a2mp");
+	if (!a2mp_workqueue)
+		return -EPERM;
+	return 0;
+}
+
+void a2mp_exit(void)
+{
+	flush_workqueue(a2mp_workqueue);
+	destroy_workqueue(a2mp_workqueue);
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 56b94c0..306b27f 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -55,6 +55,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 int disable_ertm;
 int enable_hs;
@@ -4749,6 +4750,12 @@ int __init l2cap_init(void)
 			BT_ERR("Failed to create L2CAP debug file");
 	}
 
+	err = a2mp_init();
+	if (err < 0) {
+		BT_ERR("AMP Manager initialization failed");
+		goto error;
+	}
+
 	return 0;
 
 error:
@@ -4758,6 +4765,7 @@ error:
 
 void l2cap_exit(void)
 {
+	a2mp_exit();
 	debugfs_remove(l2cap_debugfs);
 
 	if (hci_unregister_proto(&l2cap_hci_proto) < 0)
-- 
1.7.4.1

[RFCv1 08/21] Bluetooth: A2MP: Definitions for A2MP commands

[Emeltchenko Andrei]

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

Define A2MP command id and packet structures.

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index cba3d0f..35f6fcb 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -30,6 +30,78 @@ struct a2mp_cmd {
 	__u8	data[0];
 } __packed;
 
+/* A2MP command codes */
+#define A2MP_COMMAND_REJ         0x01
+struct a2mp_cmd_rej {
+	__le16	reason;
+} __packed;
+
+#define A2MP_DISCOVER_REQ        0x02
+struct a2mp_discov_req {
+	__le16	mtu;
+	__le16	ext_feat;
+} __packed;
+
+struct a2mp_cl {
+	__u8	id;
+	__u8	type;
+	__u8	status;
+} __packed;
+
+#define A2MP_DISCOVER_RSP        0x03
+struct a2mp_discov_rsp {
+	__le16     mtu;
+	__le16     ext_feat;
+	struct a2mp_cl cl[0];
+} __packed;
+
+#define A2MP_CHANGE_NOTIFY       0x04
+#define A2MP_CHANGE_RSP          0x05
+
+#define A2MP_GETINFO_REQ         0x06
+struct a2mp_info_req {
+	__u8       id;
+} __packed;
+
+#define A2MP_GETINFO_RSP         0x07
+struct a2mp_info_rsp {
+	__u8	id;
+	__u8	status;
+	__le32	total_bw;
+	__le32	max_bw;
+	__le32	min_latency;
+	__le16	pal_cap;
+	__le16	assoc_size;
+} __packed;
+
+#define A2MP_GETAMPASSOC_REQ     0x08
+struct a2mp_amp_assoc_req {
+	__u8	id;
+} __packed;
+
+#define A2MP_GETAMPASSOC_RSP     0x09
+struct a2mp_amp_assoc_rsp {
+	__u8	id;
+	__u8	status;
+	__u8	amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_REQ  0x0A
+#define A2MP_DISCONNPHYSLINK_REQ 0x0C
+struct a2mp_physlink_req {
+	__u8	local_id;
+	__u8	remote_id;
+	__u8	amp_assoc[0];
+} __packed;
+
+#define A2MP_CREATEPHYSLINK_RSP  0x0B
+#define A2MP_DISCONNPHYSLINK_RSP 0x0D
+struct a2mp_physlink_rsp {
+	__u8	local_id;
+	__u8	remote_id;
+	__u8	status;
+} __packed;
+
 struct a2mp_work_data_ready {
 	struct work_struct work;
 	struct sock *sk;
-- 
1.7.4.1

[RFCv1 09/21] Bluetooth: A2MP: Define A2MP status codes

[Emeltchenko Andrei]

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

A2MP 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/a2mp.h |   10 ++++++++++
 1 files changed, 10 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 35f6fcb..c4cc682 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -102,6 +102,16 @@ struct a2mp_physlink_rsp {
 	__u8	status;
 } __packed;
 
+/* A2MP response status */
+#define A2MP_STATUS_SUCCESS			0x00
+#define A2MP_STATUS_INVALID_CTRL_ID		0x01
+#define A2MP_STATUS_UNABLE_START_LINK_CREATION	0x02
+#define A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS	0x02
+#define A2MP_STATUS_COLLISION_OCCURED		0x03
+#define A2MP_STATUS_DISCONN_REQ_RECVD		0x04
+#define A2MP_STATUS_PHYS_LINK_EXISTS		0x05
+#define A2MP_STATUS_SECURITY_VIOLATION		0x06
+
 struct a2mp_work_data_ready {
 	struct work_struct work;
 	struct sock *sk;
-- 
1.7.4.1

[RFCv1 10/21] Bluetooth: A2MP: Process A2MP messages

[Emeltchenko Andrei]

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

Implement basic processing for AMP Manager Protocol (A2MP).

Example below shows processing unrecognized command.
...
> ACL data: handle 11 flags 0x02 dlen 12
    A2MP: code 0x00 ident 3 len 0
< ACL data: handle 11 flags 0x00 dlen 14
    A2MP: Command Reject: reason (0) - Command not recognized
...

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6ffdd12..4e45897 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -75,6 +75,63 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Handle A2MP signalling */
+void a2mp_receive(struct sock *sk, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+	struct amp_mgr *mgr;
+	int err = 0;
+
+	mgr = get_amp_mgr_sk(sk);
+	if (!mgr)
+		return;
+
+	while (skb->len >= sizeof(*hdr)) {
+		struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+		u16 len = le16_to_cpu(hdr->len);
+
+		BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len);
+
+		skb_pull(skb, sizeof(*hdr));
+
+		if (len > skb->len || !hdr->ident) {
+			err = -EINVAL;
+			break;
+		}
+
+		switch (hdr->code) {
+		case A2MP_COMMAND_REJ:
+		case A2MP_DISCOVER_REQ:
+		case A2MP_CHANGE_NOTIFY:
+		case A2MP_GETINFO_REQ:
+		case A2MP_GETAMPASSOC_REQ:
+		case A2MP_CREATEPHYSLINK_REQ:
+		case A2MP_DISCONNPHYSLINK_REQ:
+		case A2MP_CHANGE_RSP:
+		case A2MP_DISCOVER_RSP:
+		case A2MP_GETINFO_RSP:
+		case A2MP_GETAMPASSOC_RSP:
+		case A2MP_CREATEPHYSLINK_RSP:
+		case A2MP_DISCONNPHYSLINK_RSP:
+		default:
+			BT_ERR("Unknown A2MP signaling command 0x%2.2x",
+				hdr->code);
+			err = -EINVAL;
+			break;
+		}
+	}
+
+	if (err) {
+		struct a2mp_cmd_rej rej;
+		rej.reason = cpu_to_le16(0);
+
+		a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
+									&rej);
+	}
+
+	amp_mgr_put(mgr);
+}
+
 static void data_ready_worker(struct work_struct *w)
 {
 	struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
@@ -85,7 +142,7 @@ static void data_ready_worker(struct work_struct *w)
 
 	/* skb_dequeue() is thread-safe */
 	while ((skb = skb_dequeue(&sk->sk_receive_queue))) {
-		/* process a2mp here */
+		a2mp_receive(sk, skb);
 		kfree_skb(skb);
 	}
 
-- 
1.7.4.1

[RFCv1 11/21] Bluetooth: A2MP: Process A2MP Command Reject

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4e45897..800a7a8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -75,6 +75,22 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Processing A2MP messages */
+static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_cmd_rej *rej = (struct a2mp_cmd_rej *)skb->data;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*rej))
+		return -EINVAL;
+
+	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
+
+	skb_pull(skb, sizeof(*rej));
+
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -101,6 +117,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 
 		switch (hdr->code) {
 		case A2MP_COMMAND_REJ:
+			a2mp_command_rej(mgr, skb, hdr);
+			break;
+
 		case A2MP_DISCOVER_REQ:
 		case A2MP_CHANGE_NOTIFY:
 		case A2MP_GETINFO_REQ:
-- 
1.7.4.1

[RFCv1 12/21] Bluetooth: A2MP: Helper functions to count HCI devs

[Emeltchenko Andrei]

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

Helper functions used to cound HCI devices (AMP controllers) and
build controller list packet.

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 139ce2a..77a071a 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -56,6 +56,8 @@
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
 
+#define HCI_BREDR_ID	0
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_NO_RESET,
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 0a5a05d..58ad764 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -591,6 +591,20 @@ static inline struct hci_dev *hci_dev_hold(struct hci_dev *d)
 	return NULL;
 }
 
+static inline uint8_t hci_num_ctrl(void)
+{
+	uint8_t count = 0;
+	struct list_head *p;
+
+	read_lock_bh(&hci_dev_list_lock);
+	list_for_each(p, &hci_dev_list) {
+		count++;
+	}
+	read_unlock_bh(&hci_dev_list_lock);
+
+	return count;
+}
+
 #define hci_dev_lock(d)		spin_lock(&d->lock)
 #define hci_dev_unlock(d)	spin_unlock(&d->lock)
 #define hci_dev_lock_bh(d)	spin_lock_bh(&d->lock)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 800a7a8..7f931a1 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -75,6 +75,37 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
+{
+	cl->id = 0;
+	cl->type = 0;
+	cl->status = 1;
+}
+
+static void __a2mp_add_cl(struct a2mp_cl *cl, u8 num_ctrl)
+{
+	int i = 0;
+	struct hci_dev *hdev;
+
+	read_lock_bh(&hci_dev_list_lock);
+
+	__a2mp_cl_bredr(cl);
+
+	list_for_each_entry(hdev, &hci_dev_list, list) {
+		if (i++ >= num_ctrl)
+			goto out;
+
+		if (hdev->id == HCI_BREDR_ID)
+			continue;
+
+		cl[i].id = hdev->id;
+		cl[i].type = hdev->amp_type;
+		cl[i].status = hdev->amp_status;
+	}
+out:
+	read_unlock_bh(&hci_dev_list_lock);
+}
+
 /* Processing A2MP messages */
 static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 							struct a2mp_cmd *hdr)
-- 
1.7.4.1

[RFCv1 13/21] Bluetooth: A2MP: Process A2MP Discover Request

[Emeltchenko Andrei]

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

Process A2MP Discover Request, code makes sure that first
controller in the list is BREDR one. Trace is shown below:
...
> ACL data: handle 11 flags 0x02 dlen 16
    A2MP: Discover req: mtu/mps 670 mask: 0x0000
< ACL data: handle 11 flags 0x00 dlen 19
    A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
      Controller list:
        id 0, type 0, status 0x01 (Bluetooth only)
...

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c4cc682..31d98a8 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+#define A2MP_FEAT_EXT	0x8000
+
 struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7f931a1..e4ad28d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -122,6 +122,52 @@ static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static inline int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_req *req = (struct a2mp_discov_req *)skb->data;
+	struct a2mp_discov_rsp *rsp;
+	u16 ext_feat;
+	size_t len;
+	u8 num_ctrl;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	skb_pull(skb, sizeof(*req));
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu),
+						le16_to_cpu(req->ext_feat));
+
+	ext_feat = le16_to_cpu(req->ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (skb->len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("ext_feat 0x%4.4x", le16_to_cpu(req->ext_feat));
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	num_ctrl = hci_num_ctrl();
+	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof (*rsp);
+	rsp = kmalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return -ENOMEM;
+
+	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	rsp->ext_feat = 0;
+
+	__a2mp_add_cl(rsp->cl, num_ctrl);
+
+	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -152,6 +198,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_DISCOVER_REQ:
+			err = a2mp_discover_req(mgr, skb, hdr);
+			break;
+
 		case A2MP_CHANGE_NOTIFY:
 		case A2MP_GETINFO_REQ:
 		case A2MP_GETAMPASSOC_REQ:
-- 
1.7.4.1

[RFCv1 14/21] Bluetooth: A2MP: Process A2MP Change Notify

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e4ad28d..c69e949 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -168,6 +168,22 @@ static inline int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static inline int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_cl *cl = (struct a2mp_cl *)skb->data;
+
+	while (skb->len >= sizeof(*cl)) {
+		BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
+								cl->status);
+		cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
+	}
+
+	/* TODO send A2MP_CHANGE_RSP */
+
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -202,6 +218,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_CHANGE_NOTIFY:
+			err = a2mp_change_notify(mgr, skb, hdr);
+			break;
+
 		case A2MP_GETINFO_REQ:
 		case A2MP_GETAMPASSOC_REQ:
 		case A2MP_CREATEPHYSLINK_REQ:
-- 
1.7.4.1

[RFCv1 15/21] Bluetooth: A2MP: Process A2MP Get Info Request

[Emeltchenko Andrei]

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

Example of trace log for invalid controller id is shown below:
...
> ACL data: handle 11 flags 0x02 dlen 13
    A2MP: Get Info req: id 238
< ACL data: handle 11 flags 0x00 dlen 30
    A2MP: Get Info rsp: id 238 status (1) Invalid Controller ID
      total bandwidth -381650496
      max guaranteed bandwidth -274362188
      min latency -158843144
      pal capabilities 0x9750
      assoc size 49478
...
Note that If the Status field is set to Invalid Controller ID all subsequent
fields in the AMP Get Info Response shall be ignored by the receiver.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c69e949..3cef720 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -184,6 +184,38 @@ static inline int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static inline int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_req *req  = (struct a2mp_info_req *)skb->data;
+	struct hci_dev *hdev;
+	struct a2mp_info_rsp rsp;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	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)
+		hci_dev_put(hdev);
+
+	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -222,6 +254,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_GETINFO_REQ:
+			err = a2mp_getinfo_req(mgr, skb, hdr);
+			break;
+
 		case A2MP_GETAMPASSOC_REQ:
 		case A2MP_CREATEPHYSLINK_REQ:
 		case A2MP_DISCONNPHYSLINK_REQ:
-- 
1.7.4.1

[RFCv1 16/21] Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

[Emeltchenko Andrei]

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

Example trace when receiving AMP Assoc Request with wrong AMP id.
...
> ACL data: handle 11 flags 0x02 dlen 13
    A2MP: Get AMP Assoc req: id 238
< ACL data: handle 11 flags 0x00 dlen 14
    A2MP: Get AMP Assoc rsp: id 238 status (1) Invalid Controller ID
      assoc data:
...

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 3cef720..8eeb70e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -216,6 +216,35 @@ static inline int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_amp_assoc_req *req =
+				(struct a2mp_amp_assoc_req *)skb->data;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("id %d", req->id);
+
+	hdev = hci_dev_get(req->id);
+	if (!hdev || hdev->amp_type == HCI_BREDR) {
+		struct a2mp_amp_assoc_rsp rsp;
+		rsp.id = req->id;
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
+									&rsp);
+	}
+
+	if (hdev)
+		hci_dev_put(hdev);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -258,6 +287,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_GETAMPASSOC_REQ:
+			err = a2mp_getampassoc_req(mgr, skb, hdr);
+			break;
+
 		case A2MP_CREATEPHYSLINK_REQ:
 		case A2MP_DISCONNPHYSLINK_REQ:
 		case A2MP_CHANGE_RSP:
-- 
1.7.4.1

[RFCv1 17/21] Bluetooth: A2MP: Process A2MP Create Physlink Request

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 8eeb70e..ba88f34 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -245,6 +245,22 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_physlink_req *req = (struct a2mp_physlink_req *)skb->data;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
+
+	/* TODO process physlink create */
+
+	skb_pull(skb, hdr->len);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -291,6 +307,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_CREATEPHYSLINK_REQ:
+			err = a2mp_createphyslink_req(mgr, skb, hdr);
+			break;
+
 		case A2MP_DISCONNPHYSLINK_REQ:
 		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
-- 
1.7.4.1

[RFCv1 18/21] Bluetooth: A2MP: Process A2MP Disc Physlink Request

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ba88f34..acb5e15 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -261,6 +261,40 @@ static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	struct a2mp_physlink_req *req = (struct a2mp_physlink_req *)skb->data;
+	struct a2mp_physlink_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+	rsp.status = A2MP_STATUS_SUCCESS;
+
+	BT_DBG("local_id %d remote_id %d", rsp.local_id, rsp.remote_id);
+
+	hdev = hci_dev_get(req->local_id);
+	if (!hdev) {
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send_rsp;
+	}
+
+	/* TODO Disconnect Phys Link here */
+
+send_rsp:
+	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+	if (hdev)
+		hci_dev_put(hdev);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -311,6 +345,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 			break;
 
 		case A2MP_DISCONNPHYSLINK_REQ:
+			err = a2mp_discphyslink_req(mgr, skb, hdr);
+			break;
+
 		case A2MP_CHANGE_RSP:
 		case A2MP_DISCOVER_RSP:
 		case A2MP_GETINFO_RSP:
-- 
1.7.4.1

[RFCv1 19/21] Bluetooth: A2MP: Process A2MP Command Responses

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index acb5e15..faf22ca 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -295,6 +295,15 @@ send_rsp:
 	return 0;
 }
 
+static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
+							struct a2mp_cmd *hdr)
+{
+	BT_DBG("ident %d code %d", hdr->ident, hdr->code);
+
+	skb_pull(skb, hdr->len);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -354,6 +363,9 @@ void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 		case A2MP_GETAMPASSOC_RSP:
 		case A2MP_CREATEPHYSLINK_RSP:
 		case A2MP_DISCONNPHYSLINK_RSP:
+			err = a2mp_cmd_rsp(mgr, skb, hdr);
+			break;
+
 		default:
 			BT_ERR("Unknown A2MP signaling command 0x%2.2x",
 				hdr->code);
-- 
1.7.4.1

[RFCv1 20/21] Bluetooth: A2MP: Handling fixed channel

[Emeltchenko Andrei]

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

A2MP fixed channel is handled differently from normal L2CAP channel.
There is no connect and config req/rsp sequence.

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index c0af77c..aeb8676 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -592,6 +592,7 @@ enum {
 	FLAG_FLUSHABLE,
 	FLAG_EXT_CTRL,
 	FLAG_EFS_ENABLE,
+	FLAG_FIXED_CHANNEL,
 };
 
 #define __set_chan_timer(c, t) l2cap_set_timer(c, &c->chan_timer, (t))
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index faf22ca..21e808d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -467,6 +467,8 @@ static void l2cap_fixed_channel_config(struct sock *sk)
 
 	lock_sock(sk);
 
+	set_bit(FLAG_FIXED_CHANNEL, &chan->flags);
+
 	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
 	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 306b27f..cf57ae0 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -318,6 +318,10 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 
 	chan->conn = conn;
 
+	/* Fixed channel setup is already done */
+	if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
+		goto fixed_chan;
+
 	if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) {
 		if (conn->hcon->type == LE_LINK) {
 			/* LE connection */
@@ -341,6 +345,8 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 		chan->omtu = L2CAP_DEFAULT_MTU;
 	}
 
+fixed_chan:
+
 	chan->local_id		= L2CAP_BESTEFFORT_ID;
 	chan->local_stype	= L2CAP_SERV_BESTEFFORT;
 	chan->local_msdu	= L2CAP_DEFAULT_MAX_SDU_SIZE;
@@ -373,7 +379,8 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		chan_put(chan);
 
 		chan->conn = NULL;
-		hci_conn_put(conn->hcon);
+		if (!test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
+			hci_conn_put(conn->hcon);
 	}
 
 	l2cap_state_change(chan, BT_CLOSED);
@@ -389,7 +396,8 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		sk->sk_state_change(sk);
 
 	if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) &&
-			test_bit(CONF_INPUT_DONE, &chan->conf_state)))
+			test_bit(CONF_INPUT_DONE, &chan->conf_state)) &&
+			!test_bit(FLAG_FIXED_CHANNEL, &chan->flags))
 		return;
 
 	skb_queue_purge(&chan->tx_q);
@@ -1139,10 +1147,9 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 	struct hci_conn *hcon;
 	struct hci_dev *hdev;
 	__u8 auth_type;
-	int err;
+	int err = 0;
 
-	BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst),
-							chan->psm);
+	BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), chan->psm);
 
 	hdev = hci_get_route(dst, src);
 	if (!hdev)
@@ -1152,23 +1159,34 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 
 	auth_type = l2cap_get_auth_type(chan);
 
-	if (chan->dcid == L2CAP_CID_LE_DATA)
-		hcon = hci_connect(hdev, LE_LINK, dst,
-					chan->sec_level, auth_type);
-	else
-		hcon = hci_connect(hdev, ACL_LINK, dst,
-					chan->sec_level, auth_type);
+	if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags)) {
+		/* Fixed channels piggyback on existing ACL connections */
+		hcon = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst);
+		if (!hcon || !hcon->l2cap_data) {
+			err = -ENOTCONN;
+			goto done;
+		}
 
-	if (IS_ERR(hcon)) {
-		err = PTR_ERR(hcon);
-		goto done;
-	}
+		conn = hcon->l2cap_data;
+	} else {
+		if (chan->dcid == L2CAP_CID_LE_DATA)
+			hcon = hci_connect(hdev, LE_LINK, dst,
+						chan->sec_level, auth_type);
+		else
+			hcon = hci_connect(hdev, ACL_LINK, dst,
+						chan->sec_level, auth_type);
 
-	conn = l2cap_conn_add(hcon, 0);
-	if (!conn) {
-		hci_conn_put(hcon);
-		err = -ENOMEM;
-		goto done;
+		if (IS_ERR(hcon)) {
+			err = PTR_ERR(hcon);
+			goto done;
+		}
+
+		conn = l2cap_conn_add(hcon, 0);
+		if (!conn) {
+			hci_conn_put(hcon);
+			err = -ENOMEM;
+			goto done;
+		}
 	}
 
 	/* Update source addr of the socket */
@@ -1176,6 +1194,13 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 
 	l2cap_chan_add(conn, chan);
 
+	if (test_bit(FLAG_FIXED_CHANNEL, &chan->flags)) {
+		l2cap_state_change(chan, BT_CONNECTED);
+		sk->sk_state = BT_CONNECTED;
+		sk->sk_state_change(sk);
+		goto done;
+	}
+
 	l2cap_state_change(chan, BT_CONNECT);
 	__set_chan_timer(chan, sk->sk_sndtimeo);
 
@@ -1188,8 +1213,6 @@ int l2cap_chan_connect(struct l2cap_chan *chan)
 			l2cap_do_start(chan);
 	}
 
-	err = 0;
-
 done:
 	hci_dev_unlock_bh(hdev);
 	hci_dev_put(hdev);
-- 
1.7.4.1

[RFCv1 21/21] Bluetooth: A2MP: Manage incoming connections

[Emeltchenko Andrei]

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

Handle incoming A2MP connection by creating AMP manager and
processing A2MP messages in a workqueue.

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 31d98a8..d1abddc 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -17,6 +17,8 @@
 
 #define A2MP_FEAT_EXT	0x8000
 
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
+
 struct amp_mgr {
 	struct list_head	list;
 	struct l2cap_conn	*l2cap_conn;
@@ -125,6 +127,12 @@ struct a2mp_work_state_change {
 	struct sock *sk;
 };
 
+struct a2mp_work_incoming {
+	struct work_struct work;
+	struct l2cap_conn *conn;
+	struct sk_buff *skb;
+};
+
 int a2mp_init(void);
 void a2mp_exit(void);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 21e808d..2d9e975 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -615,6 +615,40 @@ finished:
 	return mgr;
 }
 
+static void incoming_worker(struct work_struct *w)
+{
+	struct a2mp_work_incoming *work = (struct a2mp_work_incoming *) w;
+	struct l2cap_conn *conn = work->conn;
+	struct amp_mgr *mgr;
+	struct sock *sk;
+
+	mgr = amp_mgr_create(conn);
+	sk = mgr->a2mp_sock->sk;
+
+	BT_DBG("mgr %p sk %p", mgr, sk);
+
+	lock_sock(sk);
+	sk->sk_backlog_rcv(sk, work->skb);
+	release_sock(sk);
+
+	kfree(work);
+}
+
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+	struct a2mp_work_incoming *work;
+
+	BT_DBG("conn %p", conn);
+	work = kmalloc(sizeof(*work), GFP_ATOMIC);
+	if (work) {
+		INIT_WORK((struct work_struct *) work, incoming_worker);
+		work->conn = conn;
+		work->skb = skb;
+		if (queue_work(a2mp_workqueue, (struct work_struct *) work) == 0)
+			kfree(work);
+	}
+}
+
 int a2mp_init(void)
 {
 	a2mp_workqueue = create_singlethread_workqueue("a2mp");
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index cf57ae0..1c312b2 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -4201,8 +4201,14 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
 
 	chan = l2cap_get_chan_by_scid(conn, cid);
 	if (!chan) {
-		BT_DBG("unknown cid 0x%4.4x", cid);
-		goto drop;
+		if (cid == L2CAP_CID_A2MP) {
+			BT_DBG("A2MP");
+			a2mp_incoming(conn, skb);
+			goto done;
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			goto drop;
+		}
 	}
 
 	sk = chan->sk;
-- 
1.7.4.1