[RFCv0 00/20] RFC A2MP implementation

[RFCv0 00/20] RFC A2MP implementation

[Emeltchenko Andrei]

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

RFC meant to indicate our current development status.

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 (20):
  Bluetooth: A2MP: create A2MP socket
  Bluetooth: A2MP: sk_data_ready for a2mp socket
  Bluetooth: A2MP: 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: process A2MP messages
  Bluetooth: A2MP: processing 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 connection

 include/net/bluetooth/a2mp.h     |  135 ++++++++
 include/net/bluetooth/hci_core.h |   14 +
 include/net/bluetooth/l2cap.h    |    5 +
 net/bluetooth/a2mp.c             |  630 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c       |   81 ++++--
 5 files changed, 845 insertions(+), 20 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 net/bluetooth/a2mp.c

-- 
1.7.4.1

[RFCv0 01/20] 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 f478301..cc3b81f 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -45,6 +45,8 @@
 #define L2CAP_DEFAULT_SDU_ITIME		0xFFFFFFFF
 #define L2CAP_DEFAULT_ACC_LAT		0xFFFFFFFF
 
+#define L2CAP_A2MP_DEFAULT_MTU		670
+
 #define L2CAP_CONN_TIMEOUT	(40000) /* 40 seconds */
 #define L2CAP_INFO_TIMEOUT	(4000)  /*  4 seconds */
 
@@ -217,6 +219,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
@@ -772,5 +775,6 @@ void l2cap_chan_destroy(struct l2cap_chan *chan);
 int l2cap_chan_connect(struct l2cap_chan *chan);
 int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len);
 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..b69dd07
--- /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;
+
+	chan->tx_win = 1;
+	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 fdb72ea..1a1fe2d 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -1924,7 +1924,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

[RFCv0 02/20] Bluetooth: A2MP: 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 b69dd07..2b41882 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_KERNEL);
+	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)
 {
@@ -64,6 +102,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

[RFCv0 03/20] Bluetooth: A2MP: 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 2b41882..d0b15d8 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_KERNEL);
+	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;
@@ -103,6 +129,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

[RFCv0 04/20] 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 d0b15d8..2fe0004 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 socket *a2mp_sock, u8 code, u8 ident, u16 len,
+								void *data)
+{
+	struct a2mp_cmd *cmd;
+
+	if (!a2mp_sock)
+		return;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	__a2mp_send(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

[RFCv0 05/20] 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 |   11 +++++++
 net/bluetooth/a2mp.c         |   60 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 71 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 7ead6a3..9827cec 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,17 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
+struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
+void remove_amp_mgr(struct amp_mgr *mgr);
+
+struct amp_mgr {
+	struct	list_head list;
+	struct	l2cap_conn *l2cap_conn;
+	struct	socket *a2mp_sock;
+	unsigned long	flags;
+};
+
 struct a2mp_cmd {
 	__u8	code;
 	__u8	ident;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2fe0004..d505c77 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,9 @@
 
 static struct workqueue_struct *a2mp_workqueue;
 
+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 +209,59 @@ static struct socket *open_a2mp_sock(struct l2cap_conn *conn)
 	}
 }
 
+/* AMP Manager functions */
+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;
+			break;
+		}
+	}
+	read_unlock(&amp_mgr_list_lock);
+
+	return found;
+}
+
+void remove_amp_mgr(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", mgr);
+
+	write_lock(&amp_mgr_list_lock);
+	sock_release(mgr->a2mp_sock);
+	list_del(&mgr->list);
+	write_unlock(&amp_mgr_list_lock);
+
+	kfree(mgr);
+}
+
+struct amp_mgr *create_amp_mgr(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);
+
+finished:
+	return mgr;
+}
-- 
1.7.4.1

[RFCv0 06/20] 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 |   14 +++++++++++++-
 1 files changed, 13 insertions(+), 1 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d505c77..190827f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -110,8 +110,20 @@ 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;
+
+	BT_DBG("sk %p state %d", sk, sk->sk_state);
+
+	switch(sk->sk_state) {
+	case BT_CLOSED:
+		mgr = get_amp_mgr_sk(sk);
+		if (mgr) {
+			BT_DBG("sk %p closing", sk);
+			remove_amp_mgr(mgr);
+		}
+		break;
+	}
 
