[deliverable/linux.git] / net / bluetooth / a2mp.c

/*
   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>
#include <net/bluetooth/a2mp.h>

/* Global AMP Manager list */
LIST_HEAD(amp_mgr_list);
DEFINE_MUTEX(amp_mgr_list_lock);

/* A2MP build & send command helper functions */
static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
{
	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 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)
{
	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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

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;
}

static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
			       struct a2mp_cmd *hdr)
{
	BT_DBG("ident %d code 0x%2.2x", 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)
{
	struct a2mp_cmd *hdr;
	struct amp_mgr *mgr = chan->data;
	int err = 0;

	amp_mgr_get(mgr);

	while (skb->len >= sizeof(*hdr)) {
		u16 len;

		hdr = (void *) skb->data;
		len = le16_to_cpu(hdr->len);

		BT_DBG("code 0x%2.2x id %d len %u", 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:
			a2mp_command_rej(mgr, skb, hdr);
			break;

		case A2MP_DISCOVER_REQ:
			err = a2mp_discover_req(mgr, skb, hdr);
			break;

		case A2MP_CHANGE_NOTIFY:
			err = a2mp_change_notify(mgr, skb, hdr);
			break;

		case A2MP_GETINFO_REQ:
			err = a2mp_getinfo_req(mgr, skb, hdr);
			break;

		case A2MP_GETAMPASSOC_REQ:
			err = a2mp_getampassoc_req(mgr, skb, hdr);
			break;

		case A2MP_CREATEPHYSLINK_REQ:
			err = a2mp_createphyslink_req(mgr, skb, hdr);
			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:
		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;
			break;
		}
	}

	if (err) {
		struct a2mp_cmd_rej rej;

		rej.reason = __constant_cpu_to_le16(0);
		hdr = (void *) skb->data;

		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);

		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_put(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_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,

	/* Not implemented for A2MP */
	.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)
{
	struct l2cap_chan *chan;
	int err;

	chan = l2cap_chan_create();
	if (!chan)
		return NULL;

	BT_DBG("chan %p", chan);

	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
	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;
}

/* AMP Manager functions */
void amp_mgr_get(struct amp_mgr *mgr)
{
	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));

	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);

	mutex_lock(&amp_mgr_list_lock);
	list_del(&mgr->list);
	mutex_unlock(&amp_mgr_list_lock);

	kfree(mgr);
}

int amp_mgr_put(struct amp_mgr *mgr)
{
	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));

	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);

	mutex_lock(&amp_mgr_list_lock);
	list_add(&mgr->list, &amp_mgr_list);
	mutex_unlock(&amp_mgr_list_lock);

	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;
}

struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
{
	struct amp_mgr *mgr;

	mutex_lock(&amp_mgr_list_lock);
	list_for_each_entry(mgr, &amp_mgr_list, list) {
		if (mgr->state == state) {
			amp_mgr_get(mgr);
			mutex_unlock(&amp_mgr_list_lock);
			return mgr;
		}
	}
	mutex_unlock(&amp_mgr_list_lock);

	return NULL;
}
Commit	Line	Data
466f8004 AE	1	/*
	2	Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved.
	3	Copyright (c) 2011,2012 Intel Corp.
	4
	5	This program is free software; you can redistribute it and/or modify
	6	it under the terms of the GNU General Public License version 2 and
	7	only version 2 as published by the Free Software Foundation.
	8
	9	This program is distributed in the hope that it will be useful,
	10	but WITHOUT ANY WARRANTY; without even the implied warranty of
	11	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	12	GNU General Public License for more details.
	13	*/
	14
	15	#include <net/bluetooth/bluetooth.h>
	16	#include <net/bluetooth/hci_core.h>
	17	#include <net/bluetooth/l2cap.h>
9740e49d	18	#include <net/bluetooth/a2mp.h>
466f8004	19
f97268fc AE	20	/* Global AMP Manager list */
	21	LIST_HEAD(amp_mgr_list);
	22	DEFINE_MUTEX(amp_mgr_list_lock);
	23
