[GitHub/mt8127/android_kernel_alcatel_ttab.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>
#include <net/bluetooth/amp.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;
}

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 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) {
		struct a2mp_info_rsp rsp;

		rsp.id = req->id;
		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;

		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
			  &rsp);
	}

	if (hdev->dev_type != HCI_BREDR) {
		mgr->state = READ_LOC_AMP_INFO;
		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
	}

	hci_dev_put(hdev);

	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;
	struct amp_mgr *tmp;

	if (le16_to_cpu(hdr->len) < sizeof(*req))
		return -EINVAL;

	BT_DBG("id %d", req->id);

	/* Make sure that other request is not processed */
	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);

	hdev = hci_dev_get(req->id);
	if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
		struct a2mp_amp_assoc_rsp rsp;
		rsp.id = req->id;

		if (tmp) {
			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
			amp_mgr_put(tmp);
		} else {
			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);

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

void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
{
	struct amp_mgr *mgr;
	struct a2mp_info_rsp rsp;

	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
	if (!mgr)
		return;

	BT_DBG("%s mgr %p", hdev->name, mgr);

	rsp.id = hdev->id;
	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;

	if (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);
	}

	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
	amp_mgr_put(mgr);
}

void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
{
	struct amp_mgr *mgr;
	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
	struct a2mp_amp_assoc_rsp *rsp;
	size_t len;

	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
	if (!mgr)
		return;

	BT_DBG("%s mgr %p", hdev->name, mgr);

	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
	rsp = kzalloc(len, GFP_KERNEL);
	if (!rsp) {
		amp_mgr_put(mgr);
		return;
	}

	rsp->id = hdev->id;

	if (status) {
		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
	} else {
		rsp->status = A2MP_STATUS_SUCCESS;
		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
	}

	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
	amp_mgr_put(mgr);
	kfree(rsp);
}
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>
903e4541	19	#include <net/bluetooth/amp.h>
466f8004	20
f97268fc AE	21	/* Global AMP Manager list */
	22	LIST_HEAD(amp_mgr_list);
	23	DEFINE_MUTEX(amp_mgr_list_lock);
	24
f6d3c6e7 AE	25	/* A2MP build & send command helper functions */
	26	static struct a2mp_cmd __a2mp_build(u8 code, u8 ident, u16 len, void data)
	27	{
	28	struct a2mp_cmd *cmd;
	29	int plen;
	30
	31	plen = sizeof(*cmd) + len;
	32	cmd = kzalloc(plen, GFP_KERNEL);
	33	if (!cmd)
	34	return NULL;
	35
	36	cmd->code = code;
	37	cmd->ident = ident;
	38	cmd->len = cpu_to_le16(len);
	39
	40	memcpy(cmd->data, data, len);
	41
	42	return cmd;
	43	}
	44
8e2a0d92	45	void a2mp_send(struct amp_mgr mgr, u8 code, u8 ident, u16 len, void data)
f6d3c6e7 AE	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;
47f2d97d AE	188	struct hci_dev *hdev;
	189
	190	if (le16_to_cpu(hdr->len) < sizeof(*req))
	191	return -EINVAL;
	192
	193	BT_DBG("id %d", req->id);
	194
47f2d97d	195	hdev = hci_dev_get(req->id);
8e2a0d92 AE	196	if (!hdev) {
	197	struct a2mp_info_rsp rsp;
	198
	199	rsp.id = req->id;
	200	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	201
	202	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
	203	&rsp);
47f2d97d AE	204	}
47f2d97d AE	205
8e2a0d92 AE	206	if (hdev->dev_type != HCI_BREDR) {
	207	mgr->state = READ_LOC_AMP_INFO;
	208	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
	209	}
47f2d97d	210
8e2a0d92	211	hci_dev_put(hdev);
47f2d97d AE	212
	213	skb_pull(skb, sizeof(*req));
	214	return 0;
	215	}
	216
