From 466f8004f364e9cb46d9124109972489eccfb404 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:01 +0300
Subject: Bluetooth: A2MP: Create A2MP channel

Create and initialize fixed A2MP channel

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 69 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)
 create mode 100644 net/bluetooth/a2mp.c

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
new file mode 100644
index 00000000000..de455a26445
--- /dev/null
+++ b/net/bluetooth/a2mp.c
@@ -0,0 +1,69 @@
+/*
+   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;
+	int err;
+
+	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;
+
+	err = l2cap_ertm_init(chan);
+	if (err < 0) {
+		l2cap_chan_del(chan, 0);
+		return NULL;
+	}
+
+	chan->conf_state = 0;
+
+	l2cap_chan_add(conn, chan);
+
+	chan->remote_mps = chan->omtu;
+	chan->mps = chan->omtu;
+
+	chan->state = BT_CONNECTED;
+
+	return chan;
+}
-- 
cgit v1.2.3-70-g09d2


From 9740e49d17e55f3832661fd99a8e0a17e921a82e Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:02 +0300
Subject: Bluetooth: A2MP: AMP Manager basic functions

Define AMP Manager and some basic functions.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 include/net/bluetooth/a2mp.h     | 30 ++++++++++++++++++++++
 include/net/bluetooth/hci_core.h |  1 +
 net/bluetooth/a2mp.c             | 54 ++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/hci_conn.c         |  4 +++
 4 files changed, 89 insertions(+)
 create mode 100644 include/net/bluetooth/a2mp.h

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
new file mode 100644
index 00000000000..ff4754000cf
--- /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 l2cap_conn	*l2cap_conn;
+	struct l2cap_chan	*a2mp_chan;
+	struct kref		kref;
+	__u8			ident;
+	__u8			handle;
+	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 d584a47d1c8..6e64b76e30a 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -332,6 +332,7 @@ struct hci_conn {
 	void		*l2cap_data;
 	void		*sco_data;
 	void		*smp_conn;
+	struct amp_mgr	*amp_mgr;
 
 	struct hci_conn	*link;
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index de455a26445..3c241c2b3e1 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",
@@ -67,3 +68,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;
+
+	conn->hcon->amp_mgr = mgr;
+
+	kref_init(&mgr->kref);
+
+	return mgr;
+}
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 126876d915f..1458667b284 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -28,6 +28,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)
 {
@@ -411,6 +412,9 @@ int hci_conn_del(struct hci_conn *conn)
 
 	hci_chan_list_flush(conn);
 
+	if (conn->amp_mgr)
+		amp_mgr_put(conn->amp_mgr);
+
 	hci_conn_hash_del(hdev, conn);
 	if (hdev->notify)
 		hdev->notify(hdev, HCI_NOTIFY_CONN_DEL);
-- 
cgit v1.2.3-70-g09d2


From f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:03 +0300
Subject: Bluetooth: A2MP: Build and Send msg helpers

Helper function to build and send A2MP messages.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 include/net/bluetooth/a2mp.h |  7 +++++++
 net/bluetooth/a2mp.c         | 46 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 53 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index ff4754000cf..654df60cfd6 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 3c241c2b3e1..53f49a0b7f9 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",
 };
-- 
cgit v1.2.3-70-g09d2


From 46d5c9088fbcc8a570bc271f77940973d9cae074 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:04 +0300
Subject: Bluetooth: A2MP: Add chan callbacks

Add L2CAP chan ops callbacks.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 55 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 55 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 53f49a0b7f9..f7c710cacf4 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,8 +63,63 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
 	kfree(cmd);
 }
 
+static void a2mp_chan_close_cb(struct l2cap_chan *chan)
+{
+	l2cap_chan_destroy(chan);
+}
+
+static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
+{
+	struct amp_mgr *mgr = chan->data;
+
+	if (!mgr)
+		return;
+
+	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 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_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan)
+{
+	BT_ERR("new_connection for chan %p not implemented", chan);
+
+	return NULL;
+}
+
+static void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err)
+{
+	BT_ERR("teardown for chan %p not implemented", chan);
+}
+
+static void a2mp_chan_no_ready(struct l2cap_chan *chan)
+{
+	BT_ERR("ready for chan %p not implemented", chan);
+}
+
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.close = a2mp_chan_close_cb,
+	.state_change = a2mp_chan_state_change_cb,
+	.alloc_skb = a2mp_chan_alloc_skb_cb,
+
+	/* Not implemented for A2MP */
+	.new_connection = a2mp_chan_no_new_conn_cb,
+	.teardown = a2mp_chan_no_teardown_cb,
+	.ready = a2mp_chan_no_ready,
 };
 
 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
-- 
cgit v1.2.3-70-g09d2