f6d3c6e7 AE	24	/* A2MP build & send command helper functions */
	25	static struct a2mp_cmd __a2mp_build(u8 code, u8 ident, u16 len, void data)
	26	{
	27	struct a2mp_cmd *cmd;
	28	int plen;
	29
	30	plen = sizeof(*cmd) + len;
	31	cmd = kzalloc(plen, GFP_KERNEL);
	32	if (!cmd)
	33	return NULL;
	34
	35	cmd->code = code;
	36	cmd->ident = ident;
	37	cmd->len = cpu_to_le16(len);
	38
	39	memcpy(cmd->data, data, len);
	40
	41	return cmd;
	42	}
	43
	44	static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
	45	void *data)
	46	{
	47	struct l2cap_chan *chan = mgr->a2mp_chan;
	48	struct a2mp_cmd *cmd;
	49	u16 total_len = len + sizeof(*cmd);
	50	struct kvec iv;
	51	struct msghdr msg;
	52
	53	cmd = __a2mp_build(code, ident, len, data);
	54	if (!cmd)
	55	return;
	56
	57	iv.iov_base = cmd;
	58	iv.iov_len = total_len;
	59
	60	memset(&msg, 0, sizeof(msg));
	61
	62	msg.msg_iov = (struct iovec *) &iv;
	63	msg.msg_iovlen = 1;
	64
	65	l2cap_chan_send(chan, &msg, total_len, 0);
	66
	67	kfree(cmd);
	68	}
	69
8598d064 AE	70	static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
	71	{
	72	cl->id = 0;
	73	cl->type = 0;
	74	cl->status = 1;
	75	}
	76
	77	/* hci_dev_list shall be locked */
	78	static void __a2mp_add_cl(struct amp_mgr mgr, struct a2mp_cl cl, u8 num_ctrl)
	79	{
	80	int i = 0;
	81	struct hci_dev *hdev;
	82
	83	__a2mp_cl_bredr(cl);
	84
	85	list_for_each_entry(hdev, &hci_dev_list, list) {
	86	/* Iterate through AMP controllers */
	87	if (hdev->id == HCI_BREDR_ID)
	88	continue;
	89
	90	/* Starting from second entry */
	91	if (++i >= num_ctrl)
	92	return;
	93
	94	cl[i].id = hdev->id;
	95	cl[i].type = hdev->amp_type;
	96	cl[i].status = hdev->amp_status;
	97	}
	98	}
	99
21dbd2ce AE	100	/* Processing A2MP messages */
	101	static int a2mp_command_rej(struct amp_mgr mgr, struct sk_buff skb,
	102	struct a2mp_cmd *hdr)
	103	{
	104	struct a2mp_cmd_rej rej = (void ) skb->data;
	105
	106	if (le16_to_cpu(hdr->len) < sizeof(*rej))
	107	return -EINVAL;
	108
	109	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
	110
	111	skb_pull(skb, sizeof(*rej));
	112
	113	return 0;
	114	}
	115
8598d064 AE	116	static int a2mp_discover_req(struct amp_mgr mgr, struct sk_buff skb,
	117	struct a2mp_cmd *hdr)
	118	{
	119	struct a2mp_discov_req req = (void ) skb->data;
	120	u16 len = le16_to_cpu(hdr->len);
	121	struct a2mp_discov_rsp *rsp;
	122	u16 ext_feat;
	123	u8 num_ctrl;
	124
	125	if (len < sizeof(*req))
	126	return -EINVAL;
	127
	128	skb_pull(skb, sizeof(*req));
	129
	130	ext_feat = le16_to_cpu(req->ext_feat);
	131
	132	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
	133
	134	/* check that packet is not broken for now */
	135	while (ext_feat & A2MP_FEAT_EXT) {
	136	if (len < sizeof(ext_feat))
	137	return -EINVAL;
	138
	139	ext_feat = get_unaligned_le16(skb->data);
	140	BT_DBG("efm 0x%4.4x", ext_feat);
	141	len -= sizeof(ext_feat);
	142	skb_pull(skb, sizeof(ext_feat));
	143	}
	144
	145	read_lock(&hci_dev_list_lock);
	146
	147	num_ctrl = __hci_num_ctrl();
	148	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
	149	rsp = kmalloc(len, GFP_ATOMIC);
	150	if (!rsp) {
	151	read_unlock(&hci_dev_list_lock);
	152	return -ENOMEM;
	153	}
	154
	155	rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
	156	rsp->ext_feat = 0;
	157
	158	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
	159
	160	read_unlock(&hci_dev_list_lock);
	161
	162	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
	163
	164	kfree(rsp);
	165	return 0;
	166	}
	167
329d81af AE	168	static int a2mp_change_notify(struct amp_mgr mgr, struct sk_buff skb,
	169	struct a2mp_cmd *hdr)
	170	{
	171	struct a2mp_cl cl = (void ) skb->data;
	172
	173	while (skb->len >= sizeof(*cl)) {
	174	BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
	175	cl->status);
	176	cl = (struct a2mp_cl ) skb_pull(skb, sizeof(cl));
	177	}
	178
	179	/* TODO send A2MP_CHANGE_RSP */
	180
	181	return 0;
	182	}
	183