a28381dc AE	217	static int a2mp_getampassoc_req(struct amp_mgr mgr, struct sk_buff skb,
	218	struct a2mp_cmd *hdr)
	219	{
	220	struct a2mp_amp_assoc_req req = (void ) skb->data;
	221	struct hci_dev *hdev;
903e4541	222	struct amp_mgr *tmp;
a28381dc AE	223
	224	if (le16_to_cpu(hdr->len) < sizeof(*req))
	225	return -EINVAL;
	226
	227	BT_DBG("id %d", req->id);
	228
903e4541 AE	229	/* Make sure that other request is not processed */
	230	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
	231
a28381dc	232	hdev = hci_dev_get(req->id);
903e4541	233	if (!hdev \|\| hdev->amp_type == HCI_BREDR \|\| tmp) {
a28381dc AE	234	struct a2mp_amp_assoc_rsp rsp;
a28381dc AE	235	rsp.id = req->id;
903e4541 AE	236
	237	if (tmp) {
	238	rsp.status = A2MP_STATUS_COLLISION_OCCURED;
	239	amp_mgr_put(tmp);
	240	} else {
	241	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	242	}
a28381dc AE	243
	244	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
	245	&rsp);
903e4541 AE	246
903e4541 AE	247	goto done;
a28381dc AE	248	}
a28381dc AE	249
903e4541	250	amp_read_loc_assoc(hdev, mgr);
a28381dc	251
903e4541	252	done:
a28381dc AE	253	if (hdev)
	254	hci_dev_put(hdev);
	255
	256	skb_pull(skb, sizeof(*req));
	257	return 0;
	258	}
	259
e072f5da AE	260	static int a2mp_createphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	261	struct a2mp_cmd *hdr)
	262	{
	263	struct a2mp_physlink_req req = (void ) skb->data;
	264
	265	struct a2mp_physlink_rsp rsp;
	266	struct hci_dev *hdev;
	267
	268	if (le16_to_cpu(hdr->len) < sizeof(*req))
	269	return -EINVAL;
	270
	271	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
	272
	273	rsp.local_id = req->remote_id;
	274	rsp.remote_id = req->local_id;
	275
	276	hdev = hci_dev_get(req->remote_id);
	277	if (!hdev \|\| hdev->amp_type != HCI_AMP) {
	278	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	279	goto send_rsp;
	280	}
	281
	282	/* TODO process physlink create */
	283
	284	rsp.status = A2MP_STATUS_SUCCESS;
	285
	286	send_rsp:
	287	if (hdev)
	288	hci_dev_put(hdev);
	289
	290	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
	291	&rsp);
	292
	293	skb_pull(skb, le16_to_cpu(hdr->len));
	294	return 0;
	295	}
	296
6113f84f AE	297	static int a2mp_discphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	298	struct a2mp_cmd *hdr)
	299	{
	300	struct a2mp_physlink_req req = (void ) skb->data;
	301	struct a2mp_physlink_rsp rsp;
	302	struct hci_dev *hdev;
	303
	304	if (le16_to_cpu(hdr->len) < sizeof(*req))
	305	return -EINVAL;
	306
	307	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
	308
	309	rsp.local_id = req->remote_id;
	310	rsp.remote_id = req->local_id;
	311	rsp.status = A2MP_STATUS_SUCCESS;
	312
	313	hdev = hci_dev_get(req->local_id);
	314	if (!hdev) {
	315	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	316	goto send_rsp;
	317	}
	318
	319	/* TODO Disconnect Phys Link here */
	320
	321	hci_dev_put(hdev);
	322
	323	send_rsp:
	324	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
	325
	326	skb_pull(skb, sizeof(*req));
	327	return 0;
	328	}
	329
f6410a84 AE	330	static inline int a2mp_cmd_rsp(struct amp_mgr mgr, struct sk_buff skb,
	331	struct a2mp_cmd *hdr)
	332	{
8e8c7e36	333	BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
f6410a84 AE	334
	335	skb_pull(skb, le16_to_cpu(hdr->len));
	336	return 0;
	337	}
	338