From 6b44d9b8d96b37f72ccd7335b32f386a67b7f1f4 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:07 +0300
Subject: Bluetooth: A2MP: Process A2MP messages

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>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 65 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 65 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index f7c710cacf4..0726c9fe005 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -63,6 +63,70 @@ 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(struct l2cap_chan *chan, struct sk_buff *skb)
+{
+	struct a2mp_cmd *hdr = (void *) skb->data;
+	struct amp_mgr *mgr = chan->data;
+	int err = 0;
+
+	amp_mgr_get(mgr);
+
+	while (skb->len >= sizeof(*hdr)) {
+		struct a2mp_cmd *hdr = (void *) skb->data;
+		u16 len = le16_to_cpu(hdr->len);
+
+		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 = __constant_cpu_to_le16(0);
+
+		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
+
+		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_close_cb(struct l2cap_chan *chan)
 {
 	l2cap_chan_destroy(chan);
@@ -112,6 +176,7 @@ static void a2mp_chan_no_ready(struct l2cap_chan *chan)
 
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
+	.recv = a2mp_chan_recv_cb,
 	.close = a2mp_chan_close_cb,
 	.state_change = a2mp_chan_state_change_cb,
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
-- 
cgit v1.2.3-70-g09d2


From 21dbd2ce35f6d2b4aa5363be6c839cdb50644e11 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:08 +0300
Subject: Bluetooth: A2MP: Process A2MP Command Reject

Placeholder for future A2MP Command Reject handler.

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

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0726c9fe005..188b4212007 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 = (void *) 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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -89,6 +105,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From 8598d064cbf22b2d84c7cd8a9fcb97138baffe3f Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:09 +0300
Subject: Bluetooth: A2MP: Process A2MP Discover Request

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>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 include/net/bluetooth/a2mp.h     |  2 +
 include/net/bluetooth/hci.h      |  3 ++
 include/net/bluetooth/hci_core.h | 13 ++++++
 net/bluetooth/a2mp.c             | 85 ++++++++++++++++++++++++++++++++++++++++
 4 files changed, 103 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 391acd7a67d..96f9cc2cf59 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 l2cap_conn	*l2cap_conn;
 	struct l2cap_chan	*a2mp_chan;
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index de09a26e422..66af2c6193d 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -59,6 +59,9 @@
 #define HCI_BREDR	0x00
 #define HCI_AMP		0x01
 
+/* First BR/EDR Controller shall have ID = 0 */
+#define HCI_BREDR_ID	0
+
 /* HCI device quirks */
 enum {
 	HCI_QUIRK_RESET_ON_CLOSE,
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 6e64b76e30a..20fd57367dd 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -641,6 +641,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 188b4212007..1cc920a62b0 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 int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
+			     struct a2mp_cmd *hdr)
+{
+	struct a2mp_discov_req *req = (void *) 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 = __constant_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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -109,6 +191,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From 329d81af29344a2ad2f9595310be74644421797a Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:10 +0300
Subject: Bluetooth: A2MP: Process A2MP Change Notify

Placeholder for A2MP Change Notify handler.

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

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 1cc920a62b0..6cdaa85ab5e 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -161,6 +161,22 @@ static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
+			      struct a2mp_cmd *hdr)
+{
+	struct a2mp_cl *cl = (void *) 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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -195,6 +211,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From 47f2d97d38816aaca94c9b6961c6eff1cfcd0bd6 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:11 +0300
Subject: Bluetooth: A2MP: Process A2MP Get Info Request

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>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 37 +++++++++++++++++++++++++++++++++++++
 1 file changed, 37 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 6cdaa85ab5e..350088e2015 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -177,6 +177,40 @@ static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
 	return 0;
 }
 
+static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
+			    struct a2mp_cmd *hdr)
+{
+	struct a2mp_info_req *req  = (void *) skb->data;
+	struct a2mp_info_rsp rsp;
+	struct hci_dev *hdev;
+
+	if (le16_to_cpu(hdr->len) < sizeof(*req))
+		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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -215,6 +249,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:12 +0300
Subject: Bluetooth: A2MP: Process A2MP Get AMP Assoc Request

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>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 35 +++++++++++++++++++++++++++++++++++
 1 file changed, 35 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 350088e2015..b8eeb33d60d 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -211,6 +211,38 @@ static 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 = (void *) 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;
+	}
+
+	/* Placeholder for HCI Read AMP Assoc */
+
+clean:
+	if (hdev)
+		hci_dev_put(hdev);
+
+	skb_pull(skb, sizeof(*req));
+	return 0;
+}
+
 /* Handle A2MP signalling */
 static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -253,6 +285,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From e072f5dab22e7bf0a10daf854acc0fc271396ee7 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:13 +0300
Subject: Bluetooth: A2MP: Process A2MP Create Physlink Request

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>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 40 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index b8eeb33d60d..ca52a91edd2 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -243,6 +243,43 @@ 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 = (void *) 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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -289,6 +326,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From 6113f84fc1a8962aed25f54a115b196e9aea151f Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:14 +0300
Subject: Bluetooth: A2MP: Process A2MP Disc Physlink Request

Placeholder for A2MP Disconnect Physlink Request.

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

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index ca52a91edd2..400696b3b81 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 = (void *) 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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -330,6 +363,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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:
-- 
cgit v1.2.3-70-g09d2


From f6410a849b76f56c78d989786eb427b85a559b9f Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:15 +0300
Subject: Bluetooth: A2MP: Process A2MP Command Responses

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

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

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 400696b3b81..6a933dab1b7 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(struct l2cap_chan *chan, struct sk_buff *skb)
 {
@@ -372,6 +381,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan *chan, 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;
-- 
cgit v1.2.3-70-g09d2


From 416fa7527d6bf658e5517ea36d2de9270be2c11e Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:16 +0300
Subject: Bluetooth: A2MP: Handling fixed channels

A2MP fixed channel do not have sk

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 include/net/bluetooth/l2cap.h |  1 +
 net/bluetooth/a2mp.c          |  3 +--
 net/bluetooth/l2cap_core.c    | 17 +++++++++++++++++
 3 files changed, 19 insertions(+), 2 deletions(-)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index aaba222306b..a00b43ecbc7 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -581,6 +581,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 6a933dab1b7..f1ec1b1d308 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -483,8 +483,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 2c616cf24c7..fc572795497 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -465,6 +465,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;
@@ -1001,6 +1008,11 @@ static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *c
 		__clear_ack_timer(chan);
 	}
 
+	if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+		__l2cap_state_change(chan, BT_DISCONN);
+		return;
+	}
+
 	req.dcid = cpu_to_le16(chan->dcid);
 	req.scid = cpu_to_le16(chan->scid);
 	l2cap_send_cmd(conn, l2cap_get_ident(conn),
@@ -1195,6 +1207,11 @@ static void l2cap_conn_ready(struct l2cap_conn *conn)
 
 		l2cap_chan_lock(chan);
 
+		if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) {
+			l2cap_chan_unlock(chan);
+			continue;
+		}
+
 		if (conn->hcon->type == LE_LINK) {
 			if (smp_conn_security(conn, chan->sec_level))
 				l2cap_chan_ready(chan);
-- 
cgit v1.2.3-70-g09d2


From 97e8e89d2d8185b7644c9941636d3682eedc517b Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Tue, 29 May 2012 13:59:17 +0300
Subject: Bluetooth: A2MP: Manage incoming connections

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

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 include/net/bluetooth/a2mp.h |  4 ++++
 net/bluetooth/a2mp.c         | 16 ++++++++++++++++
 net/bluetooth/l2cap_core.c   | 19 +++++++++++++++----
 3 files changed, 35 insertions(+), 4 deletions(-)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/a2mp.h b/include/net/bluetooth/a2mp.h
index 96f9cc2cf59..6a76e0a0705 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 f1ec1b1d308..e08ca2ac31a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -569,3 +569,19 @@ 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);
+	if (!mgr) {
+		BT_ERR("Could not create AMP manager");
+		return NULL;
+	}
+
+	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 fc572795497..3daac2c6b7b 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -37,6 +37,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;
 
