[RFCv5 00/26] RFC Bluetooth A2MP implementation

[RFCv5 00/26] RFC Bluetooth A2MP implementation

[Andrei Emeltchenko]

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

Changes:
	* RFCv5: Fix memory leaks, sparse warnings and taking comments from upstream.
	* RFCv4: redesign code to use l2cap channel lock instead of socket lock
	and general modifications. Basic implementation of HCI callback interface.
	* RFCv3: redesign code to use l2cap functions instead of kernel sockets
	L2CAP functions modified to work with channels without sk.
	* RFCv2: rebased against "workqueue" patches.
	* RFCv1: added refcnt to amp_mgr, fixed sleeping in atomic

Initially code was based of reference implementations below:

References: 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.

Andrei Emeltchenko (26):
  Bluetooth: Add set_err to state_change callback
  Bluetooth: Lock sk only if exist in state_change
  Bluetooth: A2MP: Create A2MP channel
  Bluetooth: A2MP: AMP Manager basic functions
  Bluetooth: A2MP: Build and Send msg helpers
  Bluetooth: A2MP: Add chan callbacks
  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 channels
  Bluetooth: A2MP: Manage incoming connections
  Bluetooth: physical link HCI interface to AMP
  Bluetooth: Define AMP controller statuses
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Read Local Assoc support

 include/net/bluetooth/a2mp.h     |  128 +++++++++
 include/net/bluetooth/amp.h      |   21 ++
 include/net/bluetooth/hci.h      |   50 ++++
 include/net/bluetooth/hci_core.h |   44 +++
 include/net/bluetooth/l2cap.h    |   10 +-
 net/bluetooth/Makefile           |    3 +-
 net/bluetooth/a2mp.c             |  537 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/amp.c              |  108 ++++++++
 net/bluetooth/hci_conn.c         |   15 +
 net/bluetooth/hci_core.c         |  168 ++++++++++++
 net/bluetooth/hci_event.c        |   57 ++++-
 net/bluetooth/l2cap_core.c       |   94 ++++---
 net/bluetooth/l2cap_sock.c       |    8 +-
 13 files changed, 1198 insertions(+), 45 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/a2mp.c
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.9.1

[RFCv5 01/26] Bluetooth: Add set_err to state_change callback

[Andrei Emeltchenko]

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

There are several places in the code where l2cap_state_change and
l2cap_chan_set_err used together. With combining them we remove
socket lock in l2cap_send_disconn_req and simplify code in couple
of other places with a price of adding extra parameter err to
state_change.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
---
 include/net/bluetooth/l2cap.h |    5 +++-
 net/bluetooth/l2cap_core.c    |   56 ++++++++++++++++++----------------------
 net/bluetooth/l2cap_sock.c    |    8 +++++-
 3 files changed, 36 insertions(+), 33 deletions(-)

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 35334a0..9287c24 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -506,7 +506,8 @@ struct l2cap_ops {
 	struct l2cap_chan	*(*new_connection) (void *data);
 	int			(*recv) (void *data, struct sk_buff *skb);
 	void			(*close) (void *data);
-	void			(*state_change) (void *data, int state);
+	void			(*state_change) (void *data, int state,
+								int err);
 	struct sk_buff		*(*alloc_skb) (struct l2cap_chan *chan,
 					unsigned long len, int nb, int *err);
 
@@ -867,4 +868,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
 
+void __l2cap_chan_set_err(struct l2cap_chan *chan, int err);
+
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index e66c9da..5b9a667 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -199,25 +199,25 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
 	return 0;
 }
 
-static void __l2cap_state_change(struct l2cap_chan *chan, int state)
+static void __l2cap_state_change(struct l2cap_chan *chan, int state, int err)
 {
 	BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
 						state_to_string(state));
 
 	chan->state = state;
-	chan->ops->state_change(chan->data, state);
+	chan->ops->state_change(chan->data, state, err);
 }
 
-static void l2cap_state_change(struct l2cap_chan *chan, int state)
+static inline void l2cap_state_change(struct l2cap_chan *chan, int state, int err)
 {
 	struct sock *sk = chan->sk;
 
 	lock_sock(sk);
-	__l2cap_state_change(chan, state);
+	__l2cap_state_change(chan, state, err);
 	release_sock(sk);
 }
 
-static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
+void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
 {
 	struct sock *sk = chan->sk;
 
@@ -377,11 +377,9 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 
 	lock_sock(sk);
 
-	__l2cap_state_change(chan, BT_CLOSED);
-	sock_set_flag(sk, SOCK_ZAPPED);
+	__l2cap_state_change(chan, BT_CLOSED, err);
 
-	if (err)
-		__l2cap_chan_set_err(chan, err);
+	sock_set_flag(sk, SOCK_ZAPPED);
 
 	if (parent) {
 		bt_accept_unlink(sk);
@@ -445,7 +443,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 		lock_sock(sk);
 		l2cap_chan_cleanup_listen(sk);
 
-		__l2cap_state_change(chan, BT_CLOSED);
+		__l2cap_state_change(chan, BT_CLOSED, 0);
 		sock_set_flag(sk, SOCK_ZAPPED);
 		release_sock(sk);
 		break;
@@ -470,7 +468,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
 				result = L2CAP_CR_SEC_BLOCK;
 			else
 				result = L2CAP_CR_BAD_PSM;
-			l2cap_state_change(chan, BT_DISCONN);
+			l2cap_state_change(chan, BT_DISCONN, 0);
 
 			rsp.scid   = cpu_to_le16(chan->dcid);
 			rsp.dcid   = cpu_to_le16(chan->scid);
@@ -724,7 +722,6 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
 
 static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
 {
-	struct sock *sk = chan->sk;
 	struct l2cap_disconn_req req;
 
 	if (!conn)
@@ -741,10 +738,7 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
 	l2cap_send_cmd(conn, l2cap_get_ident(conn),
 			L2CAP_DISCONN_REQ, sizeof(req), &req);
 
-	lock_sock(sk);
-	__l2cap_state_change(chan, BT_DISCONN);
-	__l2cap_chan_set_err(chan, err);
-	release_sock(sk);
+	l2cap_state_change(chan, BT_DISCONN, err);
 }
 
 /* ---- L2CAP connections ---- */
@@ -799,7 +793,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
 						parent->sk_data_ready(parent, 0);
 
 				} else {
-					__l2cap_state_change(chan, BT_CONFIG);
+					__l2cap_state_change(chan, BT_CONFIG, 0);
 					rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
 					rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
 				}
@@ -903,7 +897,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
 	__set_chan_timer(chan, sk->sk_sndtimeo);
 
-	__l2cap_state_change(chan, BT_CONNECTED);
+	__l2cap_state_change(chan, BT_CONNECTED, 0);
 	parent->sk_data_ready(parent, 0);
 
 clean:
@@ -924,7 +918,7 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
 	chan->conf_state = 0;
 	__clear_chan_timer(chan);
 
-	__l2cap_state_change(chan, BT_CONNECTED);
+	__l2cap_state_change(chan, BT_CONNECTED, 0);
 	sk->sk_state_change(sk);
 
 	if (parent)
@@ -959,7 +953,7 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
 			struct sock *sk = chan->sk;
 			__clear_chan_timer(chan);
 			lock_sock(sk);
-			__l2cap_state_change(chan, BT_CONNECTED);
+			__l2cap_state_change(chan, BT_CONNECTED, 0);
 			sk->sk_state_change(sk);
 			release_sock(sk);
 
@@ -1243,14 +1237,14 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
 	l2cap_chan_add(conn, chan);
 	l2cap_chan_lock(chan);
 
-	l2cap_state_change(chan, BT_CONNECT);
+	l2cap_state_change(chan, BT_CONNECT, 0);
 	__set_chan_timer(chan, sk->sk_sndtimeo);
 
 	if (hcon->state == BT_CONNECTED) {
 		if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
 			__clear_chan_timer(chan);
 			if (l2cap_chan_check_security(chan))
-				l2cap_state_change(chan, BT_CONNECTED);
+				l2cap_state_change(chan, BT_CONNECTED, 0);
 		} else
 			l2cap_do_start(chan);
 	}
@@ -2707,22 +2701,22 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
 	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
 		if (l2cap_chan_check_security(chan)) {
 			if (bt_sk(sk)->defer_setup) {
-				__l2cap_state_change(chan, BT_CONNECT2);
+				__l2cap_state_change(chan, BT_CONNECT2, 0);
 				result = L2CAP_CR_PEND;
 				status = L2CAP_CS_AUTHOR_PEND;
 				parent->sk_data_ready(parent, 0);
 			} else {
-				__l2cap_state_change(chan, BT_CONFIG);
+				__l2cap_state_change(chan, BT_CONFIG, 0);
 				result = L2CAP_CR_SUCCESS;
 				status = L2CAP_CS_NO_INFO;
 			}
 		} else {
-			__l2cap_state_change(chan, BT_CONNECT2);
+			__l2cap_state_change(chan, BT_CONNECT2, 0);
 			result = L2CAP_CR_PEND;
 			status = L2CAP_CS_AUTHEN_PEND;
 		}
 	} else {
-		__l2cap_state_change(chan, BT_CONNECT2);
+		__l2cap_state_change(chan, BT_CONNECT2, 0);
 		result = L2CAP_CR_PEND;
 		status = L2CAP_CS_NO_INFO;
 	}
@@ -2801,7 +2795,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
 
 	switch (result) {
 	case L2CAP_CR_SUCCESS:
-		l2cap_state_change(chan, BT_CONFIG);
+		l2cap_state_change(chan, BT_CONFIG, 0);
 		chan->ident = 0;
 		chan->dcid = dcid;
 		clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
@@ -2913,7 +2907,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
 		set_default_fcs(chan);
 
-		l2cap_state_change(chan, BT_CONNECTED);
+		l2cap_state_change(chan, BT_CONNECTED, 0);
 
 		chan->next_tx_seq = 0;
 		chan->expected_tx_seq = 0;
@@ -3044,7 +3038,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
 	if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
 		set_default_fcs(chan);
 
-		l2cap_state_change(chan, BT_CONNECTED);
+		l2cap_state_change(chan, BT_CONNECTED, 0);
 		chan->next_tx_seq = 0;
 		chan->expected_tx_seq = 0;
 		skb_queue_head_init(&chan->tx_q);
@@ -4614,12 +4608,12 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
 					if (parent)
 						parent->sk_data_ready(parent, 0);
 				} else {
-					__l2cap_state_change(chan, BT_CONFIG);
+					__l2cap_state_change(chan, BT_CONFIG, 0);
 					res = L2CAP_CR_SUCCESS;
 					stat = L2CAP_CS_NO_INFO;
 				}
 			} else {
-				__l2cap_state_change(chan, BT_DISCONN);
+				__l2cap_state_change(chan, BT_DISCONN, 0);
 				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
 				res = L2CAP_CR_SEC_BLOCK;
 				stat = L2CAP_CS_NO_INFO;
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 29122ed..7e26d63 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -919,11 +919,17 @@ static void l2cap_sock_close_cb(void *data)
 	l2cap_sock_kill(sk);
 }
 
-static void l2cap_sock_state_change_cb(void *data, int state)
+static void l2cap_sock_state_change_cb(void *data, int state, int err)
 {
 	struct sock *sk = data;
 
 	sk->sk_state = state;
+
+	if (err) {
+		struct l2cap_chan *chan = l2cap_pi(sk)->chan;
+
+		__l2cap_chan_set_err(chan, err);
+	}
 }
 
 static struct sk_buff *l2cap_sock_alloc_skb_cb(struct l2cap_chan *chan,
-- 
1.7.9.1

[RFCv5 02/26] Bluetooth: Lock sk only if exist in state_change

[Andrei Emeltchenko]

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

In L2CAP state_change lock sk only when we are dealing with
sockets. In A2MP we do not have sk so this will crash.

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

diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 5b9a667..6e801bb 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -212,9 +212,13 @@ static inline void l2cap_state_change(struct l2cap_chan *chan, int state, int er
 {
 	struct sock *sk = chan->sk;
 
-	lock_sock(sk);
+	if (sk)
+		lock_sock(sk);
+
 	__l2cap_state_change(chan, state, err);
-	release_sock(sk);
+
+	if (sk)
+		release_sock(sk);
 }
 
 void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
-- 
1.7.9.1

[RFCv5 03/26] Bluetooth: A2MP: Create A2MP channel

[Andrei Emeltchenko]

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

Create and initialize fixed A2MP channel

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9287c24..0f0ef6c 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -51,6 +51,8 @@
 #define L2CAP_CONN_TIMEOUT		msecs_to_jiffies(40000)
 #define L2CAP_INFO_TIMEOUT		msecs_to_jiffies(4000)
 
+#define L2CAP_A2MP_DEFAULT_MTU		670
+
 /* L2CAP socket address */
 struct sockaddr_l2 {
 	sa_family_t	l2_family;
@@ -224,6 +226,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
@@ -867,7 +870,8 @@ 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);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
-
+void l2cap_ertm_init(struct l2cap_chan *chan);
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 void __l2cap_chan_set_err(struct l2cap_chan *chan, int err);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 2dc5a57..fa6d94a 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_BT_CMTP)	+= cmtp/
 obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
-	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o
+	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
+	a2mp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..967e0f1
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,63 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+static struct l2cap_ops a2mp_chan_ops = {
+	.name = "L2CAP A2MP channel",
+};
+
+static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
+{
+	struct l2cap_chan *chan;
+
+	chan = l2cap_chan_create(NULL);
+
+	hci_conn_hold(conn->hcon);
+
+	BT_DBG("chan %p", chan);
+
+	chan->sec_level = BT_SECURITY_LOW;
+	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->ops = &a2mp_chan_ops;
+
+	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+
+	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
+	chan->remote_max_tx = chan->max_tx;
+
+	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
+	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
+	chan->remote_tx_win = chan->tx_win;
+
+	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);
+
+	l2cap_chan_add(conn, chan);
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	return chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 6e801bb..c3b448b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -352,7 +352,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 	list_add(&chan->list, &conn->chan_l);
 }
 
-static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	mutex_lock(&conn->chan_lock);
 	__l2cap_chan_add(conn, chan);
@@ -2038,7 +2038,7 @@ static void l2cap_ack_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+void l2cap_ertm_init(struct l2cap_chan *chan)
 {
 	chan->expected_ack_seq = 0;
 	chan->unacked_frames = 0;
-- 
1.7.9.1

[RFCv5 04/26] Bluetooth: A2MP: AMP Manager basic functions

[Andrei Emeltchenko]

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     |   30 +++++++++++++++++++++
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   53 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_conn.c         |   15 ++++++++++
 4 files changed, 99 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..0fe8ddd
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,30 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+struct amp_mgr {
+	struct list_head	list;
+	struct l2cap_conn	*l2cap_conn;
+	struct l2cap_chan	*a2mp_chan;
+	struct kref		kref;
+	__u8			ident;
+	unsigned long		flags;
+};
+
+void amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+
+#endif /* __A2MP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 8dc07fa..259d4a1 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -323,6 +323,7 @@ struct hci_conn {
 
 	struct sk_buff_head data_q;
 	struct list_head chan_list;
+	struct list_head mgr_list;
 
 	struct delayed_work disc_work;
 	struct timer_list idle_timer;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 967e0f1..b1a1e14 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -15,6 +15,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
@@ -61,3 +62,55 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 
 	return chan;
 }
+
+/* AMP Manager functions */
+void amp_mgr_get(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", 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_DBG("mgr %p", mgr);
+
+	kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", mgr);
+
+	return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+{
+	struct amp_mgr *mgr;
+
+	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+	if (!mgr)
+		return NULL;
+
+	BT_DBG("conn %p mgr %p", conn, mgr);
+
+	mgr->l2cap_conn = conn;
+
+	mgr->a2mp_chan = open_a2mp_chan(conn);
+	if (!mgr->a2mp_chan) {
+		kfree(mgr);
+		return NULL;
+	}
+
+	mgr->a2mp_chan->data = mgr;
+
+	list_add(&mgr->list, &conn->hcon->mgr_list);
+
+	kref_init(&mgr->kref);
+
+	return mgr;
+}
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 947172b..2de2773 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 static void hci_le_connect(struct hci_conn *conn)
 {
@@ -404,6 +405,8 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 
 	INIT_LIST_HEAD(&conn->chan_list);
 
+	INIT_LIST_HEAD(&conn->mgr_list);
+
 	INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout);
 	setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn);
 	setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept,
@@ -424,6 +427,16 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 	return conn;
 }
 
+static void hci_amp_mgr_list_flush(struct hci_conn *conn)
+{
+	struct amp_mgr *mgr, *n;
+
+	BT_DBG("conn %p", conn);
+
+	list_for_each_entry_safe(mgr, n, &conn->mgr_list, list)
+		amp_mgr_put(mgr);
+}
+
 int hci_conn_del(struct hci_conn *conn)
 {
 	struct hci_dev *hdev = conn->hdev;
@@ -459,6 +472,8 @@ int hci_conn_del(struct hci_conn *conn)
 
 	hci_chan_list_flush(conn);
 
+	hci_amp_mgr_list_flush(conn);
+
 	hci_conn_hash_del(hdev, conn);
 	if (hdev->notify)
 		hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
-- 
1.7.9.1

[RFCv5 05/26] Bluetooth: A2MP: Build and Send msg helpers

[Andrei Emeltchenko]

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         |   48 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 55 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0fe8ddd..995f1c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -24,6 +24,13 @@ struct amp_mgr {
 	unsigned long		flags;
 };
 
+struct a2mp_cmd {
+	__u8	code;
+	__u8	ident;
+	__le16	len;
+	__u8	data[0];
+} __packed;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b1a1e14..84275e8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,54 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* 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 amp_mgr *mgr, u8 *data, int len)
+{
+	struct l2cap_chan *chan = mgr->a2mp_chan;
+	struct kvec iv = { data, len };
+	struct msghdr msg;
+
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_iov = (struct iovec *) &iv;
+	msg.msg_iovlen = 1;
+
+	return l2cap_chan_send(chan, &msg, len, 0);
+}
+
+static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+		      void *data)
+{
+	struct a2mp_cmd *cmd;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	__a2mp_send(mgr, (u8 *)cmd, len + sizeof(*cmd));
+
+	kfree(cmd);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 };
-- 
1.7.9.1

[RFCv5 06/26] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

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