-	BT_DBG("sk %p", sk);
 	sock_put(work->sk);
 	kfree(work);
 }
-- 
1.7.4.1

[RFCv0 07/20] 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 9827cec..aafaf09 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -44,4 +44,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 190827f..afb01e8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -277,3 +277,17 @@ struct amp_mgr *create_amp_mgr(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 1a1fe2d..8b930ca 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;
@@ -4621,6 +4622,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:
@@ -4630,6 +4637,7 @@ error:
 
 void l2cap_exit(void)
 {
+	a2mp_exit();
 	debugfs_remove(l2cap_debugfs);
 
 	if (hci_unregister_proto(&l2cap_hci_proto) < 0)
-- 
1.7.4.1

[RFCv0 08/20] 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 aafaf09..e2eec2b 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -33,6 +33,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

[RFCv0 09/20] Bluetooth: A2MP: process A2MP messages

[Emeltchenko Andrei]

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

Implement basic processing for AMP Manager Protocol (A2MP).

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index afb01e8..5071acc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,61 @@ static void a2mp_send(struct socket *a2mp_sock, 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 && mgr) {
+		struct a2mp_cmd_rej rej;
+		rej.reason = cpu_to_le16(0);
+
+		a2mp_send(mgr->a2mp_sock, A2MP_COMMAND_REJ, hdr->ident,
+							sizeof(rej), &rej);
+	}
+}
+
 static void data_ready_worker(struct work_struct *w)
 {
 	struct a2mp_work_data_ready *work = (struct a2mp_work_data_ready *) w;
@@ -81,7 +136,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

[RFCv0 10/20] Bluetooth: A2MP: processing 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 5071acc..996a9dc 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,22 @@ static void a2mp_send(struct socket *a2mp_sock, 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 (skb->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)
 {
@@ -97,6 +113,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

[RFCv0 11/20] 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_core.h |   14 ++++++++++++++
 net/bluetooth/a2mp.c             |   27 +++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index d037516..a9e5168 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -543,6 +543,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 996a9dc..e10924e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -71,6 +71,33 @@ static void a2mp_send(struct socket *a2mp_sock, 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;
+		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

[RFCv0 12/20] Bluetooth: A2MP: process A2MP Discover Request

[Emeltchenko Andrei]

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


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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index e2eec2b..1782f47 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 *get_amp_mgr_sk(struct sock *sk);
 struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
 void remove_amp_mgr(struct amp_mgr *mgr);
@@ -105,6 +107,10 @@ struct a2mp_physlink_rsp {
 	__u8	status;
 } __packed;
 
+enum {
+	FLAG_DISCOVERED,
+};
+
 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 e10924e..b67696f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -114,6 +114,54 @@ 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 (skb->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);
+
+	BT_DBG("num_ctrl = %d", num_ctrl);
+
+	a2mp_send(mgr->a2mp_sock, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -144,6 +192,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

[RFCv0 13/20] 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 b67696f..3dd3cdb 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -162,6 +162,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)
 {
@@ -196,6 +212,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

[RFCv0 14/20] Bluetooth: A2MP: process A2MP Get Info 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 3dd3cdb..af3f2db 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -178,6 +178,40 @@ 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 = 1;
+
+	BT_DBG("id %d", req->id);
+	hdev = hci_dev_get(AMP_HCI_ID(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);
+	}
+
+	/* TODO send A2MP_GETINFO_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)
 {
@@ -216,6 +250,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

[RFCv0 15/20] Bluetooth: A2MP: process A2MP Get AMP Assoc Request

[Emeltchenko Andrei]

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


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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index af3f2db..3e06c93 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -212,6 +212,23 @@ 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;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("id %d", req->id);
+
+	/* TODO send A2MP_GETAMPASSOC_RSP */
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 void a2mp_receive(struct sock *sk, struct sk_buff *skb)
 {
@@ -254,6 +271,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

[RFCv0 16/20] 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 3e06c93..b584465 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -229,6 +229,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)
 {
@@ -275,6 +291,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

[RFCv0 17/20] 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 |   42 ++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 42 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b584465..662ada7 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -245,6 +245,45 @@ 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;
+	struct hci_conn *conn;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+	rsp.status = 0;
+
+	BT_DBG("local_id %d remote_id %d", rsp.local_id, rsp.remote_id);
+	hdev = hci_dev_get(AMP_HCI_ID(req->local_id));
+	if (!hdev) {
+		rsp.status = 1; /* Invalid Controller ID */
+		goto send_rsp;
+	}
+
+	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK,
+					&mgr->l2cap_conn->hcon->dst);
+	if (conn) {
+		BT_DBG("conn %p", conn);
+		hci_acl_disconn(conn, 0x13);
+	}
+
+send_rsp:
+	/* TODO send A2MP_DISCONNPHYSLINK_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)
 {
@@ -295,6 +334,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

[RFCv0 18/20] 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 662ada7..4414d74 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -284,6 +284,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)
 {
@@ -343,6 +352,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

[RFCv0 19/20] 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    |   61 +++++++++++++++++++++++++++++-----------
 3 files changed, 47 insertions(+), 17 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index cc3b81f..473976e 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -559,6 +559,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 4414d74..b4d3a13 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -451,6 +451,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 8b930ca..3dd80a5 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -316,6 +316,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 */
@@ -339,6 +343,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;
@@ -371,7 +377,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);
@@ -387,7 +394,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);
@@ -1128,23 +1136,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 */
@@ -1152,6 +1171,14 @@ 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);
+		err = 0;
+		goto done;
+	}
+
 	l2cap_state_change(chan, BT_CONNECT);
 	__set_chan_timer(chan, sk->sk_sndtimeo);
 
-- 
1.7.4.1

[RFCv0 20/20] Bluetooth: A2MP: manage incoming connection

[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 |    7 +++++++
 net/bluetooth/a2mp.c         |   34 ++++++++++++++++++++++++++++++++++
 net/bluetooth/l2cap_core.c   |   10 ++++++++--
 3 files changed, 49 insertions(+), 2 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 1782f47..fa1477f 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -20,6 +20,7 @@
 struct amp_mgr *get_amp_mgr_sk(struct sock *sk);
 struct amp_mgr *create_amp_mgr(struct l2cap_conn *conn);
 void remove_amp_mgr(struct amp_mgr *mgr);
+void a2mp_incoming(struct l2cap_conn *conn, struct sk_buff *skb);
 
 struct amp_mgr {
 	struct	list_head list;
@@ -122,6 +123,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 b4d3a13..2ffe0dd 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -581,6 +581,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 = create_amp_mgr(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_KERNEL);
+	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 3dd80a5..83cad81 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -4077,8 +4077,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

RE: [RFCv0 00/20] RFC A2MP implementation

[Peter Krystad]

Hi Andrei,

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> RFC meant to indicate our current development status.
> 
> 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

I think your approach looks fine so far. Do you plan to continue on with
adding detail to the patches in this series or will just do cleanup on them
and then prepare another set?

Peter.
 
--Peter Krystad
  Employee of Qualcomm Innovation Center, Inc.
  Qualcomm Innovation Center, Inc. is a member of Code Aurora Forum

Re: [RFCv0 00/20] RFC A2MP implementation

['Emeltchenko Andrei']

Hi Peter,

On Thu, Nov 03, 2011 at 05:20:38PM -0700, Peter Krystad wrote:
> 
> Hi Andrei,
> 
> > From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> > 
> > RFC meant to indicate our current development status.
> > 
> > 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
> 
> I think your approach looks fine so far. Do you plan to continue on with
> adding detail to the patches in this series or will just do cleanup on them
> and then prepare another set?

I was thinking about rebasing first against recent bluetooth-next (since
my code was based upon a bit different approach (like fixed channel
features list patch).

Then I am planning to add some details to A2MP message
processing. Those I could test and verify with PTS "beta A2MP" tests.

After that I was thinking about adding physlink structure (at least
heplers) and then make interface to create fake/virtual AMP so that I
could use it for A2MP Discover, etc. (maybe we could use real wifi cheap
for e.g. AMPAssoc data).

What are your plans?

Best regards 
Andrei Emeltchenko