6b44d9b8 AE	339	/* Handle A2MP signalling */
	340	static int a2mp_chan_recv_cb(struct l2cap_chan chan, struct sk_buff skb)
	341	{
d9fc1d54	342	struct a2mp_cmd *hdr;
6b44d9b8 AE	343	struct amp_mgr *mgr = chan->data;
	344	int err = 0;
	345
	346	amp_mgr_get(mgr);
	347
	348	while (skb->len >= sizeof(*hdr)) {
d9fc1d54 AE	349	u16 len;
	350
	351	hdr = (void *) skb->data;
	352	len = le16_to_cpu(hdr->len);
6b44d9b8	353
8e8c7e36	354	BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
6b44d9b8 AE	355
	356	skb_pull(skb, sizeof(*hdr));
	357
	358	if (len > skb->len \|\| !hdr->ident) {
	359	err = -EINVAL;
	360	break;
	361	}
	362
	363	mgr->ident = hdr->ident;
	364
	365	switch (hdr->code) {
	366	case A2MP_COMMAND_REJ:
21dbd2ce AE	367	a2mp_command_rej(mgr, skb, hdr);
	368	break;
	369
6b44d9b8	370	case A2MP_DISCOVER_REQ:
8598d064 AE	371	err = a2mp_discover_req(mgr, skb, hdr);
	372	break;
	373
6b44d9b8	374	case A2MP_CHANGE_NOTIFY:
329d81af AE	375	err = a2mp_change_notify(mgr, skb, hdr);
	376	break;
	377
6b44d9b8	378	case A2MP_GETINFO_REQ:
47f2d97d AE	379	err = a2mp_getinfo_req(mgr, skb, hdr);
	380	break;
	381
6b44d9b8	382	case A2MP_GETAMPASSOC_REQ:
a28381dc AE	383	err = a2mp_getampassoc_req(mgr, skb, hdr);
	384	break;
	385
6b44d9b8	386	case A2MP_CREATEPHYSLINK_REQ:
e072f5da AE	387	err = a2mp_createphyslink_req(mgr, skb, hdr);
	388	break;
	389
6b44d9b8	390	case A2MP_DISCONNPHYSLINK_REQ:
6113f84f AE	391	err = a2mp_discphyslink_req(mgr, skb, hdr);
	392	break;
	393
6b44d9b8 AE	394	case A2MP_CHANGE_RSP:
	395	case A2MP_DISCOVER_RSP:
	396	case A2MP_GETINFO_RSP:
	397	case A2MP_GETAMPASSOC_RSP:
	398	case A2MP_CREATEPHYSLINK_RSP:
	399	case A2MP_DISCONNPHYSLINK_RSP:
f6410a84 AE	400	err = a2mp_cmd_rsp(mgr, skb, hdr);
	401	break;
	402
6b44d9b8 AE	403	default:
	404	BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
	405	err = -EINVAL;
	406	break;
	407	}
	408	}
	409
	410	if (err) {
	411	struct a2mp_cmd_rej rej;
d9fc1d54	412
6b44d9b8	413	rej.reason = __constant_cpu_to_le16(0);
d9fc1d54	414	hdr = (void *) skb->data;
6b44d9b8 AE	415
	416	BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
	417
	418	a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
	419	&rej);
	420	}
	421
	422	/* Always free skb and return success error code to prevent
	423	from sending L2CAP Disconnect over A2MP channel */
	424	kfree_skb(skb);
	425
	426	amp_mgr_put(mgr);
	427
	428	return 0;
	429	}
	430
46d5c908 AE	431	static void a2mp_chan_close_cb(struct l2cap_chan *chan)
46d5c908 AE	432	{
4af66c69	433	l2cap_chan_put(chan);
46d5c908 AE	434	}
	435
	436	static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
	437	{
	438	struct amp_mgr *mgr = chan->data;
	439
	440	if (!mgr)
	441	return;
	442
	443	BT_DBG("chan %p state %s", chan, state_to_string(state));
	444
	445	chan->state = state;
	446
	447	switch (state) {
	448	case BT_CLOSED:
	449	if (mgr)
	450	amp_mgr_put(mgr);
	451	break;
	452	}
	453	}
	454
	455	static struct sk_buff a2mp_chan_alloc_skb_cb(struct l2cap_chan chan,
	456	unsigned long len, int nb)
	457	{
	458	return bt_skb_alloc(len, GFP_KERNEL);
	459	}
	460
466f8004 AE	461	static struct l2cap_ops a2mp_chan_ops = {
466f8004 AE	462	.name = "L2CAP A2MP channel",
6b44d9b8	463	.recv = a2mp_chan_recv_cb,
46d5c908 AE	464	.close = a2mp_chan_close_cb,
	465	.state_change = a2mp_chan_state_change_cb,
	466	.alloc_skb = a2mp_chan_alloc_skb_cb,
	467
	468	/* Not implemented for A2MP */
7e1af8a3 GP	469	.new_connection = l2cap_chan_no_new_connection,
	470	.teardown = l2cap_chan_no_teardown,
	471	.ready = l2cap_chan_no_ready,
466f8004 AE	472	};
	473
	474	static struct l2cap_chan a2mp_chan_open(struct l2cap_conn conn)
	475	{
	476	struct l2cap_chan *chan;
	477	int err;
	478
	479	chan = l2cap_chan_create();
	480	if (!chan)
	481	return NULL;
	482
	483	BT_DBG("chan %p", chan);
	484
416fa752	485	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
466f8004 AE	486	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
	487
	488	chan->ops = &a2mp_chan_ops;
	489
	490	l2cap_chan_set_defaults(chan);
	491	chan->remote_max_tx = chan->max_tx;
	492	chan->remote_tx_win = chan->tx_win;
	493
	494	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
	495	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
	496
	497	skb_queue_head_init(&chan->tx_q);
	498
	499	chan->mode = L2CAP_MODE_ERTM;
	500
	501	err = l2cap_ertm_init(chan);
	502	if (err < 0) {
	503	l2cap_chan_del(chan, 0);
	504	return NULL;
	505	}
	506
	507	chan->conf_state = 0;
	508
	509	l2cap_chan_add(conn, chan);
	510
	511	chan->remote_mps = chan->omtu;
	512	chan->mps = chan->omtu;
	513
	514	chan->state = BT_CONNECTED;
	515
	516	return chan;
	517	}