Add state change, close and skb allocation callbacks.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 84275e8..894c6c96 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,8 +65,42 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static void a2mp_chan_state_change_cb(void *data, int state, int err)
+{
+	struct l2cap_chan *chan = data;
+	struct amp_mgr *mgr;
+
+	BT_DBG("chan %p state %s", chan, state_to_string(state));
+
+	chan->state = state;
+
+	switch (state) {
+	case BT_CLOSED:
+		mgr = chan->data;
+		if (mgr)
+			amp_mgr_put(mgr);
+		break;
+	}
+}
+
+static void a2mp_chan_close_cb(void *data)
+{
+	struct amp_mgr *mgr = data;
+
+	l2cap_chan_destroy(mgr->a2mp_chan);
+}
+
+static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
+					unsigned long len, int nb, int *err)
+{
+	return bt_skb_alloc(len, GFP_KERNEL);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.state_change = a2mp_chan_state_change_cb,
+	.close = a2mp_chan_close_cb,
+	.alloc_skb = a2mp_chan_alloc_skb_cb,
 };
 
 static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
@@ -108,6 +142,8 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
 
+	chan->ops->state_change(chan, BT_CONNECTED, 0);
+
 	return chan;
 }
 
-- 
1.7.9.1

[RFCv5 07/26] Bluetooth: A2MP: Definitions for A2MP commands

[Andrei Emeltchenko]

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 995f1c0..6e6202a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,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;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.1

[RFCv5 08/26] Bluetooth: A2MP: Define A2MP status codes

[Andrei Emeltchenko]

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 6e6202a..c99c375 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -103,6 +103,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
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.1

[RFCv5 09/26] Bluetooth: A2MP: Process A2MP messages

[Andrei Emeltchenko]

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 |   61 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 61 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 894c6c96..d2063af 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,66 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Handle A2MP signalling */
+static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+	struct amp_mgr *mgr = data;
+	int err = 0;
+
+	amp_mgr_get(mgr);
+
+	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;
+		}
+
+		mgr->ident = hdr->ident;
+
+		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 sig cmd 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);
+	} else {
+		/* Free if success, otherwise skb will be freed by invoker */
+		kfree_skb(skb);
+	}
+
+	amp_mgr_put(mgr);
+
+	return err;
+}
+
 static void a2mp_chan_state_change_cb(void *data, int state, int err)
 {
 	struct l2cap_chan *chan = data;
@@ -98,6 +158,7 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.recv = a2mp_chan_recv_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.close = a2mp_chan_close_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
-- 
1.7.9.1

[RFCv5 10/26] Bluetooth: A2MP: Process A2MP Command Reject

[Andrei Emeltchenko]

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

Placeholder for future A2MP Command Reject handler.

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 d2063af..a2677d6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -91,6 +107,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 11/26] Bluetooth: A2MP: Helper functions to count HCI devs

[Andrei Emeltchenko]

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 |   13 +++++++++++++
 net/bluetooth/a2mp.c             |   30 ++++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 38ac0e6..3a7bd76 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 259d4a1..6a2fe3f 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -643,6 +643,19 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
 	dev_set_drvdata(&hdev->dev, data);
 }
 
+/* hci_dev_list shall be locked */
+static inline uint8_t __hci_num_ctrl(void)
+{
+	uint8_t count = 0;
+	struct list_head *p;
+
+	list_for_each(p, &hci_dev_list) {
+		count++;
+	}
+
+	return count;
+}
+
 struct hci_dev *hci_dev_get(int index);
 struct hci_dev *hci_get_route(bdaddr_t *src, bdaddr_t *dst);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a2677d6..17659f7 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,36 @@ 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;
+}
+
+/* hci_dev_list shall be locked */
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+{
+	int i = 0;
+	struct hci_dev *hdev;
+
+	__a2mp_cl_bredr(cl);
+
+	list_for_each_entry(hdev, &hci_dev_list, list) {
+		/* Iterate through AMP controllers */
+		if (hdev->id == HCI_BREDR_ID)
+			continue;
+
+		/* Starting from second entry */
+		if (++i >= num_ctrl)
+			return;
+
+		cl[i].id = hdev->id;
+		cl[i].type = hdev->amp_type;
+		cl[i].status = hdev->amp_status;
+	}
+}
+
 /* Processing A2MP messages */
 static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
-- 
1.7.9.1

[RFCv5 12/26] Bluetooth: A2MP: Process A2MP Discover Request

[Andrei Emeltchenko]

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 22
    A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
      Controller list:
        id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only)
        id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only)
...

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c99c375..a748ab0 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 17659f7..6ae20ca 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -111,6 +111,57 @@ 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));
+
+	ext_feat = le16_to_cpu(req->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), 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));
+	}
+
+	read_lock(&hci_dev_list_lock);
+
+	num_ctrl = __hci_num_ctrl();
+	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
+	rsp = kmalloc(len, GFP_ATOMIC);
+	if (!rsp) {
+		read_unlock(&hci_dev_list_lock);
+		return -ENOMEM;
+	}
+
+	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	rsp->ext_feat = 0;
+
+	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+
+	read_unlock(&hci_dev_list_lock);
+
+	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -141,6 +192,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 13/26] Bluetooth: A2MP: Process A2MP Change Notify

[Andrei Emeltchenko]

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

Placeholder for A2MP Change Notify handler.

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 6ae20ca..5781ef5 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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -196,6 +212,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 14/26] Bluetooth: A2MP: Process A2MP Get Info Request

[Andrei Emeltchenko]

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

Process A2MP Get Info Request.
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
...

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 5781ef5..ae60301 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -178,6 +178,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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -216,6 +248,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 15/26] Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

[Andrei Emeltchenko]

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 |   31 +++++++++++++++++++++++++++++++
 1 files changed, 31 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ae60301..5d974c5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -210,6 +210,34 @@ 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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -252,6 +280,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 16/26] Bluetooth: A2MP: Process A2MP Create Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Create Physlink Request.

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 5d974c5..d2d292a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -238,6 +238,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, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -284,6 +300,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 17/26] Bluetooth: A2MP: Process A2MP Disc Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Disconnect Physlink Request.

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 d2d292a..b2d0153 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -254,6 +254,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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -304,6 +338,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv5 18/26] Bluetooth: A2MP: Process A2MP Command Responses

[Andrei Emeltchenko]

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

Process A2MP responses, print cmd code and ident for now.

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 b2d0153..178832e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -288,6 +288,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, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -347,6 +356,9 @@ static int a2mp_chan_recv_cb(void *data, 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 sig cmd 0x%2.2x", hdr->code);
 			err = -EINVAL;
-- 
1.7.9.1

[RFCv5 19/26] Bluetooth: A2MP: Handling fixed channels

[Andrei Emeltchenko]

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

A2MP fixed channel do not have sk

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 0f0ef6c..44ba4c4 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -555,6 +555,7 @@ struct l2cap_conn {
 #define L2CAP_CHAN_RAW			1
 #define L2CAP_CHAN_CONN_LESS		2
 #define L2CAP_CHAN_CONN_ORIENTED	3
+#define L2CAP_CHAN_CONN_FIX_A2MP	4
 
 /* ----- L2CAP socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 178832e..2f7c461 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -430,9 +430,9 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
 
 	BT_DBG("chan %p", chan);
 
+	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
+
 	chan->sec_level = BT_SECURITY_LOW;
-	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
-	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 	chan->fcs = L2CAP_FCS_CRC16;
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index c3b448b..7583eef 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -333,6 +333,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 		chan->omtu = L2CAP_DEFAULT_MTU;
 		break;
 
+	case L2CAP_CHAN_CONN_FIX_A2MP:
+		chan->scid = L2CAP_CID_A2MP;
+		chan->dcid = L2CAP_CID_A2MP;
+		chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+		chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+		break;
+
 	default:
 		/* Raw socket can send/recv signalling messages only */
 		chan->scid = L2CAP_CID_SIGNALING;
@@ -363,7 +370,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
 	struct sock *sk = chan->sk;
 	struct l2cap_conn *conn = chan->conn;
-	struct sock *parent = bt_sk(sk)->parent;
+	struct sock *parent;
 
 	__clear_chan_timer(chan);
 
@@ -379,12 +386,15 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		hci_conn_put(conn->hcon);
 	}
 
+	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+		goto clean;
+
 	lock_sock(sk);
 
 	__l2cap_state_change(chan, BT_CLOSED, err);
-
 	sock_set_flag(sk, SOCK_ZAPPED);
 
+	parent = bt_sk(sk)->parent;
 	if (parent) {
 		bt_accept_unlink(sk);
 		parent->sk_data_ready(parent, 0);
@@ -397,6 +407,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 			test_bit(CONF_INPUT_DONE, &chan->conf_state)))
 		return;
 
+clean:
 	skb_queue_purge(&chan->tx_q);
 
 	if (chan->mode == L2CAP_MODE_ERTM) {
-- 
1.7.9.1

[RFCv5 20/26] Bluetooth: A2MP: Manage incoming connections

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a748ab0..0558088 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+#include <net/bluetooth/l2cap.h>
+
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
@@ -117,5 +119,7 @@ struct a2mp_physlink_rsp {
 
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2f7c461..105e755 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -515,3 +515,15 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	return mgr;
 }
+
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb)
+{
+	struct amp_mgr *mgr;
+
+	mgr = amp_mgr_create(conn);
+
+	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+	return mgr->a2mp_chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 7583eef..8d10e81 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -56,6 +56,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 bool disable_ertm;
 
@@ -4298,10 +4299,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);
-		/* Drop packet and return */
-		kfree_skb(skb);
-		return 0;
+		if (cid == L2CAP_CID_A2MP) {
+			chan = a2mp_channel_create(conn, skb);
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			/* Drop packet and return */
+			kfree_skb(skb);
+			return 0;
+		}
 	}
 
 	l2cap_chan_lock(chan);
-- 
1.7.9.1

[RFCv5 21/26] Bluetooth: physical link HCI interface to AMP

[Andrei Emeltchenko]

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

Adds support for physical link create/acceppt/disconnect AMP HCI
commands. To be used by the upper layer.
Backport from CodeAurora & Atheros

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 3a7bd76..3577b85 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -30,6 +30,8 @@
 #define HCI_MAX_EVENT_SIZE	260
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
+#define HCI_MAX_AMP_KEY_SIZE	32
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
@@ -525,6 +527,28 @@ struct hci_cp_io_capability_neg_reply {
 	__u8     reason;
 } __packed;
 
+#define HCI_OP_CREATE_PHY_LINK		0x0435
+struct hci_cp_create_phy_link {
+	__u8     handle;
+	__u8     key_len;
+	__u8     key_type;
+	__u8     key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_ACCEPT_PHY_LINK		0x0436
+struct hci_cp_accept_phy_link {
+	__u8	handle;
+	__u8	key_len;
+	__u8	key_type;
+	__u8	key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_DISC_PHY_LINK		0x0437
+struct hci_cp_disc_phy_link {
+	__u8     handle;
+	__u8     reason;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 286f3fc..14b727c 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -337,6 +337,51 @@ static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt)
 	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy);
 }
 
+/* AMP HCI interface */
+void hci_phylink_create_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_create_req);
+
+void hci_phylink_accept_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_accept_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_accept_req);
+
+void hci_phylink_disc_req(struct hci_dev *hdev, __u8 handle, __u8 reason)
+{
+	struct hci_cp_disc_phy_link cp;
+
+	BT_DBG("handle %d reason %d", handle, reason);
+
+	cp.handle = handle;
+	cp.reason = reason;
+	hci_send_cmd(hdev, HCI_OP_DISC_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_disc_req);
+
 /* Get HCI device by index.
  * Device is held on return. */
 struct hci_dev *hci_dev_get(int index)
-- 
1.7.9.1

[RFCv5 22/26] Bluetooth: Define AMP controller statuses

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 3577b85..1fefe2f 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -60,6 +60,15 @@
 
 #define HCI_BREDR_ID	0
 
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_NO_RESET,
-- 
1.7.9.1

[RFCv5 23/26] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6a2fe3f..c76e7c5 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -131,6 +131,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -243,6 +254,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1091,4 +1105,10 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 		int timeout);
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
+		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 14b727c..999c438 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -57,6 +57,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1845,6 +1846,9 @@ int hci_register_dev(struct hci_dev *hdev)
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 
+	INIT_LIST_HEAD(&hdev->cb_list);
+	mutex_init(&hdev->cb_list_lock);
+
 	INIT_LIST_HEAD(&hdev->blacklist);
 
 	INIT_LIST_HEAD(&hdev->uuids);
@@ -1961,6 +1965,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
 	hci_adv_entries_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2260,6 +2265,83 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.1

[RFCv5 24/26] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

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

Use workqueue to process HCI callbacks.

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index c76e7c5..b0fa634 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1110,5 +1110,7 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
 		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 999c438..625a138 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2303,6 +2303,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("Queue cmd %p opt %p", cmd, cmd->opt);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	mutex_lock(&hdev->cb_list_lock);
-- 
1.7.9.1

[RFCv5 25/26] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..2751bf6
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 105e755..2b15bf0 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -92,6 +93,10 @@ static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
 		cl[i].id = hdev->id;
 		cl[i].type = hdev->amp_type;
 		cl[i].status = hdev->amp_status;
+
+		read_unlock(&hci_dev_list_lock);
+		amp_read_loc_info(hdev, mgr);
+		read_lock(&hci_dev_list_lock);
 	}
 }
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..af009e0
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,49 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+}
+
+static void cb_destructor(struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	int err = 0;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		amp_read_loc_info_complete, mgr, cb_destructor, GFP_ATOMIC);
+
+	return err;
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 7325300..3053354 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -848,14 +849,15 @@ static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
-		struct sk_buff *skb)
+							struct sk_buff *skb)
 {
 	struct hci_rp_read_local_amp_info *rp = (void *) skb->data;
+	struct hci_cb_cmd *cmd;
 
 	BT_DBG("%s status 0x%x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto send;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -869,6 +871,13 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+send:
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.1

[RFCv5 26/26] Bluetooth: AMP: Read Local Assoc support

[Andrei Emeltchenko]

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

Adds reading AMP Assoc in HCI callback

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0558088..d3ac3fe 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,4 +122,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 
+
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 2751bf6..64d2b44 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1fefe2f..c7ad8ab 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -31,6 +31,7 @@
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
 #define HCI_MAX_AMP_KEY_SIZE	32
+#define HCI_MAX_AMP_ASSOC_SIZE	672
 
 /* HCI dev events */
 #define HCI_DEV_REG			1
@@ -853,6 +854,20 @@ struct hci_rp_read_local_amp_info {
 	__le32   be_flush_to;
 } __packed;
 
+#define HCI_OP_READ_LOCAL_AMP_ASSOC	0x140a
+struct hci_cp_read_local_amp_assoc {
+	__u8     handle;
+	__le16   len_so_far;
+	__le16   max_len;
+} __packed;
+
+struct hci_rp_read_local_amp_assoc {
+	__u8     status;
+	__u8     handle;
+	__le16   rem_len;
+	__u8     frag[0];
+} __packed;
+
 #define HCI_OP_LE_SET_EVENT_MASK	0x2001
 struct hci_cp_le_set_event_mask {
 	__u8     mask[8];
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index b0fa634..57de4a2 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -142,6 +142,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -195,6 +201,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 2b15bf0..ca73e65 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -52,8 +52,7 @@ static inline int __a2mp_send(struct amp_mgr *mgr, u8 *data, int len)
 	return l2cap_chan_send(chan, &msg, len, 0);
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct a2mp_cmd *cmd;
 
@@ -234,8 +233,12 @@ static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
 		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
 
 		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), &rsp);
+		goto done;
 	}
 