47f2d97d AE	184	static int a2mp_getinfo_req(struct amp_mgr mgr, struct sk_buff skb,
	185	struct a2mp_cmd *hdr)
	186	{
	187	struct a2mp_info_req req = (void ) skb->data;
	188	struct a2mp_info_rsp rsp;
	189	struct hci_dev *hdev;
	190
	191	if (le16_to_cpu(hdr->len) < sizeof(*req))
	192	return -EINVAL;
	193
	194	BT_DBG("id %d", req->id);
	195
	196	rsp.id = req->id;
	197	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	198
	199	hdev = hci_dev_get(req->id);
	200	if (hdev && hdev->amp_type != HCI_BREDR) {
	201	rsp.status = 0;
	202	rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
	203	rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
	204	rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
	205	rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
	206	rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
	207	}
	208
	209	if (hdev)
	210	hci_dev_put(hdev);
	211
	212	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp);
	213
	214	skb_pull(skb, sizeof(*req));
	215	return 0;
	216	}
	217
a28381dc AE	218	static int a2mp_getampassoc_req(struct amp_mgr mgr, struct sk_buff skb,
	219	struct a2mp_cmd *hdr)
	220	{
	221	struct a2mp_amp_assoc_req req = (void ) skb->data;
	222	struct hci_dev *hdev;
	223
	224	if (le16_to_cpu(hdr->len) < sizeof(*req))
	225	return -EINVAL;
	226
	227	BT_DBG("id %d", req->id);
	228
	229	hdev = hci_dev_get(req->id);
	230	if (!hdev \|\| hdev->amp_type == HCI_BREDR) {
	231	struct a2mp_amp_assoc_rsp rsp;
	232	rsp.id = req->id;
	233	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	234
	235	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
	236	&rsp);
	237	goto clean;
	238	}
	239
	240	/* Placeholder for HCI Read AMP Assoc */
	241
	242	clean:
	243	if (hdev)
	244	hci_dev_put(hdev);
	245
	246	skb_pull(skb, sizeof(*req));
	247	return 0;
	248	}
	249
e072f5da AE	250	static int a2mp_createphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	251	struct a2mp_cmd *hdr)
	252	{
	253	struct a2mp_physlink_req req = (void ) skb->data;
	254
	255	struct a2mp_physlink_rsp rsp;
	256	struct hci_dev *hdev;
	257
	258	if (le16_to_cpu(hdr->len) < sizeof(*req))
	259	return -EINVAL;
	260
	261	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
	262
	263	rsp.local_id = req->remote_id;
	264	rsp.remote_id = req->local_id;
	265
	266	hdev = hci_dev_get(req->remote_id);
	267	if (!hdev \|\| hdev->amp_type != HCI_AMP) {
	268	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	269	goto send_rsp;
	270	}
	271
	272	/* TODO process physlink create */
	273
	274	rsp.status = A2MP_STATUS_SUCCESS;
	275
	276	send_rsp:
	277	if (hdev)
	278	hci_dev_put(hdev);
	279
	280	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
	281	&rsp);
	282
	283	skb_pull(skb, le16_to_cpu(hdr->len));
	284	return 0;
	285	}
	286
6113f84f AE	287	static int a2mp_discphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	288	struct a2mp_cmd *hdr)
	289	{
	290	struct a2mp_physlink_req req = (void ) skb->data;
	291	struct a2mp_physlink_rsp rsp;
	292	struct hci_dev *hdev;
	293
	294	if (le16_to_cpu(hdr->len) < sizeof(*req))
	295	return -EINVAL;
	296
	297	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
	298
	299	rsp.local_id = req->remote_id;
	300	rsp.remote_id = req->local_id;
	301	rsp.status = A2MP_STATUS_SUCCESS;
	302
	303	hdev = hci_dev_get(req->local_id);
	304	if (!hdev) {
	305	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	306	goto send_rsp;
	307	}
	308
	309	/* TODO Disconnect Phys Link here */
	310
	311	hci_dev_put(hdev);
	312
	313	send_rsp:
	314	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
	315
	316	skb_pull(skb, sizeof(*req));
	317	return 0;
	318	}
	319
f6410a84 AE	320	static inline int a2mp_cmd_rsp(struct amp_mgr mgr, struct sk_buff skb,
	321	struct a2mp_cmd *hdr)
	322	{
8e8c7e36	323	BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
f6410a84 AE	324
	325	skb_pull(skb, le16_to_cpu(hdr->len));
	326	return 0;
	327	}
	328