9740e49d AE	518
	519	/* AMP Manager functions */
	520	void amp_mgr_get(struct amp_mgr *mgr)
	521	{
a0dfe0ab	522	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	523
	524	kref_get(&mgr->kref);
	525	}
	526
	527	static void amp_mgr_destroy(struct kref *kref)
	528	{
	529	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
	530
	531	BT_DBG("mgr %p", mgr);
	532
f97268fc AE	533	mutex_lock(&amp_mgr_list_lock);
	534	list_del(&mgr->list);
	535	mutex_unlock(&amp_mgr_list_lock);
	536
9740e49d AE	537	kfree(mgr);
	538	}
	539
	540	int amp_mgr_put(struct amp_mgr *mgr)
	541	{
a0dfe0ab	542	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	543
	544	return kref_put(&mgr->kref, &amp_mgr_destroy);
	545	}
	546
	547	static struct amp_mgr amp_mgr_create(struct l2cap_conn conn)
	548	{
	549	struct amp_mgr *mgr;
	550	struct l2cap_chan *chan;
	551
	552	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
	553	if (!mgr)
	554	return NULL;
	555
	556	BT_DBG("conn %p mgr %p", conn, mgr);
	557
	558	mgr->l2cap_conn = conn;
	559
	560	chan = a2mp_chan_open(conn);
	561	if (!chan) {
	562	kfree(mgr);
	563	return NULL;
	564	}
	565
	566	mgr->a2mp_chan = chan;
	567	chan->data = mgr;
	568
	569	conn->hcon->amp_mgr = mgr;
	570
	571	kref_init(&mgr->kref);
	572
f97268fc AE	573	mutex_lock(&amp_mgr_list_lock);
	574	list_add(&mgr->list, &amp_mgr_list);
	575	mutex_unlock(&amp_mgr_list_lock);
	576
9740e49d AE	577	return mgr;
9740e49d AE	578	}
97e8e89d AE	579
	580	struct l2cap_chan a2mp_channel_create(struct l2cap_conn conn,
	581	struct sk_buff *skb)
	582	{
	583	struct amp_mgr *mgr;
	584
	585	mgr = amp_mgr_create(conn);
	586	if (!mgr) {
	587	BT_ERR("Could not create AMP manager");
	588	return NULL;
	589	}
	590
	591	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
	592
	593	return mgr->a2mp_chan;
	594	}
f97268fc AE	595
	596	struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
	597	{
	598	struct amp_mgr *mgr;
	599
	600	mutex_lock(&amp_mgr_list_lock);
	601	list_for_each_entry(mgr, &amp_mgr_list, list) {
	602	if (mgr->state == state) {
	603	amp_mgr_get(mgr);
	604	mutex_unlock(&amp_mgr_list_lock);
	605	return mgr;
	606	}
	607	}
	608	mutex_unlock(&amp_mgr_list_lock);
	609
	610	return NULL;
	611	}
8e2a0d92 AE	612
	613	void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
	614	{
	615	struct amp_mgr *mgr;
	616	struct a2mp_info_rsp rsp;
	617
	618	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
	619	if (!mgr)
	620	return;
	621
	622	BT_DBG("%s mgr %p", hdev->name, mgr);
	623
	624	rsp.id = hdev->id;
	625	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	626
	627	if (hdev->amp_type != HCI_BREDR) {
	628	rsp.status = 0;
	629	rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
	630	rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
	631	rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
	632	rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
	633	rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
	634	}
	635
	636	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
	637	amp_mgr_put(mgr);
	638	}
903e4541 AE	639
	640	void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
	641	{
	642	struct amp_mgr *mgr;
	643	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
	644	struct a2mp_amp_assoc_rsp *rsp;
	645	size_t len;
	646
	647	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
	648	if (!mgr)
	649	return;
	650
	651	BT_DBG("%s mgr %p", hdev->name, mgr);
	652
	653	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
	654	rsp = kzalloc(len, GFP_KERNEL);
	655	if (!rsp) {
	656	amp_mgr_put(mgr);
	657	return;
	658	}
	659
	660	rsp->id = hdev->id;
	661
	662	if (status) {
	663	rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
	664	} else {
	665	rsp->status = A2MP_STATUS_SUCCESS;
	666	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
	667	}
	668
	669	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
	670	amp_mgr_put(mgr);
	671	kfree(rsp);
	672	}