[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>

/* 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 %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)
{
	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:
			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);

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

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",
	.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 = 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)
{
	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", 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;
}

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