+	amp_read_loc_assoc(hdev, mgr);
+
+done:
 	if (hdev)
 		hci_dev_put(hdev);
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index af009e0..c36bdf3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -47,3 +47,62 @@ int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 
 	return err;
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		amp_read_loc_assoc_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 3053354..37e1d11 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -44,6 +44,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -880,6 +881,45 @@ send:
 	}
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	struct hci_cb_cmd *cmd;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto send;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+send:
+	/* Run callback when all fragments received */
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 							struct sk_buff *skb)
 {
@@ -2301,6 +2341,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.1

Re: [RFCv5 01/26] Bluetooth: Add set_err to state_change callback

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-23 18:13:41 +0200]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> There are several places in the code where l2cap_state_change and
> l2cap_chan_set_err used together. With combining them we remove
> socket lock in l2cap_send_disconn_req and simplify code in couple
> of other places with a price of adding extra parameter err to
> state_change.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/l2cap.h |    5 +++-
>  net/bluetooth/l2cap_core.c    |   56 ++++++++++++++++++----------------------
>  net/bluetooth/l2cap_sock.c    |    8 +++++-
>  3 files changed, 36 insertions(+), 33 deletions(-)
> 
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index 35334a0..9287c24 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -506,7 +506,8 @@ struct l2cap_ops {
>  	struct l2cap_chan	*(*new_connection) (void *data);
>  	int			(*recv) (void *data, struct sk_buff *skb);
>  	void			(*close) (void *data);
> -	void			(*state_change) (void *data, int state);
> +	void			(*state_change) (void *data, int state,
> +								int err);
>  	struct sk_buff		*(*alloc_skb) (struct l2cap_chan *chan,
>  					unsigned long len, int nb, int *err);
>  
> @@ -867,4 +868,6 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
>  void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
>  int l2cap_chan_check_security(struct l2cap_chan *chan);
>  
> +void __l2cap_chan_set_err(struct l2cap_chan *chan, int err);
> +
>  #endif /* __L2CAP_H */
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index e66c9da..5b9a667 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -199,25 +199,25 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn)
>  	return 0;
>  }
>  
> -static void __l2cap_state_change(struct l2cap_chan *chan, int state)
> +static void __l2cap_state_change(struct l2cap_chan *chan, int state, int err)
>  {
>  	BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state),
>  						state_to_string(state));
>  
>  	chan->state = state;
> -	chan->ops->state_change(chan->data, state);
> +	chan->ops->state_change(chan->data, state, err);
>  }
>  
> -static void l2cap_state_change(struct l2cap_chan *chan, int state)
> +static inline void l2cap_state_change(struct l2cap_chan *chan, int state, int err)
>  {
>  	struct sock *sk = chan->sk;
>  
>  	lock_sock(sk);
> -	__l2cap_state_change(chan, state);
> +	__l2cap_state_change(chan, state, err);
>  	release_sock(sk);
>  }
>  
> -static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
> +void __l2cap_chan_set_err(struct l2cap_chan *chan, int err)
>  {
>  	struct sock *sk = chan->sk;
>  
> @@ -377,11 +377,9 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
>  
>  	lock_sock(sk);
>  
> -	__l2cap_state_change(chan, BT_CLOSED);
> -	sock_set_flag(sk, SOCK_ZAPPED);
> +	__l2cap_state_change(chan, BT_CLOSED, err);
>  
> -	if (err)
> -		__l2cap_chan_set_err(chan, err);
> +	sock_set_flag(sk, SOCK_ZAPPED);
>  
>  	if (parent) {
>  		bt_accept_unlink(sk);
> @@ -445,7 +443,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
>  		lock_sock(sk);
>  		l2cap_chan_cleanup_listen(sk);
>  
> -		__l2cap_state_change(chan, BT_CLOSED);
> +		__l2cap_state_change(chan, BT_CLOSED, 0);
>  		sock_set_flag(sk, SOCK_ZAPPED);
>  		release_sock(sk);
>  		break;
> @@ -470,7 +468,7 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason)
>  				result = L2CAP_CR_SEC_BLOCK;
>  			else
>  				result = L2CAP_CR_BAD_PSM;
> -			l2cap_state_change(chan, BT_DISCONN);
> +			l2cap_state_change(chan, BT_DISCONN, 0);
>  
>  			rsp.scid   = cpu_to_le16(chan->dcid);
>  			rsp.dcid   = cpu_to_le16(chan->scid);
> @@ -724,7 +722,6 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
>  
>  static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err)
>  {
> -	struct sock *sk = chan->sk;
>  	struct l2cap_disconn_req req;
>  
>  	if (!conn)
> @@ -741,10 +738,7 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
>  	l2cap_send_cmd(conn, l2cap_get_ident(conn),
>  			L2CAP_DISCONN_REQ, sizeof(req), &req);
>  
> -	lock_sock(sk);
> -	__l2cap_state_change(chan, BT_DISCONN);
> -	__l2cap_chan_set_err(chan, err);
> -	release_sock(sk);
> +	l2cap_state_change(chan, BT_DISCONN, err);
>  }
>  
>  /* ---- L2CAP connections ---- */
> @@ -799,7 +793,7 @@ static void l2cap_conn_start(struct l2cap_conn *conn)
>  						parent->sk_data_ready(parent, 0);
>  
>  				} else {
> -					__l2cap_state_change(chan, BT_CONFIG);
> +					__l2cap_state_change(chan, BT_CONFIG, 0);
>  					rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS);
>  					rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO);
>  				}
> @@ -903,7 +897,7 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
>  
>  	__set_chan_timer(chan, sk->sk_sndtimeo);
>  
> -	__l2cap_state_change(chan, BT_CONNECTED);
> +	__l2cap_state_change(chan, BT_CONNECTED, 0);
>  	parent->sk_data_ready(parent, 0);
>  
>  clean:
> @@ -924,7 +918,7 @@ static void l2cap_chan_ready(struct l2cap_chan *chan)
>  	chan->conf_state = 0;
>  	__clear_chan_timer(chan);
>  
> -	__l2cap_state_change(chan, BT_CONNECTED);
> +	__l2cap_state_change(chan, BT_CONNECTED, 0);
>  	sk->sk_state_change(sk);
>  
>  	if (parent)
> @@ -959,7 +953,7 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
>  			struct sock *sk = chan->sk;
>  			__clear_chan_timer(chan);
>  			lock_sock(sk);
> -			__l2cap_state_change(chan, BT_CONNECTED);
> +			__l2cap_state_change(chan, BT_CONNECTED, 0);
>  			sk->sk_state_change(sk);
>  			release_sock(sk);
>  
> @@ -1243,14 +1237,14 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, bdaddr_t *d
>  	l2cap_chan_add(conn, chan);
>  	l2cap_chan_lock(chan);
>  
> -	l2cap_state_change(chan, BT_CONNECT);
> +	l2cap_state_change(chan, BT_CONNECT, 0);
>  	__set_chan_timer(chan, sk->sk_sndtimeo);
>  
>  	if (hcon->state == BT_CONNECTED) {
>  		if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) {
>  			__clear_chan_timer(chan);
>  			if (l2cap_chan_check_security(chan))
> -				l2cap_state_change(chan, BT_CONNECTED);
> +				l2cap_state_change(chan, BT_CONNECTED, 0);
>  		} else
>  			l2cap_do_start(chan);
>  	}
> @@ -2707,22 +2701,22 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd
>  	if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) {
>  		if (l2cap_chan_check_security(chan)) {
>  			if (bt_sk(sk)->defer_setup) {
> -				__l2cap_state_change(chan, BT_CONNECT2);
> +				__l2cap_state_change(chan, BT_CONNECT2, 0);
>  				result = L2CAP_CR_PEND;
>  				status = L2CAP_CS_AUTHOR_PEND;
>  				parent->sk_data_ready(parent, 0);
>  			} else {
> -				__l2cap_state_change(chan, BT_CONFIG);
> +				__l2cap_state_change(chan, BT_CONFIG, 0);
>  				result = L2CAP_CR_SUCCESS;
>  				status = L2CAP_CS_NO_INFO;
>  			}
>  		} else {
> -			__l2cap_state_change(chan, BT_CONNECT2);
> +			__l2cap_state_change(chan, BT_CONNECT2, 0);
>  			result = L2CAP_CR_PEND;
>  			status = L2CAP_CS_AUTHEN_PEND;
>  		}
>  	} else {
> -		__l2cap_state_change(chan, BT_CONNECT2);
> +		__l2cap_state_change(chan, BT_CONNECT2, 0);
>  		result = L2CAP_CR_PEND;
>  		status = L2CAP_CS_NO_INFO;
>  	}
> @@ -2801,7 +2795,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd
>  
>  	switch (result) {
>  	case L2CAP_CR_SUCCESS:
> -		l2cap_state_change(chan, BT_CONFIG);
> +		l2cap_state_change(chan, BT_CONFIG, 0);
>  		chan->ident = 0;
>  		chan->dcid = dcid;
>  		clear_bit(CONF_CONNECT_PEND, &chan->conf_state);
> @@ -2913,7 +2907,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
>  	if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) {
>  		set_default_fcs(chan);
>  
> -		l2cap_state_change(chan, BT_CONNECTED);
> +		l2cap_state_change(chan, BT_CONNECTED, 0);
>  
>  		chan->next_tx_seq = 0;
>  		chan->expected_tx_seq = 0;
> @@ -3044,7 +3038,7 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
>  	if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) {
>  		set_default_fcs(chan);
>  
> -		l2cap_state_change(chan, BT_CONNECTED);
> +		l2cap_state_change(chan, BT_CONNECTED, 0);
>  		chan->next_tx_seq = 0;
>  		chan->expected_tx_seq = 0;
>  		skb_queue_head_init(&chan->tx_q);
> @@ -4614,12 +4608,12 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt)
>  					if (parent)
>  						parent->sk_data_ready(parent, 0);
>  				} else {
> -					__l2cap_state_change(chan, BT_CONFIG);
> +					__l2cap_state_change(chan, BT_CONFIG, 0);
>  					res = L2CAP_CR_SUCCESS;
>  					stat = L2CAP_CS_NO_INFO;
>  				}
>  			} else {
> -				__l2cap_state_change(chan, BT_DISCONN);
> +				__l2cap_state_change(chan, BT_DISCONN, 0);
>  				__set_chan_timer(chan, L2CAP_DISC_TIMEOUT);
>  				res = L2CAP_CR_SEC_BLOCK;
>  				stat = L2CAP_CS_NO_INFO;
> diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
> index 29122ed..7e26d63 100644
> --- a/net/bluetooth/l2cap_sock.c
> +++ b/net/bluetooth/l2cap_sock.c
> @@ -919,11 +919,17 @@ static void l2cap_sock_close_cb(void *data)
>  	l2cap_sock_kill(sk);
>  }
>  
> -static void l2cap_sock_state_change_cb(void *data, int state)
> +static void l2cap_sock_state_change_cb(void *data, int state, int err)
>  {
>  	struct sock *sk = data;
>  
>  	sk->sk_state = state;
> +
> +	if (err) {
> +		struct l2cap_chan *chan = l2cap_pi(sk)->chan;
> +
> +		__l2cap_chan_set_err(chan, err);

Why are you calling back l2cap_core.c here? It doesn't make any sense. You
are adding a extra loop in the code.

	Gustavo

Re: [RFCv5 03/26] Bluetooth: A2MP: Create A2MP channel

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-23 18:13:43 +0200]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Create and initialize fixed A2MP channel
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/l2cap.h |    6 +++-
>  net/bluetooth/Makefile        |    3 +-
>  net/bluetooth/a2mp.c          |   63 +++++++++++++++++++++++++++++++++++++++++
>  net/bluetooth/l2cap_core.c    |    4 +-
>  4 files changed, 72 insertions(+), 4 deletions(-)
>  create mode 100644 net/bluetooth/a2mp.c
> 
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index 9287c24..0f0ef6c 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -51,6 +51,8 @@
>  #define L2CAP_CONN_TIMEOUT		msecs_to_jiffies(40000)
>  #define L2CAP_INFO_TIMEOUT		msecs_to_jiffies(4000)
>  
> +#define L2CAP_A2MP_DEFAULT_MTU		670
> +
>  /* L2CAP socket address */
>  struct sockaddr_l2 {
>  	sa_family_t	l2_family;
> @@ -224,6 +226,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
> @@ -867,7 +870,8 @@ 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);
>  int l2cap_chan_check_security(struct l2cap_chan *chan);
> -
> +void l2cap_ertm_init(struct l2cap_chan *chan);
> +void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
>  void __l2cap_chan_set_err(struct l2cap_chan *chan, int err);
>  
>  #endif /* __L2CAP_H */
> diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
> index 2dc5a57..fa6d94a 100644
> --- a/net/bluetooth/Makefile
> +++ b/net/bluetooth/Makefile
> @@ -9,4 +9,5 @@ obj-$(CONFIG_BT_CMTP)	+= cmtp/
>  obj-$(CONFIG_BT_HIDP)	+= hidp/
>  
>  bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
> -	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o
> +	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
> +	a2mp.o
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> new file mode 100644
> index 0000000..967e0f1
> --- /dev/null
> +++ b/net/bluetooth/a2mp.c
> @@ -0,0 +1,63 @@
> +/*
> +   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#include <net/bluetooth/bluetooth.h>
> +#include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/l2cap.h>
> +
> +static struct l2cap_ops a2mp_chan_ops = {
> +	.name = "L2CAP A2MP channel",
> +};
> +
> +static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)

a2mp_chan_open() is a better name here. Who is calling this btw?

> +{
> +	struct l2cap_chan *chan;
> +
> +	chan = l2cap_chan_create(NULL);

I just pushed a patch that remove the sk parameter from l2cap_chan_create(),
so you don't need to pass NULL here anymore.

You failing to check l2cap_chan_create() for NULL return btw.

> +
> +	hci_conn_hold(conn->hcon);
> +
> +	BT_DBG("chan %p", chan);
> +
> +	chan->sec_level = BT_SECURITY_LOW;
> +	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->ops = &a2mp_chan_ops;
> +
> +	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
> +
> +	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
> +	chan->remote_max_tx = chan->max_tx;
> +
> +	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
> +	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
> +	chan->remote_tx_win = chan->tx_win;
> +
> +	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
> +	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
> +
> +	skb_queue_head_init(&chan->tx_q);

I think we should move all ERTM related settings here inside
l2cap_ertm_init(), it will make this code much more cleaner.

	Gustavo

Re: [RFCv5 06/26] Bluetooth: A2MP: Add chan callbacks

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-23 18:13:46 +0200]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Add state change, close and skb allocation callbacks.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/a2mp.c |   36 ++++++++++++++++++++++++++++++++++++
>  1 files changed, 36 insertions(+), 0 deletions(-)
> 
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 84275e8..894c6c96 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -65,8 +65,42 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
>  	kfree(cmd);
>  }
>  
> +static void a2mp_chan_state_change_cb(void *data, int state, int err)
> +{
> +	struct l2cap_chan *chan = data;
> +	struct amp_mgr *mgr;
> +
> +	BT_DBG("chan %p state %s", chan, state_to_string(state));
> +
> +	chan->state = state;
> +
> +	switch (state) {
> +	case BT_CLOSED:
> +		mgr = chan->data;
> +		if (mgr)
> +			amp_mgr_put(mgr);
> +		break;
> +	}
> +}
> +
> +static void a2mp_chan_close_cb(void *data)
> +{
> +	struct amp_mgr *mgr = data;
> +
> +	l2cap_chan_destroy(mgr->a2mp_chan);
> +}
> +
> +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> +					unsigned long len, int nb, int *err)

Wrong coding style here. As we just changed our coding style check the rest of
this patchset for errors like this

> +{
> +	return bt_skb_alloc(len, GFP_KERNEL);
> +}
> +
>  static struct l2cap_ops a2mp_chan_ops = {
>  	.name = "L2CAP A2MP channel",
> +	.state_change = a2mp_chan_state_change_cb,
> +	.close = a2mp_chan_close_cb,
> +	.alloc_skb = a2mp_chan_alloc_skb_cb,
>  };
>  
>  static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
> @@ -108,6 +142,8 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
>  	chan->remote_mps = chan->omtu;
>  	chan->mps = chan->omtu;
>  
> +	chan->ops->state_change(chan, BT_CONNECTED, 0);

This is the same of calling a2mp_chan_alloc_skb_cb() directly. ops is intended
to be used outside of this file, not inside it.

	Gustavo

Re: [RFCv5 03/26] Bluetooth: A2MP: Create A2MP channel

[Andrei Emeltchenko]

Hi Gustavo,

On Sun, Mar 25, 2012 at 02:12:25PM -0300, Gustavo Padovan wrote:
> > +static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
> 
> a2mp_chan_open() is a better name here. Who is calling this btw?

Agree, will change it; it is called by amp_mgr_create in the following
patch.

> > +{
> > +	struct l2cap_chan *chan;
> > +
> > +	chan = l2cap_chan_create(NULL);
> 
> I just pushed a patch that remove the sk parameter from l2cap_chan_create(),
> so you don't need to pass NULL here anymore.

Good! I did not know about this. BTW: Shall all patches go through mailing list
first?

> You failing to check l2cap_chan_create() for NULL return btw.

Will fix.

> > +
> > +	hci_conn_hold(conn->hcon);
> > +
> > +	BT_DBG("chan %p", chan);
> > +
> > +	chan->sec_level = BT_SECURITY_LOW;
> > +	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->ops = &a2mp_chan_ops;
> > +
> > +	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
> > +
> > +	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
> > +	chan->remote_max_tx = chan->max_tx;
> > +
> > +	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
> > +	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
> > +	chan->remote_tx_win = chan->tx_win;
> > +
> > +	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
> > +	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
> > +
> > +	skb_queue_head_init(&chan->tx_q);
> 
> I think we should move all ERTM related settings here inside
> l2cap_ertm_init(), it will make this code much more cleaner.

which settings? retrans_timeout and monitor_timeout are set in
configuration phase before l2cap_ertm_init and putting them
to l2cap_ertm_init would override them.

I can make a function assigning basic L2CAP settings for A2MP and for
general L2CAP channels (those in the beginning).

Best regards 
Andrei Emeltchenko 

Re: [RFCv5 06/26] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

Hi Gustavo,

On Sun, Mar 25, 2012 at 02:16:12PM -0300, Gustavo Padovan wrote:
...

> > +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> > +					unsigned long len, int nb, int *err)
> 
> Wrong coding style here. As we just changed our coding style check the rest of
> this patchset for errors like this

I believe that here the style is OK. Otherwise we would have to make
several lines or go outside 80 characters.

> > +{
> > +	return bt_skb_alloc(len, GFP_KERNEL);
> > +}
> > +
> >  static struct l2cap_ops a2mp_chan_ops = {
> >  	.name = "L2CAP A2MP channel",
> > +	.state_change = a2mp_chan_state_change_cb,
> > +	.close = a2mp_chan_close_cb,
> > +	.alloc_skb = a2mp_chan_alloc_skb_cb,
> >  };
> >  
> >  static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
> > @@ -108,6 +142,8 @@ static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
> >  	chan->remote_mps = chan->omtu;
> >  	chan->mps = chan->omtu;
> >  
> > +	chan->ops->state_change(chan, BT_CONNECTED, 0);
> 
> This is the same of calling a2mp_chan_alloc_skb_cb() directly. ops is intended
> to be used outside of this file, not inside it.

You probably mean a2mp_chan_state_change_cb? I will change to something
even simpler.

Best regards 
Andrei Emeltchenko 

Re: [RFCv5 03/26] Bluetooth: A2MP: Create A2MP channel

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-26 12:27:39 +0300]:

> Hi Gustavo,
> 
> On Sun, Mar 25, 2012 at 02:12:25PM -0300, Gustavo Padovan wrote:
> > > +static struct l2cap_chan *open_a2mp_chan(struct l2cap_conn *conn)
> > 
> > a2mp_chan_open() is a better name here. Who is calling this btw?
> 
> Agree, will change it; it is called by amp_mgr_create in the following
> patch.
> 
> > > +{
> > > +	struct l2cap_chan *chan;
> > > +
> > > +	chan = l2cap_chan_create(NULL);
> > 
> > I just pushed a patch that remove the sk parameter from l2cap_chan_create(),
> > so you don't need to pass NULL here anymore.
> 
> Good! I did not know about this. BTW: Shall all patches go through mailing list
> first?

I committed just after see your patch, it was a simple patch.

> 
> > You failing to check l2cap_chan_create() for NULL return btw.
> 
> Will fix.
> 
> > > +
> > > +	hci_conn_hold(conn->hcon);
> > > +
> > > +	BT_DBG("chan %p", chan);
> > > +
> > > +	chan->sec_level = BT_SECURITY_LOW;
> > > +	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->ops = &a2mp_chan_ops;
> > > +
> > > +	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
> > > +
> > > +	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
> > > +	chan->remote_max_tx = chan->max_tx;
> > > +
> > > +	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
> > > +	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
> > > +	chan->remote_tx_win = chan->tx_win;
> > > +
> > > +	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
> > > +	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
> > > +
> > > +	skb_queue_head_init(&chan->tx_q);
> > 
> > I think we should move all ERTM related settings here inside
> > l2cap_ertm_init(), it will make this code much more cleaner.
> 
> which settings? retrans_timeout and monitor_timeout are set in
> configuration phase before l2cap_ertm_init and putting them
> to l2cap_ertm_init would override them.
> 
> I can make a function assigning basic L2CAP settings for A2MP and for
> general L2CAP channels (those in the beginning).

Ok, let's go with this approach then.

	Gustavo

Re: [RFCv5 06/26] Bluetooth: A2MP: Add chan callbacks

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-26 14:59:52 +0300]:

> Hi Gustavo,
> 
> On Sun, Mar 25, 2012 at 02:16:12PM -0300, Gustavo Padovan wrote:
> ...
> 
> > > +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> > > +					unsigned long len, int nb, int *err)
> > 
> > Wrong coding style here. As we just changed our coding style check the rest of
> > this patchset for errors like this
> 
> I believe that here the style is OK. Otherwise we would have to make
> several lines or go outside 80 characters.

Then break it in 3 lines. You have to align everything after the opening "(".

	Gustavo

[RFCv6 00/26] RFC Bluetooth A2MP implementation

[Andrei Emeltchenko]

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

Basic A2MP implementation, callback interface to HCI and several usages of the interface.

Changes:
	* RFCv6: Remove some unneded check for sk since they are in different path
	(A2MP decoded as a data channel wrt L2CAP signalling channel). Style fixes.
	* RFCv5: Fix memory leaks, sparse warnings and taking comments from upstream.
	* RFCv4: redesign code to use l2cap channel lock instead of socket lock
	and general modifications. Basic implementation of HCI callback interface.
	* RFCv3: redesign code to use l2cap functions instead of kernel sockets
	L2CAP functions modified to work with channels without sk.
	* RFCv2: rebased against "workqueue" patches.
	* RFCv1: added refcnt to amp_mgr, fixed sleeping in atomic

Initially code was based of reference implementations below:

References: 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.

Andrei Emeltchenko (26):
  Bluetooth: Add Read Local AMP Info to init
  Bluetooth: Adds set_default function in L2CAP setup
  Bluetooth: A2MP: Create A2MP channel
  Bluetooth: A2MP: AMP Manager basic functions
  Bluetooth: A2MP: Build and Send msg helpers
  Bluetooth: A2MP: Add chan callbacks
  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 channels
  Bluetooth: A2MP: Manage incoming connections
  Bluetooth: physical link HCI interface to AMP
  Bluetooth: Define AMP controller statuses
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Read Local Assoc support

 include/net/bluetooth/a2mp.h     |  128 +++++++++
 include/net/bluetooth/amp.h      |   21 ++
 include/net/bluetooth/hci.h      |   50 ++++
 include/net/bluetooth/hci_core.h |   44 +++
 include/net/bluetooth/l2cap.h    |    7 +
 net/bluetooth/Makefile           |    3 +-
 net/bluetooth/a2mp.c             |  558 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/amp.c              |  108 ++++++++
 net/bluetooth/hci_conn.c         |   15 +
 net/bluetooth/hci_core.c         |  171 ++++++++++++
 net/bluetooth/hci_event.c        |   57 ++++-
 net/bluetooth/l2cap_core.c       |   45 +++-
 net/bluetooth/l2cap_sock.c       |    8 +-
 13 files changed, 1198 insertions(+), 17 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/a2mp.c
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.9.1

[RFCv6 01/26] Bluetooth: Add Read Local AMP Info to init

[Andrei Emeltchenko]

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

AMP Info will be used in Discovery Response.

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

diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 4e9a164..f807146 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -253,6 +253,9 @@ static void amp_init(struct hci_dev *hdev)
 
 	/* Read Local Version */
 	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_VERSION, 0, NULL);