@@ -5132,10 +5133,20 @@ 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);
+			if (!chan) {
+				kfree_skb(skb);
+				return 0;
+			}
+
+			l2cap_chan_lock(chan);
+		} else {
+			BT_DBG("unknown cid 0x%4.4x", cid);
+			/* Drop packet and return */
+			kfree_skb(skb);
+			return 0;
+		}
 	}
 
 	BT_DBG("chan %p, len %d", chan, skb->len);
-- 
cgit v1.2.3-70-g09d2


From 3cabbfdad3150e0083a64c951701c970ef970c77 Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Thu, 31 May 2012 11:01:37 +0300
Subject: Bluetooth: A2MP: Do not reference hci_conn

Make A2MP channel special channel which do not reference hci_conn.
This prevents from keeping ACL connection open when all L2CAP
channels got closed.

hci_conn_hold and hci_conn_put are not reference counts on the
hci_conn structure in the typical way.  They are reference counts for
the ACL.  When you do the last hci_conn_put, the ACL is disconnected
after a timeout.

Reported-by: Mat Martineau <mathewm@codeaurora.org>
Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c       | 2 --
 net/bluetooth/l2cap_core.c | 4 +++-
 2 files changed, 3 insertions(+), 3 deletions(-)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index e08ca2ac31a..0772c680abe 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -481,8 +481,6 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 
 	BT_DBG("chan %p", chan);
 