6b44d9b8 AE	329	/* Handle A2MP signalling */
	330	static int a2mp_chan_recv_cb(struct l2cap_chan chan, struct sk_buff skb)
	331	{
d9fc1d54	332	struct a2mp_cmd *hdr;
6b44d9b8 AE	333	struct amp_mgr *mgr = chan->data;
	334	int err = 0;
	335
	336	amp_mgr_get(mgr);
	337
	338	while (skb->len >= sizeof(*hdr)) {
d9fc1d54 AE	339	u16 len;
	340
	341	hdr = (void *) skb->data;
	342	len = le16_to_cpu(hdr->len);
6b44d9b8	343
8e8c7e36	344	BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
6b44d9b8 AE	345
	346	skb_pull(skb, sizeof(*hdr));
	347
	348	if (len > skb->len \|\| !hdr->ident) {
	349	err = -EINVAL;
	350	break;
	351	}
	352
	353	mgr->ident = hdr->ident;
	354
	355	switch (hdr->code) {
	356	case A2MP_COMMAND_REJ:
21dbd2ce AE	357	a2mp_command_rej(mgr, skb, hdr);
	358	break;
	359
6b44d9b8	360	case A2MP_DISCOVER_REQ:
8598d064 AE	361	err = a2mp_discover_req(mgr, skb, hdr);
	362	break;
	363
6b44d9b8	364	case A2MP_CHANGE_NOTIFY:
329d81af AE	365	err = a2mp_change_notify(mgr, skb, hdr);
	366	break;
	367
6b44d9b8	368	case A2MP_GETINFO_REQ:
47f2d97d AE	369	err = a2mp_getinfo_req(mgr, skb, hdr);
	370	break;
	371
6b44d9b8	372	case A2MP_GETAMPASSOC_REQ:
a28381dc AE	373	err = a2mp_getampassoc_req(mgr, skb, hdr);
	374	break;
	375
6b44d9b8	376	case A2MP_CREATEPHYSLINK_REQ:
e072f5da AE	377	err = a2mp_createphyslink_req(mgr, skb, hdr);
	378	break;
	379
6b44d9b8	380	case A2MP_DISCONNPHYSLINK_REQ:
6113f84f AE	381	err = a2mp_discphyslink_req(mgr, skb, hdr);
	382	break;
	383
6b44d9b8 AE	384	case A2MP_CHANGE_RSP:
	385	case A2MP_DISCOVER_RSP:
	386	case A2MP_GETINFO_RSP:
	387	case A2MP_GETAMPASSOC_RSP:
	388	case A2MP_CREATEPHYSLINK_RSP:
	389	case A2MP_DISCONNPHYSLINK_RSP:
f6410a84 AE	390	err = a2mp_cmd_rsp(mgr, skb, hdr);
	391	break;
	392
6b44d9b8 AE	393	default:
	394	BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
	395	err = -EINVAL;
	396	break;
	397	}
	398	}
	399
	400	if (err) {
	401	struct a2mp_cmd_rej rej;
d9fc1d54	402
6b44d9b8	403	rej.reason = __constant_cpu_to_le16(0);
d9fc1d54	404	hdr = (void *) skb->data;
6b44d9b8 AE	405
	406	BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
	407
	408	a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
	409	&rej);
	410	}
	411
	412	/* Always free skb and return success error code to prevent
	413	from sending L2CAP Disconnect over A2MP channel */
	414	kfree_skb(skb);
	415
	416	amp_mgr_put(mgr);
	417
	418	return 0;
	419	}
	420
46d5c908 AE	421	static void a2mp_chan_close_cb(struct l2cap_chan *chan)
46d5c908 AE	422	{
4af66c69	423	l2cap_chan_put(chan);
46d5c908 AE	424	}
	425
	426	static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
	427	{
	428	struct amp_mgr *mgr = chan->data;
	429
	430	if (!mgr)
	431	return;
	432
	433	BT_DBG("chan %p state %s", chan, state_to_string(state));
	434
	435	chan->state = state;
	436
	437	switch (state) {
	438	case BT_CLOSED:
	439	if (mgr)
	440	amp_mgr_put(mgr);
	441	break;
	442	}
	443	}
	444
	445	static struct sk_buff a2mp_chan_alloc_skb_cb(struct l2cap_chan chan,
	446	unsigned long len, int nb)
	447	{
	448	return bt_skb_alloc(len, GFP_KERNEL);
	449	}
	450