+
+	/* Read Local AMP Info */
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
 }
 
 static void hci_init_req(struct hci_dev *hdev, unsigned long opt)
-- 
1.7.9.1

[RFCv6 02/26] Bluetooth: Adds set_default function in L2CAP setup

[Andrei Emeltchenko]

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

Some parameters in L2CAP chan are set to default similar way in
socket based channels and A2MP channels. Adds common function which
sets all defaults.

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index f6f0500..c70e2cf 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -915,5 +915,6 @@ 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);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
+void l2cap_chan_set_defaults(struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 3caff27..6ce16db 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -297,6 +297,17 @@ void l2cap_chan_destroy(struct l2cap_chan *chan)
 	l2cap_chan_put(chan);
 }
 
+void l2cap_chan_set_defaults(struct l2cap_chan *chan)
+{
+	chan->fcs  = L2CAP_FCS_CRC16;
+	chan->max_tx = L2CAP_DEFAULT_MAX_TX;
+	chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
+	chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
+	chan->sec_level = BT_SECURITY_LOW;
+
+	set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+}
+
 static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn,
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index 1d3e9c3..ae1d78e 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -1006,12 +1006,8 @@ static void l2cap_sock_init(struct sock *sk, struct sock *parent)
 		} else {
 			chan->mode = L2CAP_MODE_BASIC;
 		}
-		chan->max_tx = L2CAP_DEFAULT_MAX_TX;
-		chan->fcs  = L2CAP_FCS_CRC16;
-		chan->tx_win = L2CAP_DEFAULT_TX_WINDOW;
-		chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW;
-		chan->sec_level = BT_SECURITY_LOW;
-		set_bit(FLAG_FORCE_ACTIVE, &chan->flags);
+
+		l2cap_chan_set_defaults(chan);
 	}
 
 	/* Default config options */
-- 
1.7.9.1

[RFCv6 03/26] Bluetooth: A2MP: Create A2MP channel

[Andrei Emeltchenko]

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

Create and initialize fixed A2MP channel

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index c70e2cf..09cbd33 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -51,6 +51,8 @@
 #define L2CAP_CONN_TIMEOUT		msecs_to_jiffies(40000)
 #define L2CAP_INFO_TIMEOUT		msecs_to_jiffies(4000)
 
+#define L2CAP_A2MP_DEFAULT_MTU		670
+
 /* L2CAP socket address */
 struct sockaddr_l2 {
 	sa_family_t	l2_family;
@@ -230,6 +232,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
@@ -916,5 +919,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
+void l2cap_ertm_init(struct l2cap_chan *chan);
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 2dc5a57..fa6d94a 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_BT_CMTP)	+= cmtp/
 obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
-	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o
+	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
+	a2mp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..1f733b5
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,59 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+static struct l2cap_ops a2mp_chan_ops = {
+	.name = "L2CAP A2MP channel",
+};
+
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+{
+	struct l2cap_chan *chan;
+
+	chan = l2cap_chan_create();
+	if (!chan)
+		return NULL;
+
+	BT_DBG("chan %p", chan);
+
+	hci_conn_hold(conn->hcon);
+
+	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
+
+	chan->ops = &a2mp_chan_ops;
+
+	l2cap_chan_set_defaults(chan);
+	chan->remote_max_tx = chan->max_tx;
+	chan->remote_tx_win = chan->tx_win;
+
+	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);
+
+	l2cap_chan_add(conn, chan);
+
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	return chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 6ce16db..a080b00 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -357,7 +357,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 	list_add(&chan->list, &conn->chan_l);
 }
 
-static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	mutex_lock(&conn->chan_lock);
 	__l2cap_chan_add(conn, chan);
@@ -2049,7 +2049,7 @@ static void l2cap_ack_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-static inline void l2cap_ertm_init(struct l2cap_chan *chan)
+void l2cap_ertm_init(struct l2cap_chan *chan)
 {
 	chan->expected_ack_seq = 0;
 	chan->unacked_frames = 0;
-- 
1.7.9.1

[RFCv6 04/26] Bluetooth: A2MP: AMP Manager basic functions

[Andrei Emeltchenko]

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     |   30 ++++++++++++++++++++
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   55 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_conn.c         |   15 ++++++++++
 4 files changed, 101 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..0fe8ddd
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,30 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+struct amp_mgr {
+	struct list_head	list;
+	struct l2cap_conn	*l2cap_conn;
+	struct l2cap_chan	*a2mp_chan;
+	struct kref		kref;
+	__u8			ident;
+	unsigned long		flags;
+};
+
+void amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+
+#endif /* __A2MP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index c8d5beb..e411d4e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -323,6 +323,7 @@ struct hci_conn {
 
 	struct sk_buff_head data_q;
 	struct list_head chan_list;
+	struct list_head mgr_list;
 
 	struct delayed_work disc_work;
 	struct timer_list idle_timer;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 1f733b5..7e707ce 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -15,6 +15,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
@@ -57,3 +58,57 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	return chan;
 }
+
+/* AMP Manager functions */
+void amp_mgr_get(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", 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_DBG("mgr %p", mgr);
+
+	kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", mgr);
+
+	return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+{
+	struct amp_mgr *mgr;
+	struct l2cap_chan *chan;
+
+	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+	if (!mgr)
+		return NULL;
+
+	BT_DBG("conn %p mgr %p", conn, mgr);
+
+	mgr->l2cap_conn = conn;
+
+	chan = a2mp_chan_open(conn);
+	if (!chan) {
+		kfree(mgr);
+		return NULL;
+	}
+
+	mgr->a2mp_chan = chan;
+	chan->data = mgr;
+
+	list_add(&mgr->list, &conn->hcon->mgr_list);
+
+	kref_init(&mgr->kref);
+
+	return mgr;
+}
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 947172b..2de2773 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 static void hci_le_connect(struct hci_conn *conn)
 {
@@ -404,6 +405,8 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 
 	INIT_LIST_HEAD(&conn->chan_list);
 
+	INIT_LIST_HEAD(&conn->mgr_list);
+
 	INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout);
 	setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn);
 	setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept,
@@ -424,6 +427,16 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 	return conn;
 }
 
+static void hci_amp_mgr_list_flush(struct hci_conn *conn)
+{
+	struct amp_mgr *mgr, *n;
+
+	BT_DBG("conn %p", conn);
+
+	list_for_each_entry_safe(mgr, n, &conn->mgr_list, list)
+		amp_mgr_put(mgr);
+}
+
 int hci_conn_del(struct hci_conn *conn)
 {
 	struct hci_dev *hdev = conn->hdev;
@@ -459,6 +472,8 @@ int hci_conn_del(struct hci_conn *conn)
 
 	hci_chan_list_flush(conn);
 
+	hci_amp_mgr_list_flush(conn);
+
 	hci_conn_hash_del(hdev, conn);
 	if (hdev->notify)
 		hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
-- 
1.7.9.1

[RFCv6 05/26] Bluetooth: A2MP: Build and Send msg helpers

[Andrei Emeltchenko]

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         |   48 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 55 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0fe8ddd..995f1c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -24,6 +24,13 @@ struct amp_mgr {
 	unsigned long		flags;
 };
 
+struct a2mp_cmd {
+	__u8	code;
+	__u8	ident;
+	__le16	len;
+	__u8	data[0];
+} __packed;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 7e707ce..b572f9f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,54 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* 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 amp_mgr *mgr, u8 *data, int len)
+{
+	struct l2cap_chan *chan = mgr->a2mp_chan;
+	struct kvec iv = { data, len };
+	struct msghdr msg;
+
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_iov = (struct iovec *) &iv;
+	msg.msg_iovlen = 1;
+
+	return l2cap_chan_send(chan, &msg, len, 0);
+}
+
+static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+		      void *data)
+{
+	struct a2mp_cmd *cmd;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	__a2mp_send(mgr, (u8 *)cmd, len + sizeof(*cmd));
+
+	kfree(cmd);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 };
-- 
1.7.9.1

[RFCv6 06/26] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

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

Add state change, close and skb allocation callbacks.

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 b572f9f..980382b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,8 +65,43 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static void a2mp_chan_state_change_cb(void *data, int state)
+{
+	struct l2cap_chan *chan = data;
+	struct amp_mgr *mgr;
+
+	BT_DBG("chan %p state %s", chan, state_to_string(state));
+
+	chan->state = state;
+
+	switch (state) {
+	case BT_CLOSED:
+		mgr = chan->data;
+		if (mgr)
+			amp_mgr_put(mgr);
+		break;
+	}
+}
+
+static void a2mp_chan_close_cb(void *data)
+{
+	struct amp_mgr *mgr = data;
+
+	l2cap_chan_destroy(mgr->a2mp_chan);
+}
+
+static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
+					      unsigned long len, int nb,
+					      int *err)
+{
+	return bt_skb_alloc(len, GFP_KERNEL);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.state_change = a2mp_chan_state_change_cb,
+	.close = a2mp_chan_close_cb,
+	.alloc_skb = a2mp_chan_alloc_skb_cb,
 };
 
 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
@@ -104,6 +139,8 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
 
+	a2mp_chan_state_change_cb(chan, BT_CONNECTED);
+
 	return chan;
 }
 
-- 
1.7.9.1

[RFCv6 07/26] Bluetooth: A2MP: Definitions for A2MP commands

[Andrei Emeltchenko]

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 995f1c0..6e6202a 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,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;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.1

[RFCv6 08/26] Bluetooth: A2MP: Define A2MP status codes

[Andrei Emeltchenko]

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 6e6202a..c99c375 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -103,6 +103,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
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.1

[RFCv6 09/26] Bluetooth: A2MP: Process A2MP messages

[Andrei Emeltchenko]

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 |   62 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 files changed, 62 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 980382b..25d505e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,67 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Handle A2MP signalling */
+static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+	struct amp_mgr *mgr = data;
+	int err = 0;
+
+	amp_mgr_get(mgr);
+
+	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;
+		}
+
+		mgr->ident = hdr->ident;
+
+		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 sig cmd 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);
+	}
+
+	/* Always free skb and return success error code to prevent
+	   from sending L2CAP Disconnect over A2MP channel */
+	kfree_skb(skb);
+
+	amp_mgr_put(mgr);
+
+	return 0;
+}
+
 static void a2mp_chan_state_change_cb(void *data, int state)
 {
 	struct l2cap_chan *chan = data;
@@ -99,6 +160,7 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.recv = a2mp_chan_recv_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.close = a2mp_chan_close_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
-- 
1.7.9.1

[RFCv6 10/26] Bluetooth: A2MP: Process A2MP Command Reject

[Andrei Emeltchenko]

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

Placeholder for future A2MP Command Reject handler.

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 25d505e..745ab0c 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -91,6 +107,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 11/26] Bluetooth: A2MP: Helper functions to count HCI devs

[Andrei Emeltchenko]

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 |   13 +++++++++++++
 net/bluetooth/a2mp.c             |   30 ++++++++++++++++++++++++++++++
 3 files changed, 45 insertions(+), 0 deletions(-)

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 38ac0e6..3a7bd76 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 e411d4e..9f9a0d2 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -643,6 +643,19 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
 	dev_set_drvdata(&hdev->dev, data);
 }
 
+/* hci_dev_list shall be locked */
+static inline uint8_t __hci_num_ctrl(void)
+{
+	uint8_t count = 0;
+	struct list_head *p;
+
+	list_for_each(p, &hci_dev_list) {
+		count++;
+	}
+
+	return count;
+}
+
 struct hci_dev *hci_dev_get(int index);
 struct hci_dev *hci_get_route(bdaddr_t *src, bdaddr_t *dst);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 745ab0c..bb3de0f 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -65,6 +65,36 @@ 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;
+}
+
+/* hci_dev_list shall be locked */
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+{
+	int i = 0;
+	struct hci_dev *hdev;
+
+	__a2mp_cl_bredr(cl);
+
+	list_for_each_entry(hdev, &hci_dev_list, list) {
+		/* Iterate through AMP controllers */
+		if (hdev->id == HCI_BREDR_ID)
+			continue;
+
+		/* Starting from second entry */
+		if (++i >= num_ctrl)
+			return;
+
+		cl[i].id = hdev->id;
+		cl[i].type = hdev->amp_type;
+		cl[i].status = hdev->amp_status;
+	}
+}
+
 /* Processing A2MP messages */
 static inline int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 				   struct a2mp_cmd *hdr)
-- 
1.7.9.1

[RFCv6 12/26] Bluetooth: A2MP: Process A2MP Discover Request

[Andrei Emeltchenko]

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 22
    A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
      Controller list:
        id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only)
        id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only)
...

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c99c375..a748ab0 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 bb3de0f..1ec2fc8 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -111,6 +111,57 @@ 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));
+
+	ext_feat = le16_to_cpu(req->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), 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));
+	}
+
+	read_lock(&hci_dev_list_lock);
+
+	num_ctrl = __hci_num_ctrl();
+	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
+	rsp = kmalloc(len, GFP_ATOMIC);
+	if (!rsp) {
+		read_unlock(&hci_dev_list_lock);
+		return -ENOMEM;
+	}
+
+	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	rsp->ext_feat = 0;
+
+	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+
+	read_unlock(&hci_dev_list_lock);
+
+	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -141,6 +192,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 13/26] Bluetooth: A2MP: Process A2MP Change Notify

[Andrei Emeltchenko]

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

Placeholder for A2MP Change Notify handler.

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 1ec2fc8..d8a74a8 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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -196,6 +212,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 14/26] Bluetooth: A2MP: Process A2MP Get Info Request

[Andrei Emeltchenko]

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

Process A2MP Get Info Request.
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
...

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 |   37 +++++++++++++++++++++++++++++++++++++
 1 files changed, 37 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index d8a74a8..5bb4f32 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 a2mp_info_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("id %d", req->id);
+
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	hdev = hci_dev_get(req->id);
+	if (hdev && hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	if (hdev)
+		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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -216,6 +250,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 15/26] Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

[Andrei Emeltchenko]

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 |   36 ++++++++++++++++++++++++++++++++++++
 1 files changed, 36 insertions(+), 0 deletions(-)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5bb4f32..95f5bcd 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -212,6 +212,39 @@ 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);
+		goto clean;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
+
+clean:
+	if (hdev)
+		hci_dev_put(hdev);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -254,6 +287,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 16/26] Bluetooth: A2MP: Process A2MP Create Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Create Physlink Request.
Handles requests with invalid controler id as shown below:

...
> ACL data: handle 11 flags 0x02 dlen 50
    A2MP: Create Physical Link req: local id 1 remote id 85
      Assoc data:
        <skipped>
< ACL data: handle 11 flags 0x00 dlen 15
    A2MP: Create Physical Link rsp: local id 85 remote id 1 status 1
      Invalid Controller ID
...

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 95f5bcd..a5f831b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -245,6 +245,42 @@ clean:
 	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;
+	struct a2mp_physlink_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+
+	hdev = hci_dev_get(req->remote_id);
+	if (!hdev || hdev->amp_type != HCI_AMP) {
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send_rsp;
+	}
+
+	/* TODO process physlink create */
+
+	rsp.status = A2MP_STATUS_SUCCESS;
+
+send_rsp:
+	if (hdev)
+		hci_dev_put(hdev);
+
+	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
+		  &rsp);
+
+	skb_pull(skb, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -291,6 +327,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 17/26] Bluetooth: A2MP: Process A2MP Disc Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Disconnect Physlink Request.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a5f831b..177a066 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -281,6 +281,39 @@ send_rsp:
 	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;
+
+	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+	rsp.status = A2MP_STATUS_SUCCESS;
+
+	hdev = hci_dev_get(req->local_id);
+	if (!hdev) {
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send_rsp;
+	}
+
+	/* TODO Disconnect Phys Link here */
+
+	hci_dev_put(hdev);
+
+send_rsp:
+	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -331,6 +364,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.1

[RFCv6 18/26] Bluetooth: A2MP: Process A2MP Command Responses

[Andrei Emeltchenko]

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