-	hci_conn_hold(conn->hcon);
-
 	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
 	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
 
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 953ac53ae43..bef5266563a 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -514,7 +514,9 @@ void l2cap_chan_del(struct l2cap_chan *chan, int err)
 		l2cap_chan_put(chan);
 
 		chan->conn = NULL;
-		hci_conn_put(conn->hcon);
+
+		if (chan->chan_type != L2CAP_CHAN_CONN_FIX_A2MP)
+			hci_conn_put(conn->hcon);
 	}
 
 	if (chan->ops->teardown)
-- 
cgit v1.2.3-70-g09d2


From 7e1af8a3a51dbf5dc7392fb294a0830f7e853aa8 Mon Sep 17 00:00:00 2001
From: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
Date: Tue, 29 May 2012 13:19:26 -0300
Subject: Bluetooth: Create empty l2cap ops function

A2MP doesn't use part of the L2CAP chan ops API so we just create general
empty function instead of the A2MP specific one.

Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
Signed-off-by: Johan Hedberg <johan.hedberg@intel.com>
---
 include/net/bluetooth/l2cap.h | 12 ++++++++++++
 net/bluetooth/a2mp.c          | 23 +++--------------------
 2 files changed, 15 insertions(+), 20 deletions(-)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index ce99c5683d9..d80e3f0691b 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -740,6 +740,18 @@ static inline __u16 __next_seq(struct l2cap_chan *chan, __u16 seq)
 	return (seq + 1) % (chan->tx_win_max + 1);
 }
 
+static inline struct l2cap_chan *l2cap_chan_no_new_connection(struct l2cap_chan *chan)
+{
+	return NULL;
+}
+
+static inline void l2cap_chan_no_teardown(struct l2cap_chan *chan, int err)
+{
+}
+
+static inline void l2cap_chan_no_ready(struct l2cap_chan *chan)
+{
+}
 
 extern bool disable_ertm;
 
diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index 0772c680abe..fb93250b393 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -440,23 +440,6 @@ static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
 	return bt_skb_alloc(len, GFP_KERNEL);
 }
 
-static struct l2cap_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan)
-{
-	BT_ERR("new_connection for chan %p not implemented", chan);
-
-	return NULL;
-}
-
-static void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err)
-{
-	BT_ERR("teardown for chan %p not implemented", chan);
-}
-
-static void a2mp_chan_no_ready(struct l2cap_chan *chan)
-{
-	BT_ERR("ready for chan %p not implemented", chan);
-}
-
 static struct l2cap_ops a2mp_chan_ops = {
 	.name = "L2CAP A2MP channel",
 	.recv = a2mp_chan_recv_cb,
@@ -465,9 +448,9 @@ static struct l2cap_ops a2mp_chan_ops = {
 	.alloc_skb = a2mp_chan_alloc_skb_cb,
 
 	/* Not implemented for A2MP */
-	.new_connection = a2mp_chan_no_new_conn_cb,
-	.teardown = a2mp_chan_no_teardown_cb,
-	.ready = a2mp_chan_no_ready,
+	.new_connection = l2cap_chan_no_new_connection,
+	.teardown = l2cap_chan_no_teardown,
+	.ready = l2cap_chan_no_ready,
 };
 
 static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
-- 
cgit v1.2.3-70-g09d2


From a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fda Mon Sep 17 00:00:00 2001
From: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Date: Wed, 11 Jul 2012 14:43:35 +0300
Subject: Bluetooth: debug: Print amp_mgr refcnt

Improve debug output.

Signed-off-by: Andrei Emeltchenko <andrei.emeltchenko@intel.com>
Signed-off-by: Gustavo Padovan <gustavo.padovan@collabora.co.uk>
---
 net/bluetooth/a2mp.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'net/bluetooth/a2mp.c')

diff --git a/net/bluetooth/a2mp.c b/net/bluetooth/a2mp.c
index fb93250b393..4ff0bf3ba9a 100644
--- a/net/bluetooth/a2mp.c
+++ b/net/bluetooth/a2mp.c
@@ -501,7 +501,7 @@ static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
 /* AMP Manager functions */
 void amp_mgr_get(struct amp_mgr *mgr)
 {
-	BT_DBG("mgr %p", mgr);
+	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 
 	kref_get(&mgr->kref);
 }
@@ -517,7 +517,7 @@ static void amp_mgr_destroy(struct kref *kref)
 
 int amp_mgr_put(struct amp_mgr *mgr)
 {
-	BT_DBG("mgr %p", mgr);
+	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
 
 	return kref_put(&mgr->kref, &amp_mgr_destroy);
 }
-- 
cgit v1.2.3-70-g09d2