466f8004 AE	451	static struct l2cap_ops a2mp_chan_ops = {
466f8004 AE	452	.name = "L2CAP A2MP channel",
6b44d9b8	453	.recv = a2mp_chan_recv_cb,
46d5c908 AE	454	.close = a2mp_chan_close_cb,
	455	.state_change = a2mp_chan_state_change_cb,
	456	.alloc_skb = a2mp_chan_alloc_skb_cb,
	457
	458	/* Not implemented for A2MP */
7e1af8a3 GP	459	.new_connection = l2cap_chan_no_new_connection,
	460	.teardown = l2cap_chan_no_teardown,
	461	.ready = l2cap_chan_no_ready,
466f8004 AE	462	};
	463
	464	static struct l2cap_chan a2mp_chan_open(struct l2cap_conn conn)
	465	{
	466	struct l2cap_chan *chan;
	467	int err;
	468
	469	chan = l2cap_chan_create();
	470	if (!chan)
	471	return NULL;
	472
	473	BT_DBG("chan %p", chan);
	474
416fa752	475	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
466f8004 AE	476	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
	477
	478	chan->ops = &a2mp_chan_ops;
	479
	480	l2cap_chan_set_defaults(chan);
	481	chan->remote_max_tx = chan->max_tx;
	482	chan->remote_tx_win = chan->tx_win;
	483
	484	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
	485	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
	486
	487	skb_queue_head_init(&chan->tx_q);
	488
	489	chan->mode = L2CAP_MODE_ERTM;
	490
	491	err = l2cap_ertm_init(chan);
	492	if (err < 0) {
	493	l2cap_chan_del(chan, 0);
	494	return NULL;
	495	}
	496
	497	chan->conf_state = 0;
	498
	499	l2cap_chan_add(conn, chan);
	500
	501	chan->remote_mps = chan->omtu;
	502	chan->mps = chan->omtu;
	503
	504	chan->state = BT_CONNECTED;
	505
	506	return chan;
	507	}
9740e49d AE	508
	509	/* AMP Manager functions */
	510	void amp_mgr_get(struct amp_mgr *mgr)
	511	{
a0dfe0ab	512	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	513
	514	kref_get(&mgr->kref);
	515	}
	516
	517	static void amp_mgr_destroy(struct kref *kref)
	518	{
	519	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
	520
	521	BT_DBG("mgr %p", mgr);
	522
f97268fc AE	523	mutex_lock(&amp_mgr_list_lock);
	524	list_del(&mgr->list);
	525	mutex_unlock(&amp_mgr_list_lock);
	526
9740e49d AE	527	kfree(mgr);
	528	}
	529
	530	int amp_mgr_put(struct amp_mgr *mgr)
	531	{
a0dfe0ab	532	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	533
	534	return kref_put(&mgr->kref, &amp_mgr_destroy);
	535	}
	536
	537	static struct amp_mgr amp_mgr_create(struct l2cap_conn conn)
	538	{
	539	struct amp_mgr *mgr;
	540	struct l2cap_chan *chan;
	541
	542	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
	543	if (!mgr)
	544	return NULL;
	545
	546	BT_DBG("conn %p mgr %p", conn, mgr);
	547
	548	mgr->l2cap_conn = conn;
	549
	550	chan = a2mp_chan_open(conn);
	551	if (!chan) {
	552	kfree(mgr);
	553	return NULL;
	554	}
	555
	556	mgr->a2mp_chan = chan;
	557	chan->data = mgr;
	558
	559	conn->hcon->amp_mgr = mgr;
	560
	561	kref_init(&mgr->kref);
	562
f97268fc AE	563	mutex_lock(&amp_mgr_list_lock);
	564	list_add(&mgr->list, &amp_mgr_list);
	565	mutex_unlock(&amp_mgr_list_lock);
	566
9740e49d AE	567	return mgr;
9740e49d AE	568	}
97e8e89d AE	569
	570	struct l2cap_chan a2mp_channel_create(struct l2cap_conn conn,
	571	struct sk_buff *skb)
	572	{
	573	struct amp_mgr *mgr;
	574
	575	mgr = amp_mgr_create(conn);
	576	if (!mgr) {
	577	BT_ERR("Could not create AMP manager");
	578	return NULL;
	579	}
	580
	581	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
	582
	583	return mgr->a2mp_chan;
	584	}
f97268fc AE	585
	586	struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
	587	{
	588	struct amp_mgr *mgr;
	589
	590	mutex_lock(&amp_mgr_list_lock);
	591	list_for_each_entry(mgr, &amp_mgr_list, list) {
	592	if (mgr->state == state) {
	593	amp_mgr_get(mgr);
	594	mutex_unlock(&amp_mgr_list_lock);
	595	return mgr;
	596	}
	597	}
	598	mutex_unlock(&amp_mgr_list_lock);
	599
	600	return NULL;
	601	}