Process A2MP responses, print cmd code and ident for now.

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 177a066..0453372 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -314,6 +314,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, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -373,6 +382,9 @@ static int a2mp_chan_recv_cb(void *data, 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 sig cmd 0x%2.2x", hdr->code);
 			err = -EINVAL;
-- 
1.7.9.1

[RFCv6 19/26] Bluetooth: A2MP: Handling fixed channels

[Andrei Emeltchenko]

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

A2MP fixed channel do not have sk

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 09cbd33..13ed508 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -565,6 +565,7 @@ struct l2cap_conn {
 #define L2CAP_CHAN_RAW			1
 #define L2CAP_CHAN_CONN_LESS		2
 #define L2CAP_CHAN_CONN_ORIENTED	3
+#define L2CAP_CHAN_CONN_FIX_A2MP	4
 
 /* ----- L2CAP socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0453372..dcb86c5 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -460,8 +460,7 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	hci_conn_hold(conn->hcon);
 
-	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
-	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 
 	chan->ops = &a2mp_chan_ops;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index a080b00..483ec9f 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -338,6 +338,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 		chan->omtu = L2CAP_DEFAULT_MTU;
 		break;
 
+	case L2CAP_CHAN_CONN_FIX_A2MP:
+		chan->scid = L2CAP_CID_A2MP;
+		chan->dcid = L2CAP_CID_A2MP;
+		chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+		chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+		break;
+
 	default:
 		/* Raw socket can send/recv signalling messages only */
 		chan->scid = L2CAP_CID_SIGNALING;
@@ -368,7 +375,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
 	struct sock *sk = chan->sk;
 	struct l2cap_conn *conn = chan->conn;
-	struct sock *parent = bt_sk(sk)->parent;
+	struct sock *parent;
 
 	__clear_chan_timer(chan);
 
@@ -384,6 +391,9 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		hci_conn_put(conn->hcon);
 	}
 
+	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+		goto clean;
+
 	lock_sock(sk);
 
 	__l2cap_state_change(chan, BT_CLOSED);
@@ -392,6 +402,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 	if (err)
 		__l2cap_chan_set_err(chan, err);
 
+	parent = bt_sk(sk)->parent;
 	if (parent) {
 		bt_accept_unlink(sk);
 		parent->sk_data_ready(parent, 0);
@@ -404,6 +415,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 			test_bit(CONF_INPUT_DONE, &chan->conf_state)))
 		return;
 
+clean:
 	skb_queue_purge(&chan->tx_q);
 
 	if (chan->mode == L2CAP_MODE_ERTM) {
@@ -1315,7 +1327,8 @@ static void l2cap_monitor_timeout(struct work_struct *work)
 	l2cap_chan_lock(chan);
 
 	if (chan->retry_count >= chan->remote_max_tx) {
-		l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
+		if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
+			l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
 		l2cap_chan_unlock(chan);
 		l2cap_chan_put(chan);
 		return;
-- 
1.7.9.1

[RFCv6 20/26] Bluetooth: A2MP: Manage incoming connections

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index a748ab0..0558088 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+#include <net/bluetooth/l2cap.h>
+
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
@@ -117,5 +119,7 @@ struct a2mp_physlink_rsp {
 
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index dcb86c5..243f7d1 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -540,3 +540,15 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	return mgr;
 }
+
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb)
+{
+	struct amp_mgr *mgr;
+
+	mgr = amp_mgr_create(conn);
+
+	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+	return mgr->a2mp_chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 483ec9f..4f0dcb9 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -56,6 +56,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 bool disable_ertm;
 
@@ -4311,10 +4312,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);
-		/* Drop packet and return */
-		kfree_skb(skb);
-		return 0;
+		if (cid == L2CAP_CID_A2MP) {
+			chan = a2mp_channel_create(conn, skb);
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			/* Drop packet and return */
+			kfree_skb(skb);
+			return 0;
+		}
 	}
 
 	l2cap_chan_lock(chan);
-- 
1.7.9.1

[RFCv6 21/26] Bluetooth: physical link HCI interface to AMP

[Andrei Emeltchenko]

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

Adds support for physical link create/acceppt/disconnect AMP HCI
commands. To be used by the upper layer.
Backport from CodeAurora & Atheros

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 3a7bd76..3577b85 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -30,6 +30,8 @@
 #define HCI_MAX_EVENT_SIZE	260
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
+#define HCI_MAX_AMP_KEY_SIZE	32
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
@@ -525,6 +527,28 @@ struct hci_cp_io_capability_neg_reply {
 	__u8     reason;
 } __packed;
 
+#define HCI_OP_CREATE_PHY_LINK		0x0435
+struct hci_cp_create_phy_link {
+	__u8     handle;
+	__u8     key_len;
+	__u8     key_type;
+	__u8     key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_ACCEPT_PHY_LINK		0x0436
+struct hci_cp_accept_phy_link {
+	__u8	handle;
+	__u8	key_len;
+	__u8	key_type;
+	__u8	key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_DISC_PHY_LINK		0x0437
+struct hci_cp_disc_phy_link {
+	__u8     handle;
+	__u8     reason;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index f807146..f27e3e8 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -340,6 +340,51 @@ static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt)
 	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy);
 }
 
+/* AMP HCI interface */
+void hci_phylink_create_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_create_req);
+
+void hci_phylink_accept_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_accept_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_accept_req);
+
+void hci_phylink_disc_req(struct hci_dev *hdev, __u8 handle, __u8 reason)
+{
+	struct hci_cp_disc_phy_link cp;
+
+	BT_DBG("handle %d reason %d", handle, reason);
+
+	cp.handle = handle;
+	cp.reason = reason;
+	hci_send_cmd(hdev, HCI_OP_DISC_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_disc_req);
+
 /* Get HCI device by index.
  * Device is held on return. */
 struct hci_dev *hci_dev_get(int index)
-- 
1.7.9.1

[RFCv6 22/26] Bluetooth: Define AMP controller statuses

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 3577b85..1fefe2f 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -60,6 +60,15 @@
 
 #define HCI_BREDR_ID	0
 
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_NO_RESET,
-- 
1.7.9.1

[RFCv6 23/26] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 9f9a0d2..8a18e8a 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -131,6 +131,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -243,6 +254,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1093,4 +1107,10 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 		int timeout);
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
+		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index f27e3e8..8073d7a 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -57,6 +57,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1848,6 +1849,9 @@ int hci_register_dev(struct hci_dev *hdev)
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 
+	INIT_LIST_HEAD(&hdev->cb_list);
+	mutex_init(&hdev->cb_list_lock);
+
 	INIT_LIST_HEAD(&hdev->blacklist);
 
 	INIT_LIST_HEAD(&hdev->uuids);
@@ -1964,6 +1968,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
 	hci_adv_entries_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2263,6 +2268,83 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.1

[RFCv6 24/26] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

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

Use workqueue to process HCI callbacks.

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 8a18e8a..290a87d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1112,5 +1112,7 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
 		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 8073d7a..a220e2a 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2306,6 +2306,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("Queue cmd %p opt %p", cmd, cmd->opt);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	mutex_lock(&hdev->cb_list_lock);
-- 
1.7.9.1

[RFCv6 25/26] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..2751bf6
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 243f7d1..5a50a0b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -92,6 +93,10 @@ static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
 		cl[i].id = hdev->id;
 		cl[i].type = hdev->amp_type;
 		cl[i].status = hdev->amp_status;
+
+		read_unlock(&hci_dev_list_lock);
+		amp_read_loc_info(hdev, mgr);
+		read_lock(&hci_dev_list_lock);
 	}
 }
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..af009e0
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,49 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+}
+
+static void cb_destructor(struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	int err = 0;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		amp_read_loc_info_complete, mgr, cb_destructor, GFP_ATOMIC);
+
+	return err;
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 7325300..3053354 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -848,14 +849,15 @@ static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
-		struct sk_buff *skb)
+							struct sk_buff *skb)
 {
 	struct hci_rp_read_local_amp_info *rp = (void *) skb->data;
+	struct hci_cb_cmd *cmd;
 
 	BT_DBG("%s status 0x%x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto send;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -869,6 +871,13 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+send:
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.1

[RFCv6 26/26] Bluetooth: AMP: Read Local Assoc support

[Andrei Emeltchenko]

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

Adds reading AMP Assoc in HCI callback

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0558088..d3ac3fe 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -122,4 +122,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 
+
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 2751bf6..64d2b44 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1fefe2f..c7ad8ab 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -31,6 +31,7 @@
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
 #define HCI_MAX_AMP_KEY_SIZE	32
+#define HCI_MAX_AMP_ASSOC_SIZE	672
 
 /* HCI dev events */
 #define HCI_DEV_REG			1
@@ -853,6 +854,20 @@ struct hci_rp_read_local_amp_info {
 	__le32   be_flush_to;
 } __packed;
 
+#define HCI_OP_READ_LOCAL_AMP_ASSOC	0x140a
+struct hci_cp_read_local_amp_assoc {
+	__u8     handle;
+	__le16   len_so_far;
+	__le16   max_len;
+} __packed;
+
+struct hci_rp_read_local_amp_assoc {
+	__u8     status;
+	__u8     handle;
+	__le16   rem_len;
+	__u8     frag[0];
+} __packed;
+
 #define HCI_OP_LE_SET_EVENT_MASK	0x2001
 struct hci_cp_le_set_event_mask {
 	__u8     mask[8];
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 290a87d..a0173da 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -142,6 +142,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -195,6 +201,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 5a50a0b..2745c68 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -52,8 +52,7 @@ static inline int __a2mp_send(struct amp_mgr *mgr, u8 *data, int len)
 	return l2cap_chan_send(chan, &msg, len, 0);
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct a2mp_cmd *cmd;
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index af009e0..c36bdf3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -47,3 +47,62 @@ int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 
 	return err;
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		amp_read_loc_assoc_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 3053354..37e1d11 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -44,6 +44,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -880,6 +881,45 @@ send:
 	}
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	struct hci_cb_cmd *cmd;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto send;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+send:
+	/* Run callback when all fragments received */
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 							struct sk_buff *skb)
 {
@@ -2301,6 +2341,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.1

Re: [RFCv6 01/26] Bluetooth: Add Read Local AMP Info to init

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:24 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> AMP Info will be used in Discovery Response.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/hci_core.c |    3 +++
>  1 files changed, 3 insertions(+), 0 deletions(-)

Applied, thanks.

	Gustavo

Re: [RFCv6 02/26] Bluetooth: Adds set_default function in L2CAP setup

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:25 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Some parameters in L2CAP chan are set to default similar way in
> socket based channels and A2MP channels. Adds common function which
> sets all defaults.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/l2cap.h |    1 +
>  net/bluetooth/l2cap_core.c    |   11 +++++++++++
>  net/bluetooth/l2cap_sock.c    |    8 ++------
>  3 files changed, 14 insertions(+), 6 deletions(-)

Applied, thanks.

	Gustavo

Re: [RFCv6 05/26] Bluetooth: A2MP: Build and Send msg helpers

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:28 +0300]:

> 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         |   48 ++++++++++++++++++++++++++++++++++++++++++
>  2 files changed, 55 insertions(+), 0 deletions(-)
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index 0fe8ddd..995f1c0 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -24,6 +24,13 @@ struct amp_mgr {
>  	unsigned long		flags;
>  };
>  
> +struct a2mp_cmd {
> +	__u8	code;
> +	__u8	ident;
> +	__le16	len;
> +	__u8	data[0];
> +} __packed;
> +
>  void amp_mgr_get(struct amp_mgr *mgr);
>  int amp_mgr_put(struct amp_mgr *mgr);
>  
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 7e707ce..b572f9f 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -17,6 +17,54 @@
>  #include <net/bluetooth/l2cap.h>
>  #include <net/bluetooth/a2mp.h>
>  
> +/* 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 amp_mgr *mgr, u8 *data, int len)
> +{
> +	struct l2cap_chan *chan = mgr->a2mp_chan;
> +	struct kvec iv = { data, len };
> +	struct msghdr msg;
> +
> +	memset(&msg, 0, sizeof(msg));
> +
> +	msg.msg_iov = (struct iovec *) &iv;
> +	msg.msg_iovlen = 1;
> +
> +	return l2cap_chan_send(chan, &msg, len, 0);
> +}
> +
> +static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
> +		      void *data)
> +{
> +	struct a2mp_cmd *cmd;
> +
> +	cmd = __a2mp_build(code, ident, len, data);
> +	if (!cmd)
> +		return;
> +
> +	__a2mp_send(mgr, (u8 *)cmd, len + sizeof(*cmd));

Do you really need __a2mp_send(), are you going to use it somewhere else? If
not just inline its code here.

	Gustavo

Re: [RFCv6 06/26] Bluetooth: A2MP: Add chan callbacks

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:29 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Add state change, close and skb allocation callbacks.
> 
> 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 b572f9f..980382b 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -65,8 +65,43 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
>  	kfree(cmd);
>  }
>  
> +static void a2mp_chan_state_change_cb(void *data, int state)
> +{
> +	struct l2cap_chan *chan = data;
> +	struct amp_mgr *mgr;

data is already chan->data. So it is mgr here actually.

> +
> +	BT_DBG("chan %p state %s", chan, state_to_string(state));
> +
> +	chan->state = state;
> +
> +	switch (state) {
> +	case BT_CLOSED:
> +		mgr = chan->data;
> +		if (mgr)
> +			amp_mgr_put(mgr);
> +		break;
> +	}
> +}
> +
> +static void a2mp_chan_close_cb(void *data)
> +{
> +	struct amp_mgr *mgr = data;
> +
> +	l2cap_chan_destroy(mgr->a2mp_chan);
> +}
> +
> +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> +					      unsigned long len, int nb,
> +					      int *err)

See the patch I sent to the mailing list removing the err param.

> +{
> +	return bt_skb_alloc(len, GFP_KERNEL);
> +}
> +
>  static struct l2cap_ops a2mp_chan_ops = {
>  	.name = "L2CAP A2MP channel",
> +	.state_change = a2mp_chan_state_change_cb,
> +	.close = a2mp_chan_close_cb,
> +	.alloc_skb = a2mp_chan_alloc_skb_cb,
>  };
>  
>  static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
> @@ -104,6 +139,8 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
>  	chan->remote_mps = chan->omtu;
>  	chan->mps = chan->omtu;
>  
> +	a2mp_chan_state_change_cb(chan, BT_CONNECTED);

I would just replace this with

	chan->state = BT_CONNECTED;


	Gustavo

Re: [RFCv6 10/26] Bluetooth: A2MP: Process A2MP Command Reject

[Gustavo Padovan]

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:33 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Placeholder for future A2MP Command Reject handler.
> 
> 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 25d505e..745ab0c 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -65,6 +65,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)

Remove the inline, let the compiler choose it for you.

> +{
> +	struct a2mp_cmd_rej *rej = (struct a2mp_cmd_rej *) skb->data;
> +
> +	if (le16_to_cpu(hdr->len) < sizeof(*rej))

Isn't != better here, if I understood correctly this has a fixed size.

	Gustavo

Re: [RFCv6 12/26] Bluetooth: A2MP: Process A2MP Discover Request

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-03-28 16:31:35 +0300]:

> 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 22
>     A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
>       Controller list:
>         id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only)
>         id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only)
> ...
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>

I think patch 11 and 12 can be merged.

	Gustavo

Re: [RFCv6 05/26] Bluetooth: A2MP: Build and Send msg helpers

[Andrei Emeltchenko]

Hi Gustavo,

On Fri, Apr 06, 2012 at 07:44:39PM -0300, Gustavo Padovan wrote:
> > +static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
> > +		      void *data)
> > +{
> > +	struct a2mp_cmd *cmd;
> > +
> > +	cmd = __a2mp_build(code, ident, len, data);
> > +	if (!cmd)
> > +		return;
> > +
> > +	__a2mp_send(mgr, (u8 *)cmd, len + sizeof(*cmd));
> 
> Do you really need __a2mp_send(), are you going to use it somewhere else? If
> not just inline its code here.

Will do this way.

Best regards 
Andrei Emeltchenko 

Re: [RFCv6 06/26] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

Hi Gustavo,

On Fri, Apr 06, 2012 at 08:20:25PM -0300, Gustavo Padovan wrote:
> > +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> > +					      unsigned long len, int nb,
> > +					      int *err)
> 
> See the patch I sent to the mailing list removing the err param.

Since I can compile my code I assume that the patch is not yet applied.

The other comments will be fixed.

Best regards 
Andrei Emeltchenko 

Re: [RFCv6 10/26] Bluetooth: A2MP: Process A2MP Command Reject

[Andrei Emeltchenko]

Hi Gustavo,

On Fri, Apr 06, 2012 at 08:23:43PM -0300, Gustavo Padovan wrote:
> > --- a/net/bluetooth/a2mp.c
> > +++ b/net/bluetooth/a2mp.c
> > @@ -65,6 +65,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)
> 
> Remove the inline, let the compiler choose it for you.

OK

> > +	struct a2mp_cmd_rej *rej = (struct a2mp_cmd_rej *) skb->data;
> > +
> > +	if (le16_to_cpu(hdr->len) < sizeof(*rej))
> 
> Isn't != better here, if I understood correctly this has a fixed size.

Actually no, it has also Data (0 or more octets):
The length and content of the Data field depends on the Reason code. If
the reason code is 0x0000 "Command not recognized" no Data field is used.

Best regards 
Andrei Emeltchenko 

[RFCv7 00/23] RFC Bluetooth A2MP implementation

[Andrei Emeltchenko]

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

Basic A2MP implementation, callback interface to HCI and several usages of the interface.

Changes:
	* RFCv7: Taking comments from review in linux-bluetooth.
	* RFCv6: Remove some unneded check for sk since they are in different path
	(A2MP decoded as a data channel wrt L2CAP signalling channel). Style fixes.
	* RFCv5: Fix memory leaks, sparse warnings and taking comments from upstream.
	* RFCv4: redesign code to use l2cap channel lock instead of socket lock
	and general modifications. Basic implementation of HCI callback interface.
	* RFCv3: redesign code to use l2cap functions instead of kernel sockets
	L2CAP functions modified to work with channels without sk.
	* RFCv2: rebased against "workqueue" patches.
	* RFCv1: added refcnt to amp_mgr, fixed sleeping in atomic

Initially code was based of reference implementations below:

References: 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.

Andrei Emeltchenko (23):
  Bluetooth: A2MP: Create A2MP channel
  Bluetooth: A2MP: AMP Manager basic functions
  Bluetooth: A2MP: Build and Send msg helpers
  Bluetooth: A2MP: Add chan callbacks
  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: 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 channels
  Bluetooth: A2MP: Manage incoming connections
  Bluetooth: physical link HCI interface to AMP
  Bluetooth: Define AMP controller statuses
  Bluetooth: General HCI callback implementation
  Bluetooth: Process HCI callbacks in a workqueue
  Bluetooth: AMP: Use HCI callback for Read AMP Info
  Bluetooth: AMP: Read Local Assoc support

 include/net/bluetooth/a2mp.h     |  129 +++++++++
 include/net/bluetooth/amp.h      |   21 ++
 include/net/bluetooth/hci.h      |   51 ++++
 include/net/bluetooth/hci_core.h |   44 +++
 include/net/bluetooth/l2cap.h    |    6 +
 net/bluetooth/Makefile           |    3 +-
 net/bluetooth/a2mp.c             |  559 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/amp.c              |  108 ++++++++
 net/bluetooth/hci_conn.c         |   15 +
 net/bluetooth/hci_core.c         |  168 ++++++++++++
 net/bluetooth/hci_event.c        |   57 +++-
 net/bluetooth/l2cap_core.c       |   34 ++-
 12 files changed, 1184 insertions(+), 11 deletions(-)
 create mode 100644 include/net/bluetooth/a2mp.h
 create mode 100644 include/net/bluetooth/amp.h
 create mode 100644 net/bluetooth/a2mp.c
 create mode 100644 net/bluetooth/amp.c

-- 
1.7.9.5

[RFCv7 01/23] Bluetooth: A2MP: Create A2MP channel

[Andrei Emeltchenko]

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

Create and initialize fixed A2MP channel

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 0fac788..a8c4457 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -51,6 +51,8 @@
 #define L2CAP_CONN_TIMEOUT		msecs_to_jiffies(40000)
 #define L2CAP_INFO_TIMEOUT		msecs_to_jiffies(4000)
 
+#define L2CAP_A2MP_DEFAULT_MTU		670
+
 /* L2CAP socket address */
 struct sockaddr_l2 {
 	sa_family_t	l2_family;
@@ -230,6 +232,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
@@ -927,5 +930,7 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len,
 void l2cap_chan_busy(struct l2cap_chan *chan, int busy);
 int l2cap_chan_check_security(struct l2cap_chan *chan);
 void l2cap_chan_set_defaults(struct l2cap_chan *chan);
+int l2cap_ertm_init(struct l2cap_chan *chan);
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan);
 
 #endif /* __L2CAP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 2dc5a57..fa6d94a 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -9,4 +9,5 @@ obj-$(CONFIG_BT_CMTP)	+= cmtp/
 obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
-	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o
+	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
+	a2mp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 0000000..1f733b5
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,59 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+static struct l2cap_ops a2mp_chan_ops = {
+	.name = "L2CAP A2MP channel",
+};
+
+static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
+{
+	struct l2cap_chan *chan;
+
+	chan = l2cap_chan_create();
+	if (!chan)
+		return NULL;
+
+	BT_DBG("chan %p", chan);
+
+	hci_conn_hold(conn->hcon);
+
+	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
+
+	chan->ops = &a2mp_chan_ops;
+
+	l2cap_chan_set_defaults(chan);
+	chan->remote_max_tx = chan->max_tx;
+	chan->remote_tx_win = chan->tx_win;
+
+	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);
+
+	l2cap_chan_add(conn, chan);
+
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	return chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 97bb212..b6d0d0b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -472,7 +472,7 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 	list_add(&chan->list, &conn->chan_l);
 }
 
-static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
+void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 {
 	mutex_lock(&conn->chan_lock);
 	__l2cap_chan_add(conn, chan);
@@ -2273,7 +2273,7 @@ static void l2cap_ack_timeout(struct work_struct *work)
 	l2cap_chan_put(chan);
 }
 
-static inline int l2cap_ertm_init(struct l2cap_chan *chan)
+int l2cap_ertm_init(struct l2cap_chan *chan)
 {
 	int err;
 
-- 
1.7.9.5

[RFCv7 02/23] Bluetooth: A2MP: AMP Manager basic functions

[Andrei Emeltchenko]

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     |   30 +++++++++++++++++++++
 include/net/bluetooth/hci_core.h |    1 +
 net/bluetooth/a2mp.c             |   54 ++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_conn.c         |   15 +++++++++++
 4 files changed, 100 insertions(+)
 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..0fe8ddd
--- /dev/null
+++ b/include/net/bluetooth/a2mp.h
@@ -0,0 +1,30 @@
+/*
+   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __A2MP_H
+#define __A2MP_H
+
+struct amp_mgr {
+	struct list_head	list;
+	struct l2cap_conn	*l2cap_conn;
+	struct l2cap_chan	*a2mp_chan;
+	struct kref		kref;
+	__u8			ident;
+	unsigned long		flags;
+};
+
+void amp_mgr_get(struct amp_mgr *mgr);
+int amp_mgr_put(struct amp_mgr *mgr);
+
+#endif /* __A2MP_H */
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index ef6e654..16ab35d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -324,6 +324,7 @@ struct hci_conn {
 
 	struct sk_buff_head data_q;
 	struct list_head chan_list;
+	struct list_head mgr_list;
 
 	struct delayed_work disc_work;
 	struct timer_list idle_timer;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 1f733b5..85cd29a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -15,6 +15,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
+#include <net/bluetooth/a2mp.h>
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
@@ -57,3 +58,56 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	return chan;
 }
+
+/* AMP Manager functions */
+void amp_mgr_get(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", mgr);
+
+	kref_get(&mgr->kref);
+}
+
+static void amp_mgr_destroy(struct kref *kref)
+{
+	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
+
+	BT_DBG("mgr %p", mgr);
+
+	kfree(mgr);
+}
+
+int amp_mgr_put(struct amp_mgr *mgr)
+{
+	BT_DBG("mgr %p", mgr);
+
+	return kref_put(&mgr->kref, &amp_mgr_destroy);
+}
+
+static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
+{
+	struct amp_mgr *mgr;
+	struct l2cap_chan *chan;
+
+	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
+	if (!mgr)
+		return NULL;
+
+	BT_DBG("conn %p mgr %p", conn, mgr);
+
+	mgr->l2cap_conn = conn;
+
+	chan = a2mp_chan_open(conn);
+	if (!chan) {
+		kfree(mgr);
+		return NULL;
+	}
+
+	mgr->a2mp_chan = chan;
+	chan->data = mgr;
+
+	list_add(&mgr->list, &conn->hcon->mgr_list);
+
+	kref_init(&mgr->kref);
+
+	return mgr;
+}
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 947172b..2de2773 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 static void hci_le_connect(struct hci_conn *conn)
 {
@@ -404,6 +405,8 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 
 	INIT_LIST_HEAD(&conn->chan_list);
 
+	INIT_LIST_HEAD(&conn->mgr_list);
+
 	INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout);
 	setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn);
 	setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept,
@@ -424,6 +427,16 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
 	return conn;
 }
 
+static void hci_amp_mgr_list_flush(struct hci_conn *conn)
+{
+	struct amp_mgr *mgr, *n;
+
+	BT_DBG("conn %p", conn);
+
+	list_for_each_entry_safe(mgr, n, &conn->mgr_list, list)
+		amp_mgr_put(mgr);
+}
+
 int hci_conn_del(struct hci_conn *conn)
 {
 	struct hci_dev *hdev = conn->hdev;
@@ -459,6 +472,8 @@ int hci_conn_del(struct hci_conn *conn)
 
 	hci_chan_list_flush(conn);
 
+	hci_amp_mgr_list_flush(conn);
+
 	hci_conn_hash_del(hdev, conn);
 	if (hdev->notify)
 		hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
-- 
1.7.9.5

[RFCv7 03/23] Bluetooth: A2MP: Build and Send msg helpers

[Andrei Emeltchenko]

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(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 0fe8ddd..995f1c0 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -24,6 +24,13 @@ struct amp_mgr {
 	unsigned long		flags;
 };
 
+struct a2mp_cmd {
+	__u8	code;
+	__u8	ident;
+	__le16	len;
+	__u8	data[0];
+} __packed;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 85cd29a..cb3bf74 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -17,6 +17,52 @@
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
 
+/* 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 void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
+		      void *data)
+{
+	struct l2cap_chan *chan = mgr->a2mp_chan;
+	struct a2mp_cmd *cmd;
+	u16 total_len = len + sizeof(*cmd);
+	struct kvec iv;
+	struct msghdr msg;
+
+	cmd = __a2mp_build(code, ident, len, data);
+	if (!cmd)
+		return;
+
+	iv.iov_base = cmd;
+	iv.iov_len = total_len;
+
+	memset(&msg, 0, sizeof(msg));
+
+	msg.msg_iov = (struct iovec *) &iv;
+	msg.msg_iovlen = 1;
+
+	l2cap_chan_send(chan, &msg, total_len, 0);
+
+	kfree(cmd);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 };
-- 
1.7.9.5

[RFCv7 04/23] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

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

Add state change, close and skb allocation callbacks.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index cb3bf74..67a7ad4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,8 +63,46 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static void a2mp_chan_state_change_cb(void *data, int state)
+{
+	struct amp_mgr *mgr = data;
+	struct l2cap_chan *chan;
+
+	if (!mgr)
+		return;
+
+	chan = mgr->a2mp_chan;
+
+	BT_DBG("chan %p state %s", chan, state_to_string(state));
+
+	chan->state = state;
+
+	switch (state) {
+	case BT_CLOSED:
+		if (mgr)
+			amp_mgr_put(mgr);
+		break;
+	}
+}
+
+static void a2mp_chan_close_cb(void *data)
+{
+	struct amp_mgr *mgr = data;
+
+	l2cap_chan_destroy(mgr->a2mp_chan);
+}
+
+static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
+					      unsigned long len, int nb)
+{
+	return bt_skb_alloc(len, GFP_KERNEL);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.state_change = a2mp_chan_state_change_cb,
+	.close = a2mp_chan_close_cb,
+	.alloc_skb = a2mp_chan_alloc_skb_cb,
 };
 
 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
@@ -102,6 +140,8 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 	chan->remote_mps = chan->omtu;
 	chan->mps = chan->omtu;
 
+	chan->state = BT_CONNECTED;
+
 	return chan;
 }
 
-- 
1.7.9.5

[RFCv7 05/23] Bluetooth: A2MP: Definitions for A2MP commands

[Andrei Emeltchenko]

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

Define A2MP command IDs and packet structures.

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 995f1c0..39160f5 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -31,6 +31,79 @@ struct a2mp_cmd {
 	__u8	data[0];
 } __packed;
 
+/* A2MP command codes */
+#define A2MP_COMMAND_REJ         0x01
+struct a2mp_cmd_rej {
+	__le16	reason;
+	__u8	data[0];
+} __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;
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.5

[RFCv7 06/23] Bluetooth: A2MP: Define A2MP status codes

[Andrei Emeltchenko]

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 file changed, 10 insertions(+)

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 39160f5..2eb46ee 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -104,6 +104,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
+
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
 
-- 
1.7.9.5

[RFCv7 07/23] Bluetooth: A2MP: Process A2MP messages

[Andrei Emeltchenko]

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 |   62 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 62 insertions(+)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 67a7ad4..e3b91cd 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,67 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Handle A2MP signalling */
+static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (struct a2mp_cmd *) skb->data;
+	struct amp_mgr *mgr = data;
+	int err = 0;
+
+	amp_mgr_get(mgr);
+
+	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;
+		}
+
+		mgr->ident = hdr->ident;
+
+		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 sig cmd 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);
+	}
+
+	/* Always free skb and return success error code to prevent
+	   from sending L2CAP Disconnect over A2MP channel */
+	kfree_skb(skb);
+
+	amp_mgr_put(mgr);
+
+	return 0;
+}
+
 static void a2mp_chan_state_change_cb(void *data, int state)
 {
 	struct amp_mgr *mgr = data;
@@ -100,6 +161,7 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.recv = a2mp_chan_recv_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.close = a2mp_chan_close_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
-- 
1.7.9.5

[RFCv7 08/23] Bluetooth: A2MP: Process A2MP Command Reject

[Andrei Emeltchenko]

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

Placeholder for future A2MP Command Reject handler.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e3b91cd..4f3e6f9 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,22 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+/* Processing A2MP messages */
+static 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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -89,6 +105,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 09/23] Bluetooth: A2MP: Process A2MP Discover Request

[Andrei Emeltchenko]

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

Adds helper functions to count HCI devs and 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 22
    A2MP: Discover rsp: mtu/mps 670 mask: 0x0000
      Controller list:
        id 0 type 0 (BR-EDR) status 0x01 (Bluetooth only)
        id 1 type 1 (802.11 AMP) status 0x01 (Bluetooth only)
...

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 2eb46ee..c9aa9c5 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/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 16ab35d..67e8eaa 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -644,6 +644,19 @@ static inline void hci_set_drvdata(struct hci_dev *hdev, void *data)
 	dev_set_drvdata(&hdev->dev, data);
 }
 
+/* hci_dev_list shall be locked */
+static inline uint8_t __hci_num_ctrl(void)
+{
+	uint8_t count = 0;
+	struct list_head *p;
+
+	list_for_each(p, &hci_dev_list) {
+		count++;
+	}
+
+	return count;
+}
+
 struct hci_dev *hci_dev_get(int index);
 struct hci_dev *hci_get_route(bdaddr_t *src, bdaddr_t *dst);
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4f3e6f9..b4eb294 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,36 @@ 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;
+}
+
+/* hci_dev_list shall be locked */
+static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
+{
+	int i = 0;
+	struct hci_dev *hdev;
+
+	__a2mp_cl_bredr(cl);
+
+	list_for_each_entry(hdev, &hci_dev_list, list) {
+		/* Iterate through AMP controllers */
+		if (hdev->id == HCI_BREDR_ID)
+			continue;
+
+		/* Starting from second entry */
+		if (++i >= num_ctrl)
+			return;
+
+		cl[i].id = hdev->id;
+		cl[i].type = hdev->amp_type;
+		cl[i].status = hdev->amp_status;
+	}
+}
+
 /* Processing A2MP messages */
 static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
 			    struct a2mp_cmd *hdr)
@@ -79,6 +109,58 @@ static 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;
+	u16 len = le16_to_cpu(hdr->len);
+	struct a2mp_discov_rsp *rsp;
+	u16 ext_feat;
+	u8 num_ctrl;
+
+	if (len < sizeof(*req))
+		return -EINVAL;
+
+	skb_pull(skb, sizeof(*req));
+
+	ext_feat = le16_to_cpu(req->ext_feat);
+
+	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
+
+	/* check that packet is not broken for now */
+	while (ext_feat & A2MP_FEAT_EXT) {
+		if (len < sizeof(ext_feat))
+			return -EINVAL;
+
+		ext_feat = get_unaligned_le16(skb->data);
+		BT_DBG("efm 0x%4.4x", ext_feat);
+		len -= sizeof(ext_feat);
+		skb_pull(skb, sizeof(ext_feat));
+	}
+
+	read_lock(&hci_dev_list_lock);
+
+	num_ctrl = __hci_num_ctrl();
+	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
+	rsp = kmalloc(len, GFP_ATOMIC);
+	if (!rsp) {
+		read_unlock(&hci_dev_list_lock);
+		return -ENOMEM;
+	}
+
+	rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
+	rsp->ext_feat = 0;
+
+	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
+
+	read_unlock(&hci_dev_list_lock);
+
+	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
+
+	kfree(rsp);
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -109,6 +191,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 10/23] Bluetooth: A2MP: Process A2MP Change Notify

[Andrei Emeltchenko]

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

Placeholder for A2MP Change Notify handler.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b4eb294..ee29095 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -161,6 +161,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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -195,6 +211,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 11/23] Bluetooth: A2MP: Process A2MP Get Info Request

[Andrei Emeltchenko]

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

Process A2MP Get Info Request.
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
...

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 |   37 +++++++++++++++++++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ee29095..c876b44 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -177,6 +177,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 a2mp_info_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("id %d", req->id);
+
+	rsp.id = req->id;
+	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+
+	hdev = hci_dev_get(req->id);
+	if (hdev && hdev->amp_type != HCI_BREDR) {
+		rsp.status = 0;
+		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
+		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
+		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
+		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
+		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
+	}
+
+	if (hdev)
+		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 */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -215,6 +249,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 12/23] Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

[Andrei Emeltchenko]

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 |   36 ++++++++++++++++++++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index c876b44..a3b6b70 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -211,6 +211,39 @@ 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);
+		goto clean;
+	}
+
+	amp_read_loc_assoc(hdev, mgr);
+
+clean:
+	if (hdev)
+		hci_dev_put(hdev);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -253,6 +286,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 13/23] Bluetooth: A2MP: Process A2MP Create Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Create Physlink Request.
Handles requests with invalid controler id as shown below:

...
> ACL data: handle 11 flags 0x02 dlen 50
    A2MP: Create Physical Link req: local id 1 remote id 85
      Assoc data:
        <skipped>
< ACL data: handle 11 flags 0x00 dlen 15
    A2MP: Create Physical Link rsp: local id 85 remote id 1 status 1
      Invalid Controller ID
...

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a3b6b70..6cb7161 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -244,6 +244,42 @@ clean:
 	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;
+	struct a2mp_physlink_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		return -EINVAL;
+
+	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+
+	hdev = hci_dev_get(req->remote_id);
+	if (!hdev || hdev->amp_type != HCI_AMP) {
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send_rsp;
+	}
+
+	/* TODO process physlink create */
+
+	rsp.status = A2MP_STATUS_SUCCESS;
+
+send_rsp:
+	if (hdev)
+		hci_dev_put(hdev);
+
+	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
+		  &rsp);
+
+	skb_pull(skb, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -290,6 +326,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 14/23] Bluetooth: A2MP: Process A2MP Disc Physlink Request

[Andrei Emeltchenko]

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

Placeholder for A2MP Disconnect Physlink Request.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6cb7161..af22981 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -280,6 +280,39 @@ send_rsp:
 	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;
+
+	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
+
+	rsp.local_id = req->remote_id;
+	rsp.remote_id = req->local_id;
+	rsp.status = A2MP_STATUS_SUCCESS;
+
+	hdev = hci_dev_get(req->local_id);
+	if (!hdev) {
+		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send_rsp;
+	}
+
+	/* TODO Disconnect Phys Link here */
+
+	hci_dev_put(hdev);
+
+send_rsp:
+	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -330,6 +363,9 @@ static int a2mp_chan_recv_cb(void *data, 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.9.5

[RFCv7 15/23] Bluetooth: A2MP: Process A2MP Command Responses

[Andrei Emeltchenko]

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

Process A2MP responses, print cmd code and ident for now.

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

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index af22981..4675785 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -313,6 +313,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, le16_to_cpu(hdr->len));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(void *data, struct sk_buff *skb)
 {
@@ -372,6 +381,9 @@ static int a2mp_chan_recv_cb(void *data, 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 sig cmd 0x%2.2x", hdr->code);
 			err = -EINVAL;
-- 
1.7.9.5

[RFCv7 16/23] Bluetooth: A2MP: Handling fixed channels

[Andrei Emeltchenko]

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

A2MP fixed channel do not have sk

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

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index a8c4457..e36f7c5 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -576,6 +576,7 @@ struct l2cap_conn {
 #define L2CAP_CHAN_RAW			1
 #define L2CAP_CHAN_CONN_LESS		2
 #define L2CAP_CHAN_CONN_ORIENTED	3
+#define L2CAP_CHAN_CONN_FIX_A2MP	4
 
 /* ----- L2CAP socket info ----- */
 #define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 4675785..aa93e0b 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -462,8 +462,7 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	hci_conn_hold(conn->hcon);
 
-	chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
-	chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 
 	chan->ops = &a2mp_chan_ops;
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b6d0d0b..d0aeb56 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -453,6 +453,13 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan)
 		chan->omtu = L2CAP_DEFAULT_MTU;
 		break;
 
+	case L2CAP_CHAN_CONN_FIX_A2MP:
+		chan->scid = L2CAP_CID_A2MP;
+		chan->dcid = L2CAP_CID_A2MP;
+		chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
+		chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
+		break;
+
 	default:
 		/* Raw socket can send/recv signalling messages only */
 		chan->scid = L2CAP_CID_SIGNALING;
@@ -483,7 +490,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 {
 	struct sock *sk = chan->sk;
 	struct l2cap_conn *conn = chan->conn;
-	struct sock *parent = bt_sk(sk)->parent;
+	struct sock *parent;
 
 	__clear_chan_timer(chan);
 
@@ -499,6 +506,9 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		hci_conn_put(conn->hcon);
 	}
 
+	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP)
+		goto clean;
+
 	lock_sock(sk);
 
 	__l2cap_state_change(chan, BT_CLOSED);
@@ -507,6 +517,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 	if (err)
 		__l2cap_chan_set_err(chan, err);
 
+	parent = bt_sk(sk)->parent;
 	if (parent) {
 		bt_accept_unlink(sk);
 		parent->sk_data_ready(parent, 0);
@@ -519,6 +530,7 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err)
 			test_bit(CONF_INPUT_DONE, &chan->conf_state)))
 		return;
 
+clean:
 	skb_queue_purge(&chan->tx_q);
 
 	if (chan->mode == L2CAP_MODE_ERTM) {
@@ -1543,7 +1555,8 @@ static void l2cap_monitor_timeout(struct work_struct *work)
 	l2cap_chan_lock(chan);
 
 	if (chan->retry_count >= chan->remote_max_tx) {
-		l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
+		if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
+			l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED);
 		l2cap_chan_unlock(chan);
 		l2cap_chan_put(chan);
 		return;
-- 
1.7.9.5

[RFCv7 17/23] Bluetooth: A2MP: Manage incoming connections

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index c9aa9c5..6304e14 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -15,6 +15,8 @@
 #ifndef __A2MP_H
 #define __A2MP_H
 
+#include <net/bluetooth/l2cap.h>
+
 #define A2MP_FEAT_EXT	0x8000
 
 struct amp_mgr {
@@ -118,5 +120,7 @@ struct a2mp_physlink_rsp {
 
 void amp_mgr_get(struct amp_mgr *mgr);
 int amp_mgr_put(struct amp_mgr *mgr);
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb);
 
 #endif /* __A2MP_H */
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index aa93e0b..a49acb6 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -541,3 +541,15 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
 
 	return mgr;
 }
+
+struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
+				       struct sk_buff *skb)
+{
+	struct amp_mgr *mgr;
+
+	mgr = amp_mgr_create(conn);
+
+	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
+
+	return mgr->a2mp_chan;
+}
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index d0aeb56..ed434b0 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -56,6 +56,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/smp.h>
+#include <net/bluetooth/a2mp.h>
 
 bool disable_ertm;
 
@@ -4554,10 +4555,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);
-		/* Drop packet and return */
-		kfree_skb(skb);
-		return 0;
+		if (cid == L2CAP_CID_A2MP) {
+			chan = a2mp_channel_create(conn, skb);
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			/* Drop packet and return */
+			kfree_skb(skb);
+			return 0;
+		}
 	}
 
 	l2cap_chan_lock(chan);
-- 
1.7.9.5

[RFCv7 18/23] Bluetooth: physical link HCI interface to AMP

[Andrei Emeltchenko]

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

Adds support for physical link create/acceppt/disconnect AMP HCI
commands. To be used by the upper layer.
Backport from CodeAurora & Atheros

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 346f087..49bf231 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -30,6 +30,8 @@
 #define HCI_MAX_EVENT_SIZE	260
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
+#define HCI_MAX_AMP_KEY_SIZE	32
+
 /* HCI dev events */
 #define HCI_DEV_REG			1
 #define HCI_DEV_UNREG			2
@@ -523,6 +525,28 @@ struct hci_cp_io_capability_neg_reply {
 	__u8     reason;
 } __packed;
 
+#define HCI_OP_CREATE_PHY_LINK		0x0435
+struct hci_cp_create_phy_link {
+	__u8     handle;
+	__u8     key_len;
+	__u8     key_type;
+	__u8     key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_ACCEPT_PHY_LINK		0x0436
+struct hci_cp_accept_phy_link {
+	__u8	handle;
+	__u8	key_len;
+	__u8	key_type;
+	__u8	key[HCI_MAX_AMP_KEY_SIZE];
+} __packed;
+
+#define HCI_OP_DISC_PHY_LINK		0x0437
+struct hci_cp_disc_phy_link {
+	__u8     handle;
+	__u8     reason;
+} __packed;
+
 #define HCI_OP_SNIFF_MODE		0x0803
 struct hci_cp_sniff_mode {
 	__le16   handle;
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index c2e710b..1806762 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -340,6 +340,51 @@ static void hci_linkpol_req(struct hci_dev *hdev, unsigned long opt)
 	hci_send_cmd(hdev, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy);
 }
 
+/* AMP HCI interface */
+void hci_phylink_create_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_create_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_create_req);
+
+void hci_phylink_accept_req(struct hci_dev *hdev, __u8 handle, __u8 key_len,
+						__u8 key_type, __u8 *key)
+{
+	struct hci_cp_accept_phy_link cp;
+
+	cp.handle	= handle;
+	cp.key_type	= key_type;
+	cp.key_len	= min_t(__u8, key_len, HCI_MAX_AMP_KEY_SIZE);
+
+	BT_DBG("key len %d, phy handle %d", cp.key_len, cp.handle);
+
+	memcpy(cp.key, key, cp.key_len);
+	hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_accept_req);
+
+void hci_phylink_disc_req(struct hci_dev *hdev, __u8 handle, __u8 reason)
+{
+	struct hci_cp_disc_phy_link cp;
+
+	BT_DBG("handle %d reason %d", handle, reason);
+
+	cp.handle = handle;
+	cp.reason = reason;
+	hci_send_cmd(hdev, HCI_OP_DISC_PHY_LINK, sizeof(cp), &cp);
+}
+EXPORT_SYMBOL(hci_phylink_disc_req);
+
 /* Get HCI device by index.
  * Device is held on return. */
 struct hci_dev *hci_dev_get(int index)
-- 
1.7.9.5

[RFCv7 19/23] Bluetooth: Define AMP controller statuses

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 49bf231..1320c06 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -58,6 +58,18 @@
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
 
+/* First BR/EDR Controller shall have ID = 0 */
+#define HCI_BREDR_ID	0
+
+/* AMP controller status */
+#define AMP_CTRL_POWERED_DOWN			0x00
+#define AMP_CTRL_BLUETOOTH_ONLY			0x01
+#define AMP_CTRL_NO_CAPACITY			0x02
+#define AMP_CTRL_LOW_CAPACITY			0x03
+#define AMP_CTRL_MEDIUM_CAPACITY		0x04
+#define AMP_CTRL_HIGH_CAPACITY			0x05
+#define AMP_CTRL_FULL_CAPACITY			0x06
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_NO_RESET,
-- 
1.7.9.5

[RFCv7 20/23] Bluetooth: General HCI callback implementation

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 67e8eaa..276092e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -131,6 +131,17 @@ struct le_scan_params {
 
 #define HCI_MAX_SHORT_NAME_LENGTH	10
 
+struct hci_dev;
+
+struct hci_cb_cmd {
+	struct list_head list;
+	u16 opcode;
+	u8 status;
+	void *opt;
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+	void (*destructor)(struct hci_cb_cmd *cmd);
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -243,6 +254,9 @@ struct hci_dev {
 
 	struct list_head	mgmt_pending;
 
+	struct mutex		cb_list_lock;
+	struct list_head	cb_list;
+
 	struct discovery_state	discovery;
 	struct hci_conn_hash	conn_hash;
 	struct list_head	blacklist;
@@ -1091,4 +1105,10 @@ int hci_le_scan(struct hci_dev *hdev, u8 type, u16 interval, u16 window,
 		int timeout);
 int hci_cancel_le_scan(struct hci_dev *hdev);
 
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode);
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
+		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 1806762..9deee5f 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -57,6 +57,7 @@
 static void hci_rx_work(struct work_struct *work);
 static void hci_cmd_work(struct work_struct *work);
 static void hci_tx_work(struct work_struct *work);
+static void hci_cb_clear(struct hci_dev *hdev);
 
 /* HCI device list */
 LIST_HEAD(hci_dev_list);
@@ -1851,6 +1852,9 @@ int hci_register_dev(struct hci_dev *hdev)
 
 	INIT_LIST_HEAD(&hdev->mgmt_pending);
 
+	INIT_LIST_HEAD(&hdev->cb_list);
+	mutex_init(&hdev->cb_list_lock);
+
 	INIT_LIST_HEAD(&hdev->blacklist);
 
 	INIT_LIST_HEAD(&hdev->uuids);
@@ -1967,6 +1971,7 @@ void hci_unregister_dev(struct hci_dev *hdev)
 	hci_smp_ltks_clear(hdev);
 	hci_remote_oob_data_clear(hdev);
 	hci_adv_entries_clear(hdev);
+	hci_cb_clear(hdev);
 	hci_dev_unlock(hdev);
 
 	hci_dev_put(hdev);
@@ -2266,6 +2271,83 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 	return 0;
 }
 
+static int hci_add_cb(struct hci_dev *hdev, __u16 opcode,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	struct hci_cb_cmd *cmd;
+
+	cmd = kmalloc(sizeof(*cmd), flags);
+	if (!cmd)
+		return -ENOMEM;
+
+	cmd->cb = cb;
+	cmd->opcode = opcode;
+	cmd->opt = opt;
+	cmd->status = 0;
+	cmd->destructor = destructor;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_add(&cmd->list, &hdev->cb_list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return 0;
+}
+
+struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
+{
+	struct hci_cb_cmd *cmd;
+
+	mutex_lock(&hdev->cb_list_lock);
+	list_for_each_entry(cmd, &hdev->cb_list, list)
+		if (cmd->opcode == opcode) {
+			mutex_unlock(&hdev->cb_list_lock);
+			return cmd;
+		}
+	mutex_unlock(&hdev->cb_list_lock);
+
+	return NULL;
+}
+
+void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
+{
+	mutex_lock(&hdev->cb_list_lock);
+	list_del(&cmd->list);
+	mutex_unlock(&hdev->cb_list_lock);
+
+	if (cmd->destructor) {
+		cmd->destructor(cmd);
+	} else {
+		kfree(cmd->opt);
+		kfree(cmd);
+	}
+}
+
+static void hci_cb_clear(struct hci_dev *hdev)
+{
+	struct hci_cb_cmd *cmd, *tmp;
+
+	list_for_each_entry_safe(cmd, tmp, &hdev->cb_list, list)
+		hci_remove_cb(hdev, cmd);
+}
+
+/* Send HCI command with callback */
+int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
+		void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd),
+	void *opt, void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags)
+{
+	int ret;
+
+	if (!cb)
+		return -EINVAL;
+
+	ret = hci_add_cb(hdev, opcode, cb, opt, destructor, flags);
+	if (ret)
+		return ret;
+
+	return hci_send_cmd(hdev, opcode, plen, param);
+}
+
 /* Get data from the previously sent command */
 void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode)
 {
-- 
1.7.9.5

[RFCv7 21/23] Bluetooth: Process HCI callbacks in a workqueue

[Andrei Emeltchenko]

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

Use workqueue to process HCI callbacks.

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

diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 276092e..2fa5c3d 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -1110,5 +1110,7 @@ int hci_cmd_cb(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param,
 	void (*cb)(struct hci_dev *hdev, struct hci_cb_cmd *cmd), void *opt,
 		void (*destructor)(struct hci_cb_cmd *cmd), gfp_t flags);
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd);
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue);
 
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 9deee5f..82431f5 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -2309,6 +2309,47 @@ struct hci_cb_cmd *hci_find_cb(struct hci_dev *hdev, __u16 opcode)
 	return NULL;
 }
 
+struct hci_cb_work {
+	struct work_struct work;
+	struct hci_dev *hdev;
+	struct hci_cb_cmd *cmd;
+};
+
+static void hci_cb_worker(struct work_struct *w)
+{
+	struct hci_cb_work *work = (struct hci_cb_work *) w;
+	struct hci_cb_cmd *cmd = work->cmd;
+	struct hci_dev *hdev = work->hdev;
+
+	cmd->cb(hdev, cmd);
+
+	hci_remove_cb(hdev, cmd);
+	kfree(w);
+	hci_dev_put(hdev);
+}
+
+void hci_queue_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd,
+					struct workqueue_struct *workqueue)
+{
+	struct hci_cb_work *work;
+
+	BT_DBG("Queue cmd %p opt %p", cmd, cmd->opt);
+
+	work = kmalloc(sizeof(*work), GFP_KERNEL);
+	if (!work)
+		return;
+
+	INIT_WORK(&work->work, hci_cb_worker);
+	work->hdev = hdev;
+	work->cmd = cmd;
+	hci_dev_hold(hdev);
+
+	if (!queue_work(workqueue, &work->work)) {
+		kfree(work);
+		hci_dev_put(hdev);
+	}
+}
+
 void hci_remove_cb(struct hci_dev *hdev, struct hci_cb_cmd *cmd)
 {
 	mutex_lock(&hdev->cb_list_lock);
-- 
1.7.9.5

[RFCv7 22/23] Bluetooth: AMP: Use HCI callback for Read AMP Info

[Andrei Emeltchenko]

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

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

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

diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
new file mode 100644
index 0000000..2751bf6
--- /dev/null
+++ b/include/net/bluetooth/amp.h
@@ -0,0 +1,19 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __AMP_H
+#define __AMP_H
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+
+#endif /* __AMP_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index fa6d94a..dea6a28 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,4 +10,4 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o
+	a2mp.o amp.o
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index a49acb6..39a3009 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -16,6 +16,7 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/l2cap.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* A2MP build & send command helper functions */
 static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
@@ -90,6 +91,10 @@ static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
 		cl[i].id = hdev->id;
 		cl[i].type = hdev->amp_type;
 		cl[i].status = hdev->amp_status;
+
+		read_unlock(&hci_dev_list_lock);
+		amp_read_loc_info(hdev, mgr);
+		read_lock(&hci_dev_list_lock);
 	}
 }
 
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
new file mode 100644
index 0000000..af009e0
--- /dev/null
+++ b/net/bluetooth/amp.c
@@ -0,0 +1,49 @@
+/*
+   Copyright (c) 2011,2012 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/workqueue.h>
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
+
+static void amp_read_loc_info_complete(struct hci_dev *hdev,
+				       struct hci_cb_cmd *cmd)
+{
+	BT_DBG("%s cmd %p mgr %p", hdev->name, cmd, cmd->opt);
+}
+
+static void cb_destructor(struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+
+	BT_DBG("Destructor cmd %p mgr %p", cmd, mgr);
+
+	amp_mgr_put(mgr);
+	kfree(cmd);
+}
+
+int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	int err = 0;
+
+	BT_DBG("%s mgr %p", hdev->name, mgr);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL,
+		amp_read_loc_info_complete, mgr, cb_destructor, GFP_ATOMIC);
+
+	return err;
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 8a13f90..0282d78 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -43,6 +43,7 @@
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/a2mp.h>
 
 /* Handle HCI Event packets */
 
@@ -848,14 +849,15 @@ static void hci_cc_write_ca_timeout(struct hci_dev *hdev, struct sk_buff *skb)
 }
 
 static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
-		struct sk_buff *skb)
+							struct sk_buff *skb)
 {
 	struct hci_rp_read_local_amp_info *rp = (void *) skb->data;
+	struct hci_cb_cmd *cmd;
 
 	BT_DBG("%s status 0x%x", hdev->name, rp->status);
 
 	if (rp->status)
-		return;
+		goto send;
 
 	hdev->amp_status = rp->amp_status;
 	hdev->amp_total_bw = __le32_to_cpu(rp->total_bw);
@@ -869,6 +871,13 @@ static void hci_cc_read_local_amp_info(struct hci_dev *hdev,
 	hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to);
 
 	hci_req_complete(hdev, HCI_OP_READ_LOCAL_AMP_INFO, rp->status);
+
+send:
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_INFO);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
 }
 
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
-- 
1.7.9.5

[RFCv7 23/23] Bluetooth: AMP: Read Local Assoc support

[Andrei Emeltchenko]

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

Adds reading AMP Assoc in HCI callback

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

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 6304e14..387987b 100644
--- a/include/net/bluetooth/a2mp.h
+++ b/include/net/bluetooth/a2mp.h
@@ -123,4 +123,7 @@ int amp_mgr_put(struct amp_mgr *mgr);
 struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
 				       struct sk_buff *skb);
 
+
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data);
+
 #endif /* __A2MP_H */
diff --git a/include/net/bluetooth/amp.h b/include/net/bluetooth/amp.h
index 2751bf6..64d2b44 100644
--- a/include/net/bluetooth/amp.h
+++ b/include/net/bluetooth/amp.h
@@ -15,5 +15,7 @@
 #define __AMP_H
 
 int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr);
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle);
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr);
 
 #endif /* __AMP_H */
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 1320c06..d6b3a4a 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -31,6 +31,7 @@
 #define HCI_MAX_FRAME_SIZE	(HCI_MAX_ACL_SIZE + 4)
 
 #define HCI_MAX_AMP_KEY_SIZE	32
+#define HCI_MAX_AMP_ASSOC_SIZE	672
 
 /* HCI dev events */
 #define HCI_DEV_REG			1
@@ -854,6 +855,20 @@ struct hci_rp_read_local_amp_info {
 	__le32   be_flush_to;
 } __packed;
 
+#define HCI_OP_READ_LOCAL_AMP_ASSOC	0x140a
+struct hci_cp_read_local_amp_assoc {
+	__u8     handle;
+	__le16   len_so_far;
+	__le16   max_len;
+} __packed;
+
+struct hci_rp_read_local_amp_assoc {
+	__u8     status;
+	__u8     handle;
+	__le16   rem_len;
+	__u8     frag[0];
+} __packed;
+
 #define HCI_OP_LE_SET_EVENT_MASK	0x2001
 struct hci_cp_le_set_event_mask {
 	__u8     mask[8];
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 2fa5c3d..0e4cb1e 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -142,6 +142,12 @@ struct hci_cb_cmd {
 	void (*destructor)(struct hci_cb_cmd *cmd);
 };
 
+struct amp_assoc {
+	__u16	len;
+	__u16	offset;
+	__u8	data[HCI_MAX_AMP_ASSOC_SIZE];
+};
+
 #define NUM_REASSEMBLY 4
 struct hci_dev {
 	struct list_head list;
@@ -195,6 +201,8 @@ struct hci_dev {
 	__u32		amp_max_flush_to;
 	__u32		amp_be_flush_to;
 
+	struct amp_assoc	loc_assoc;
+
 	__u8		flow_ctl_mode;
 
 	unsigned int	auto_accept_delay;
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 39a3009..f9cfd55 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -38,8 +38,7 @@ static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
 	return cmd;
 }
 
-static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
-		      void *data)
+void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
 {
 	struct l2cap_chan *chan = mgr->a2mp_chan;
 	struct a2mp_cmd *cmd;
diff --git a/net/bluetooth/amp.c b/net/bluetooth/amp.c
index af009e0..c36bdf3 100644
--- a/net/bluetooth/amp.c
+++ b/net/bluetooth/amp.c
@@ -47,3 +47,62 @@ int amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr)
 
 	return err;
 }
+
+void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+
+	BT_DBG("%s: handle %d", hdev->name, phy_handle);
+
+	cp.handle = phy_handle;
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+	cp.len_so_far = cpu_to_le16(loc_assoc->offset);
+
+	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp);
+}
+
+static void amp_read_loc_assoc_complete(struct hci_dev *hdev,
+					struct hci_cb_cmd *cmd)
+{
+	struct amp_mgr *mgr = cmd->opt;
+	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
+	struct a2mp_amp_assoc_rsp *rsp;
+	size_t len;
+
+	BT_DBG("%s: cmd %p", hdev->name, cmd);
+
+	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
+	rsp = kzalloc(len, GFP_KERNEL);
+	if (!rsp)
+		return;
+
+	rsp->id = hdev->id;
+
+	if (cmd->status) {
+		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
+		goto send;
+	}
+
+	rsp->status = A2MP_STATUS_SUCCESS;
+	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
+
+send:
+	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
+	kfree(rsp);
+}
+
+void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr)
+{
+	struct hci_cp_read_local_amp_assoc cp;
+
+	memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc));
+	memset(&cp, 0, sizeof(cp));
+
+	cp.max_len = cpu_to_le16(hdev->amp_assoc_size);
+
+	amp_mgr_get(mgr);
+
+	hci_cmd_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp,
+		amp_read_loc_assoc_complete, mgr, cb_destructor, GFP_KERNEL);
+}
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 0282d78..7ee0eb7 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -44,6 +44,7 @@
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/a2mp.h>
+#include <net/bluetooth/amp.h>
 
 /* Handle HCI Event packets */
 
@@ -880,6 +881,45 @@ send:
 	}
 }
 
+static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev,
+					struct sk_buff *skb)
+{
+	struct hci_rp_read_local_amp_assoc *rp = (void *)skb->data;
+	struct amp_assoc *assoc = &hdev->loc_assoc;
+	struct hci_cb_cmd *cmd;
+	size_t rem_len, frag_len;
+
+	BT_DBG("%s status 0x%x", hdev->name, rp->status);
+
+	if (rp->status)
+		goto send;
+
+	frag_len = skb->len - sizeof(*rp);
+	rem_len = __le16_to_cpu(rp->rem_len);
+
+	if (rem_len > frag_len) {
+		memcpy(assoc->data + assoc->offset, rp->frag, frag_len);
+		assoc->offset += frag_len;
+
+		/* Read other fragments */
+		amp_read_loc_assoc_frag(hdev, rp->handle);
+
+		return;
+	}
+
+	memcpy(assoc->data + assoc->offset, rp->frag, rem_len);
+	assoc->len = assoc->offset + rem_len;
+	assoc->offset = 0;
+
+send:
+	/* Run callback when all fragments received */
+	cmd = hci_find_cb(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC);
+	if (cmd) {
+		cmd->status = rp->status;
+		hci_queue_cb(hdev, cmd, hdev->workqueue);
+	}
+}
+
 static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
 							struct sk_buff *skb)
 {
@@ -2305,6 +2345,10 @@ static inline void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *sk
 		hci_cc_read_local_amp_info(hdev, skb);
 		break;
 
+	case HCI_OP_READ_LOCAL_AMP_ASSOC:
+		hci_cc_read_local_amp_assoc(hdev, skb);
+		break;
+
 	case HCI_OP_DELETE_STORED_LINK_KEY:
 		hci_cc_delete_stored_link_key(hdev, skb);
 		break;
-- 
1.7.9.5

Re: [RFCv7 04/23] Bluetooth: A2MP: Add chan callbacks

[Ulisses Furquim]

Hi Andrei,

On Fri, Apr 20, 2012 at 8:09 AM, Andrei Emeltchenko
<Andrei.Emeltchenko.news@gmail.com> wrote:
> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
>
> Add state change, close and skb allocation callbacks.
>
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  net/bluetooth/a2mp.c |   40 ++++++++++++++++++++++++++++++++++++++++
>  1 file changed, 40 insertions(+)
>
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index cb3bf74..67a7ad4 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -63,8 +63,46 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
>        kfree(cmd);
>  }
>
> +static void a2mp_chan_state_change_cb(void *data, int state)
> +{
> +       struct amp_mgr *mgr = data;
> +       struct l2cap_chan *chan;
> +
> +       if (!mgr)
> +               return;
> +
> +       chan = mgr->a2mp_chan;
> +
> +       BT_DBG("chan %p state %s", chan, state_to_string(state));
> +
> +       chan->state = state;

Do we have to l2cap_chan_lock() here? Or is that taken care somewhere else?

> +       switch (state) {
> +       case BT_CLOSED:
> +               if (mgr)
> +                       amp_mgr_put(mgr);
> +               break;
> +       }
> +}
> +
> +static void a2mp_chan_close_cb(void *data)
> +{
> +       struct amp_mgr *mgr = data;
> +
> +       l2cap_chan_destroy(mgr->a2mp_chan);
> +}
> +
> +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
> +                                             unsigned long len, int nb)
> +{
> +       return bt_skb_alloc(len, GFP_KERNEL);
> +}
> +
>  static struct l2cap_ops a2mp_chan_ops = {
>        .name = "L2CAP A2MP channel",
> +       .state_change = a2mp_chan_state_change_cb,
> +       .close = a2mp_chan_close_cb,
> +       .alloc_skb = a2mp_chan_alloc_skb_cb,
>  };
>
>  static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
> @@ -102,6 +140,8 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
>        chan->remote_mps = chan->omtu;
>        chan->mps = chan->omtu;
>
> +       chan->state = BT_CONNECTED;
> +
>        return chan;
>  }
>
> --
> 1.7.9.5

Regards,

-- 
Ulisses Furquim
ProFUSION embedded systems
http://profusion.mobi
Mobile: +55 19 9250 0942
Skype: ulissesffs

Re: [RFCv7 02/23] Bluetooth: A2MP: AMP Manager basic functions

[Ulisses Furquim]

Hi Andrei,

On Fri, Apr 20, 2012 at 8:09 AM, Andrei Emeltchenko
<Andrei.Emeltchenko.news@gmail.com> wrote:
> 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     |   30 +++++++++++++++++++++
>  include/net/bluetooth/hci_core.h |    1 +
>  net/bluetooth/a2mp.c             |   54 ++++++++++++++++++++++++++++++++++++++
>  net/bluetooth/hci_conn.c         |   15 +++++++++++
>  4 files changed, 100 insertions(+)
>  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..0fe8ddd
> --- /dev/null
> +++ b/include/net/bluetooth/a2mp.h
> @@ -0,0 +1,30 @@
> +/*
> +   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
> +   Copyright (c) 2011,2012 Intel Corp.
> +
> +   This program is free software; you can redistribute it and/or modify
> +   it under the terms of the GNU General Public License version 2 and
> +   only version 2 as published by the Free Software Foundation.
> +
> +   This program is distributed in the hope that it will be useful,
> +   but WITHOUT ANY WARRANTY; without even the implied warranty of
> +   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
> +   GNU General Public License for more details.
> +*/
> +
> +#ifndef __A2MP_H
> +#define __A2MP_H
> +
> +struct amp_mgr {
> +       struct list_head        list;
> +       struct l2cap_conn       *l2cap_conn;
> +       struct l2cap_chan       *a2mp_chan;
> +       struct kref             kref;
> +       __u8                    ident;
> +       unsigned long           flags;
> +};
> +
> +void amp_mgr_get(struct amp_mgr *mgr);
> +int amp_mgr_put(struct amp_mgr *mgr);
> +
> +#endif /* __A2MP_H */
> diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
> index ef6e654..16ab35d 100644
> --- a/include/net/bluetooth/hci_core.h
> +++ b/include/net/bluetooth/hci_core.h
> @@ -324,6 +324,7 @@ struct hci_conn {
>
>        struct sk_buff_head data_q;
>        struct list_head chan_list;
> +       struct list_head mgr_list;
>
>        struct delayed_work disc_work;
>        struct timer_list idle_timer;
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index 1f733b5..85cd29a 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -15,6 +15,7 @@
>  #include <net/bluetooth/bluetooth.h>
>  #include <net/bluetooth/hci_core.h>
>  #include <net/bluetooth/l2cap.h>
> +#include <net/bluetooth/a2mp.h>
>
>  static struct l2cap_ops a2mp_chan_ops = {
>        .name = "L2CAP A2MP channel",
> @@ -57,3 +58,56 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
>
>        return chan;
>  }
> +
> +/* AMP Manager functions */
> +void amp_mgr_get(struct amp_mgr *mgr)

I'm probably late to say this but maybe amp_mgr_hold() instead of
_get() like we already have in the stack would be better?

Regards,

-- Ulisses

> +{
> +       BT_DBG("mgr %p", mgr);
> +
> +       kref_get(&mgr->kref);
> +}
> +
> +static void amp_mgr_destroy(struct kref *kref)
> +{
> +       struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
> +
> +       BT_DBG("mgr %p", mgr);
> +
> +       kfree(mgr);
> +}
> +
> +int amp_mgr_put(struct amp_mgr *mgr)
> +{
> +       BT_DBG("mgr %p", mgr);
> +
> +       return kref_put(&mgr->kref, &amp_mgr_destroy);
> +}
> +
> +static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
> +{
> +       struct amp_mgr *mgr;
> +       struct l2cap_chan *chan;
> +
> +       mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
> +       if (!mgr)
> +               return NULL;
> +
> +       BT_DBG("conn %p mgr %p", conn, mgr);
> +
> +       mgr->l2cap_conn = conn;
> +
> +       chan = a2mp_chan_open(conn);
> +       if (!chan) {
> +               kfree(mgr);
> +               return NULL;
> +       }
> +
> +       mgr->a2mp_chan = chan;
> +       chan->data = mgr;
> +
> +       list_add(&mgr->list, &conn->hcon->mgr_list);
> +
> +       kref_init(&mgr->kref);
> +
> +       return mgr;
> +}
> diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
> index 947172b..2de2773 100644
> --- a/net/bluetooth/hci_conn.c
> +++ b/net/bluetooth/hci_conn.c
> @@ -43,6 +43,7 @@
>
>  #include <net/bluetooth/bluetooth.h>
>  #include <net/bluetooth/hci_core.h>
> +#include <net/bluetooth/a2mp.h>
>
>  static void hci_le_connect(struct hci_conn *conn)
>  {
> @@ -404,6 +405,8 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
>
>        INIT_LIST_HEAD(&conn->chan_list);
>
> +       INIT_LIST_HEAD(&conn->mgr_list);
> +
>        INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout);
>        setup_timer(&conn->idle_timer, hci_conn_idle, (unsigned long)conn);
>        setup_timer(&conn->auto_accept_timer, hci_conn_auto_accept,
> @@ -424,6 +427,16 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
>        return conn;
>  }
>
> +static void hci_amp_mgr_list_flush(struct hci_conn *conn)
> +{
> +       struct amp_mgr *mgr, *n;
> +
> +       BT_DBG("conn %p", conn);
> +
> +       list_for_each_entry_safe(mgr, n, &conn->mgr_list, list)
> +               amp_mgr_put(mgr);
> +}
> +
>  int hci_conn_del(struct hci_conn *conn)
>  {
>        struct hci_dev *hdev = conn->hdev;
> @@ -459,6 +472,8 @@ int hci_conn_del(struct hci_conn *conn)
>
>        hci_chan_list_flush(conn);
>
> +       hci_amp_mgr_list_flush(conn);
> +
>        hci_conn_hash_del(hdev, conn);
>        if (hdev->notify)
>                hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
> --
> 1.7.9.5
>
> --
> To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
> the body of a message to majordomo@vger.kernel.org
> More majordomo info at  http://vger.kernel.org/majordomo-info.html

-- 
Ulisses Furquim
ProFUSION embedded systems
http://profusion.mobi
Mobile: +55 19 9250 0942
Skype: ulissesffs

Re: [RFCv7 02/23] Bluetooth: A2MP: AMP Manager basic functions

[Andrei Emeltchenko]

Hi Ulisses,

On Fri, Apr 20, 2012 at 11:51:53AM -0300, Ulisses Furquim wrote:
> > +/* AMP Manager functions */
> > +void amp_mgr_get(struct amp_mgr *mgr)
> 
> I'm probably late to say this but maybe amp_mgr_hold() instead of
> _get() like we already have in the stack would be better?

I actually like more get since this indicates that kref is used with
kref_get as shown below.

Best regards 
Andrei Emeltchenko 

> 
> Regards,
> 
> -- Ulisses
> 
> > +{
> > +       BT_DBG("mgr %p", mgr);
> > +
> > +       kref_get(&mgr->kref);
> > +}
> > +

Re: [RFCv7 04/23] Bluetooth: A2MP: Add chan callbacks

[Andrei Emeltchenko]

Hi Ulisses,

On Fri, Apr 20, 2012 at 11:09:29AM -0300, Ulisses Furquim wrote:
> > +static void a2mp_chan_state_change_cb(void *data, int state)
> > +{
> > +       struct amp_mgr *mgr = data;
> > +       struct l2cap_chan *chan;
> > +
> > +       if (!mgr)
> > +               return;
> > +
> > +       chan = mgr->a2mp_chan;
> > +
> > +       BT_DBG("chan %p state %s", chan, state_to_string(state));
> > +
> > +       chan->state = state;
> 
> Do we have to l2cap_chan_lock() here? Or is that taken care somewhere else?

Yes by caller of state_change

Best regards 
Andrei Emeltchenko 

Re: [RFCv7 17/23] Bluetooth: A2MP: Manage incoming connections

[Gustavo Padovan]

Hi Andrei,

* Andrei Emeltchenko <Andrei.Emeltchenko.news@gmail.com> [2012-04-20 14:09:45 +0300]:

> From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> 
> Handle incoming A2MP connection by creating AMP manager and
> processing A2MP messages.
> 
> Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
> ---
>  include/net/bluetooth/a2mp.h |    4 ++++
>  net/bluetooth/a2mp.c         |   12 ++++++++++++
>  net/bluetooth/l2cap_core.c   |   13 +++++++++----
>  3 files changed, 25 insertions(+), 4 deletions(-)
> 
> diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
> index c9aa9c5..6304e14 100644
> --- a/include/net/bluetooth/a2mp.h
> +++ b/include/net/bluetooth/a2mp.h
> @@ -15,6 +15,8 @@
>  #ifndef __A2MP_H
>  #define __A2MP_H
>  
> +#include <net/bluetooth/l2cap.h>
> +
>  #define A2MP_FEAT_EXT	0x8000
>  
>  struct amp_mgr {
> @@ -118,5 +120,7 @@ struct a2mp_physlink_rsp {
>  
>  void amp_mgr_get(struct amp_mgr *mgr);
>  int amp_mgr_put(struct amp_mgr *mgr);
> +struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
> +				       struct sk_buff *skb);
>  
>  #endif /* __A2MP_H */
> diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
> index aa93e0b..a49acb6 100644
> --- a/net/bluetooth/a2mp.c
> +++ b/net/bluetooth/a2mp.c
> @@ -541,3 +541,15 @@ static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
>  
>  	return mgr;
>  }
> +
> +struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
> +				       struct sk_buff *skb)
> +{
> +	struct amp_mgr *mgr;
> +
> +	mgr = amp_mgr_create(conn);
> +
> +	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
> +
> +	return mgr->a2mp_chan;
> +}
> diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
> index d0aeb56..ed434b0 100644
> --- a/net/bluetooth/l2cap_core.c
> +++ b/net/bluetooth/l2cap_core.c
> @@ -56,6 +56,7 @@
>  #include <net/bluetooth/hci_core.h>
>  #include <net/bluetooth/l2cap.h>
>  #include <net/bluetooth/smp.h>
> +#include <net/bluetooth/a2mp.h>
>  
>  bool disable_ertm;
>  
> @@ -4554,10 +4555,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);
> -		/* Drop packet and return */
> -		kfree_skb(skb);
> -		return 0;
> +		if (cid == L2CAP_CID_A2MP) {
> +			chan = a2mp_channel_create(conn, skb);

This code should be inside l2cap_recv_frame. take a look there.

	Gustavo

Re: [RFCv7 17/23] Bluetooth: A2MP: Manage incoming connections

[Andrei Emeltchenko]

Hi Gustavo,

On Wed, Apr 25, 2012 at 04:58:25PM -0300, Gustavo Padovan wrote:
... 
> > @@ -4554,10 +4555,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);
> > -		/* Drop packet and return */
> > -		kfree_skb(skb);
> > -		return 0;
> > +		if (cid == L2CAP_CID_A2MP) {
> > +			chan = a2mp_channel_create(conn, skb);
> 
> This code should be inside l2cap_recv_frame. take a look there.

No, it shouldn't. We do not want to reimplement ERTM logic inside A2MP.

Best regards 
Andrei Emeltchenko