[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>
#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 u8 __next_ident(struct amp_mgr *mgr)
{
	if (++mgr->ident == 0)
		mgr->ident = 1;

	return mgr->ident;
}

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_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
			     struct a2mp_cmd *hdr)
{
	struct a2mp_discov_rsp *rsp = (void *) skb->data;
	u16 len = le16_to_cpu(hdr->len);
	struct a2mp_cl *cl;
	u16 ext_feat;

	if (len < sizeof(*rsp))
		return -EINVAL;

	len -= sizeof(*rsp);
	skb_pull(skb, sizeof(*rsp));

	ext_feat = le16_to_cpu(rsp->ext_feat);

	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->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));
	}

	cl = (void *) skb->data;
	while (len >= sizeof(*cl)) {
		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
		       cl->status);

		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
			struct a2mp_info_req req;

			req.id = cl->id;
			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
				  sizeof(req), &req);
		}

		len -= sizeof(*cl);
		cl = (void *) skb_pull(skb, sizeof(*cl));
	}

	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_DISCOVER_RSP:
			err = a2mp_discover_rsp(mgr, skb, hdr);
			break;

		case A2MP_CHANGE_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
aa09537d AE	70	static u8 __next_ident(struct amp_mgr *mgr)
	71	{
	72	if (++mgr->ident == 0)
	73	mgr->ident = 1;
	74
	75	return mgr->ident;
	76	}
	77
8598d064 AE	78	static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
	79	{
	80	cl->id = 0;
	81	cl->type = 0;
	82	cl->status = 1;
	83	}
	84
	85	/* hci_dev_list shall be locked */
	86	static void __a2mp_add_cl(struct amp_mgr mgr, struct a2mp_cl cl, u8 num_ctrl)
	87	{
	88	int i = 0;
	89	struct hci_dev *hdev;
	90
	91	__a2mp_cl_bredr(cl);
	92
	93	list_for_each_entry(hdev, &hci_dev_list, list) {
	94	/* Iterate through AMP controllers */
	95	if (hdev->id == HCI_BREDR_ID)
	96	continue;
	97
	98	/* Starting from second entry */
	99	if (++i >= num_ctrl)
	100	return;
	101
	102	cl[i].id = hdev->id;
	103	cl[i].type = hdev->amp_type;
	104	cl[i].status = hdev->amp_status;
	105	}
	106	}
	107
21dbd2ce AE	108	/* Processing A2MP messages */
	109	static int a2mp_command_rej(struct amp_mgr mgr, struct sk_buff skb,
	110	struct a2mp_cmd *hdr)
	111	{
	112	struct a2mp_cmd_rej rej = (void ) skb->data;
	113
	114	if (le16_to_cpu(hdr->len) < sizeof(*rej))
	115	return -EINVAL;
	116
	117	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
	118
	119	skb_pull(skb, sizeof(*rej));
	120
	121	return 0;
	122	}
	123
8598d064 AE	124	static int a2mp_discover_req(struct amp_mgr mgr, struct sk_buff skb,
	125	struct a2mp_cmd *hdr)
	126	{
	127	struct a2mp_discov_req req = (void ) skb->data;
	128	u16 len = le16_to_cpu(hdr->len);
	129	struct a2mp_discov_rsp *rsp;
	130	u16 ext_feat;
	131	u8 num_ctrl;
	132
	133	if (len < sizeof(*req))
	134	return -EINVAL;
	135
	136	skb_pull(skb, sizeof(*req));
	137
	138	ext_feat = le16_to_cpu(req->ext_feat);
	139
	140	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
	141
	142	/* check that packet is not broken for now */
	143	while (ext_feat & A2MP_FEAT_EXT) {
	144	if (len < sizeof(ext_feat))
	145	return -EINVAL;
	146
	147	ext_feat = get_unaligned_le16(skb->data);
	148	BT_DBG("efm 0x%4.4x", ext_feat);
	149	len -= sizeof(ext_feat);
	150	skb_pull(skb, sizeof(ext_feat));
	151	}
	152
	153	read_lock(&hci_dev_list_lock);
	154
	155	num_ctrl = __hci_num_ctrl();
	156	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
	157	rsp = kmalloc(len, GFP_ATOMIC);
	158	if (!rsp) {
	159	read_unlock(&hci_dev_list_lock);
	160	return -ENOMEM;
	161	}
	162
	163	rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
	164	rsp->ext_feat = 0;
	165
	166	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
	167
	168	read_unlock(&hci_dev_list_lock);
	169
	170	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
	171
	172	kfree(rsp);
	173	return 0;
	174	}
	175
aa09537d AE	176	static int a2mp_discover_rsp(struct amp_mgr mgr, struct sk_buff skb,
	177	struct a2mp_cmd *hdr)
	178	{
	179	struct a2mp_discov_rsp rsp = (void ) skb->data;
	180	u16 len = le16_to_cpu(hdr->len);
	181	struct a2mp_cl *cl;
	182	u16 ext_feat;
	183
	184	if (len < sizeof(*rsp))
	185	return -EINVAL;
	186
	187	len -= sizeof(*rsp);
	188	skb_pull(skb, sizeof(*rsp));
	189
	190	ext_feat = le16_to_cpu(rsp->ext_feat);
	191
	192	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
	193
	194	/* check that packet is not broken for now */
	195	while (ext_feat & A2MP_FEAT_EXT) {
	196	if (len < sizeof(ext_feat))
	197	return -EINVAL;
	198
	199	ext_feat = get_unaligned_le16(skb->data);
	200	BT_DBG("efm 0x%4.4x", ext_feat);
	201	len -= sizeof(ext_feat);
	202	skb_pull(skb, sizeof(ext_feat));
	203	}
	204
	205	cl = (void *) skb->data;
	206	while (len >= sizeof(*cl)) {
	207	BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
	208	cl->status);
	209
	210	if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
	211	struct a2mp_info_req req;
	212
	213	req.id = cl->id;
	214	a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
	215	sizeof(req), &req);
	216	}
	217
	218	len -= sizeof(*cl);
	219	cl = (void ) skb_pull(skb, sizeof(cl));
	220	}
	221
	222	return 0;
	223	}
	224
329d81af AE	225	static int a2mp_change_notify(struct amp_mgr mgr, struct sk_buff skb,
	226	struct a2mp_cmd *hdr)
	227	{
	228	struct a2mp_cl cl = (void ) skb->data;
	229
	230	while (skb->len >= sizeof(*cl)) {
	231	BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
	232	cl->status);
	233	cl = (struct a2mp_cl ) skb_pull(skb, sizeof(cl));
	234	}
	235
	236	/* TODO send A2MP_CHANGE_RSP */
	237
	238	return 0;
	239	}
	240
47f2d97d AE	241	static int a2mp_getinfo_req(struct amp_mgr mgr, struct sk_buff skb,
	242	struct a2mp_cmd *hdr)
	243	{
	244	struct a2mp_info_req req = (void ) skb->data;
47f2d97d AE	245	struct hci_dev *hdev;
	246
	247	if (le16_to_cpu(hdr->len) < sizeof(*req))
	248	return -EINVAL;
	249
	250	BT_DBG("id %d", req->id);
	251
47f2d97d	252	hdev = hci_dev_get(req->id);
8e2a0d92 AE	253	if (!hdev) {
	254	struct a2mp_info_rsp rsp;
	255
	256	rsp.id = req->id;
	257	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	258
	259	a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
	260	&rsp);
47f2d97d AE	261	}
47f2d97d AE	262
8e2a0d92 AE	263	if (hdev->dev_type != HCI_BREDR) {
	264	mgr->state = READ_LOC_AMP_INFO;
	265	hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
	266	}
47f2d97d	267
8e2a0d92	268	hci_dev_put(hdev);
47f2d97d AE	269
	270	skb_pull(skb, sizeof(*req));
	271	return 0;
	272	}
	273
a28381dc AE	274	static int a2mp_getampassoc_req(struct amp_mgr mgr, struct sk_buff skb,
	275	struct a2mp_cmd *hdr)
	276	{
	277	struct a2mp_amp_assoc_req req = (void ) skb->data;
	278	struct hci_dev *hdev;
903e4541	279	struct amp_mgr *tmp;
a28381dc AE	280
	281	if (le16_to_cpu(hdr->len) < sizeof(*req))
	282	return -EINVAL;
	283
	284	BT_DBG("id %d", req->id);
	285
903e4541 AE	286	/* Make sure that other request is not processed */
	287	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
	288
a28381dc	289	hdev = hci_dev_get(req->id);
903e4541	290	if (!hdev \|\| hdev->amp_type == HCI_BREDR \|\| tmp) {
a28381dc AE	291	struct a2mp_amp_assoc_rsp rsp;
a28381dc AE	292	rsp.id = req->id;
903e4541 AE	293
	294	if (tmp) {
	295	rsp.status = A2MP_STATUS_COLLISION_OCCURED;
	296	amp_mgr_put(tmp);
	297	} else {
	298	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	299	}
a28381dc AE	300
	301	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
	302	&rsp);
903e4541 AE	303
903e4541 AE	304	goto done;
a28381dc AE	305	}
a28381dc AE	306
903e4541	307	amp_read_loc_assoc(hdev, mgr);
a28381dc	308
903e4541	309	done:
a28381dc AE	310	if (hdev)
	311	hci_dev_put(hdev);
	312
	313	skb_pull(skb, sizeof(*req));
	314	return 0;
	315	}
	316
e072f5da AE	317	static int a2mp_createphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	318	struct a2mp_cmd *hdr)
	319	{
	320	struct a2mp_physlink_req req = (void ) skb->data;
	321
	322	struct a2mp_physlink_rsp rsp;
	323	struct hci_dev *hdev;
	324
	325	if (le16_to_cpu(hdr->len) < sizeof(*req))
	326	return -EINVAL;
	327
	328	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
	329
	330	rsp.local_id = req->remote_id;
	331	rsp.remote_id = req->local_id;
	332
	333	hdev = hci_dev_get(req->remote_id);
	334	if (!hdev \|\| hdev->amp_type != HCI_AMP) {
	335	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	336	goto send_rsp;
	337	}
	338
	339	/* TODO process physlink create */
	340
	341	rsp.status = A2MP_STATUS_SUCCESS;
	342
	343	send_rsp:
	344	if (hdev)
	345	hci_dev_put(hdev);
	346
	347	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
	348	&rsp);
	349
	350	skb_pull(skb, le16_to_cpu(hdr->len));
	351	return 0;
	352	}
	353
6113f84f AE	354	static int a2mp_discphyslink_req(struct amp_mgr mgr, struct sk_buff skb,
	355	struct a2mp_cmd *hdr)
	356	{
	357	struct a2mp_physlink_req req = (void ) skb->data;
	358	struct a2mp_physlink_rsp rsp;
	359	struct hci_dev *hdev;
	360
	361	if (le16_to_cpu(hdr->len) < sizeof(*req))
	362	return -EINVAL;
	363
	364	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
	365
	366	rsp.local_id = req->remote_id;
	367	rsp.remote_id = req->local_id;
	368	rsp.status = A2MP_STATUS_SUCCESS;
	369
	370	hdev = hci_dev_get(req->local_id);
	371	if (!hdev) {
	372	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	373	goto send_rsp;
	374	}
	375
	376	/* TODO Disconnect Phys Link here */
	377
	378	hci_dev_put(hdev);
	379
	380	send_rsp:
	381	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
	382
	383	skb_pull(skb, sizeof(*req));
	384	return 0;
	385	}
	386
f6410a84 AE	387	static inline int a2mp_cmd_rsp(struct amp_mgr mgr, struct sk_buff skb,
	388	struct a2mp_cmd *hdr)
	389	{
8e8c7e36	390	BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
f6410a84 AE	391
	392	skb_pull(skb, le16_to_cpu(hdr->len));
	393	return 0;
	394	}
	395
6b44d9b8 AE	396	/* Handle A2MP signalling */
	397	static int a2mp_chan_recv_cb(struct l2cap_chan chan, struct sk_buff skb)
	398	{
d9fc1d54	399	struct a2mp_cmd *hdr;
6b44d9b8 AE	400	struct amp_mgr *mgr = chan->data;
	401	int err = 0;
	402
	403	amp_mgr_get(mgr);
	404
	405	while (skb->len >= sizeof(*hdr)) {
d9fc1d54 AE	406	u16 len;
	407
	408	hdr = (void *) skb->data;
	409	len = le16_to_cpu(hdr->len);
6b44d9b8	410
8e8c7e36	411	BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
6b44d9b8 AE	412
	413	skb_pull(skb, sizeof(*hdr));
	414
	415	if (len > skb->len \|\| !hdr->ident) {
	416	err = -EINVAL;
	417	break;
	418	}
	419
	420	mgr->ident = hdr->ident;
	421
	422	switch (hdr->code) {
	423	case A2MP_COMMAND_REJ:
21dbd2ce AE	424	a2mp_command_rej(mgr, skb, hdr);
	425	break;
	426
6b44d9b8	427	case A2MP_DISCOVER_REQ:
8598d064 AE	428	err = a2mp_discover_req(mgr, skb, hdr);
	429	break;
	430
6b44d9b8	431	case A2MP_CHANGE_NOTIFY:
329d81af AE	432	err = a2mp_change_notify(mgr, skb, hdr);
	433	break;
	434
6b44d9b8	435	case A2MP_GETINFO_REQ:
47f2d97d AE	436	err = a2mp_getinfo_req(mgr, skb, hdr);
	437	break;
	438
6b44d9b8	439	case A2MP_GETAMPASSOC_REQ:
a28381dc AE	440	err = a2mp_getampassoc_req(mgr, skb, hdr);
	441	break;
	442
6b44d9b8	443	case A2MP_CREATEPHYSLINK_REQ:
e072f5da AE	444	err = a2mp_createphyslink_req(mgr, skb, hdr);
	445	break;
	446
6b44d9b8	447	case A2MP_DISCONNPHYSLINK_REQ:
6113f84f AE	448	err = a2mp_discphyslink_req(mgr, skb, hdr);
	449	break;
	450
6b44d9b8	451	case A2MP_DISCOVER_RSP:
aa09537d AE	452	err = a2mp_discover_rsp(mgr, skb, hdr);
	453	break;
	454
	455	case A2MP_CHANGE_RSP:
6b44d9b8 AE	456	case A2MP_GETINFO_RSP:
	457	case A2MP_GETAMPASSOC_RSP:
	458	case A2MP_CREATEPHYSLINK_RSP:
	459	case A2MP_DISCONNPHYSLINK_RSP:
f6410a84 AE	460	err = a2mp_cmd_rsp(mgr, skb, hdr);
	461	break;
	462
6b44d9b8 AE	463	default:
	464	BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
	465	err = -EINVAL;
	466	break;
	467	}
	468	}
	469
	470	if (err) {
	471	struct a2mp_cmd_rej rej;
d9fc1d54	472
6b44d9b8	473	rej.reason = __constant_cpu_to_le16(0);
d9fc1d54	474	hdr = (void *) skb->data;
6b44d9b8 AE	475
	476	BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
	477
	478	a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
	479	&rej);
	480	}
	481
	482	/* Always free skb and return success error code to prevent
	483	from sending L2CAP Disconnect over A2MP channel */
	484	kfree_skb(skb);
	485
	486	amp_mgr_put(mgr);
	487
	488	return 0;
	489	}
	490
46d5c908 AE	491	static void a2mp_chan_close_cb(struct l2cap_chan *chan)
46d5c908 AE	492	{
4af66c69	493	l2cap_chan_put(chan);
46d5c908 AE	494	}
	495
	496	static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
	497	{
	498	struct amp_mgr *mgr = chan->data;
	499
	500	if (!mgr)
	501	return;
	502
	503	BT_DBG("chan %p state %s", chan, state_to_string(state));
	504
	505	chan->state = state;
	506
	507	switch (state) {
	508	case BT_CLOSED:
	509	if (mgr)
	510	amp_mgr_put(mgr);
	511	break;
	512	}
	513	}
	514
	515	static struct sk_buff a2mp_chan_alloc_skb_cb(struct l2cap_chan chan,
	516	unsigned long len, int nb)
	517	{
	518	return bt_skb_alloc(len, GFP_KERNEL);
	519	}
	520
466f8004 AE	521	static struct l2cap_ops a2mp_chan_ops = {
466f8004 AE	522	.name = "L2CAP A2MP channel",
6b44d9b8	523	.recv = a2mp_chan_recv_cb,
46d5c908 AE	524	.close = a2mp_chan_close_cb,
	525	.state_change = a2mp_chan_state_change_cb,
	526	.alloc_skb = a2mp_chan_alloc_skb_cb,
	527
	528	/* Not implemented for A2MP */
7e1af8a3 GP	529	.new_connection = l2cap_chan_no_new_connection,
	530	.teardown = l2cap_chan_no_teardown,
	531	.ready = l2cap_chan_no_ready,
466f8004 AE	532	};
	533
	534	static struct l2cap_chan a2mp_chan_open(struct l2cap_conn conn)
	535	{
	536	struct l2cap_chan *chan;
	537	int err;
	538
	539	chan = l2cap_chan_create();
	540	if (!chan)
	541	return NULL;
	542
	543	BT_DBG("chan %p", chan);
	544
416fa752	545	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
466f8004 AE	546	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
	547
	548	chan->ops = &a2mp_chan_ops;
	549
	550	l2cap_chan_set_defaults(chan);
	551	chan->remote_max_tx = chan->max_tx;
	552	chan->remote_tx_win = chan->tx_win;
	553
	554	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
	555	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
	556
	557	skb_queue_head_init(&chan->tx_q);
	558
	559	chan->mode = L2CAP_MODE_ERTM;
	560
	561	err = l2cap_ertm_init(chan);
	562	if (err < 0) {
	563	l2cap_chan_del(chan, 0);
	564	return NULL;
	565	}
	566
	567	chan->conf_state = 0;
	568
	569	l2cap_chan_add(conn, chan);
	570
	571	chan->remote_mps = chan->omtu;
	572	chan->mps = chan->omtu;
	573
	574	chan->state = BT_CONNECTED;
	575
	576	return chan;
	577	}
9740e49d AE	578
	579	/* AMP Manager functions */
	580	void amp_mgr_get(struct amp_mgr *mgr)
	581	{
a0dfe0ab	582	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	583
	584	kref_get(&mgr->kref);
	585	}
	586
	587	static void amp_mgr_destroy(struct kref *kref)
	588	{
	589	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
	590
	591	BT_DBG("mgr %p", mgr);
	592
f97268fc AE	593	mutex_lock(&amp_mgr_list_lock);
	594	list_del(&mgr->list);
	595	mutex_unlock(&amp_mgr_list_lock);
	596
9740e49d AE	597	kfree(mgr);
	598	}
	599
	600	int amp_mgr_put(struct amp_mgr *mgr)
	601	{
a0dfe0ab	602	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
9740e49d AE	603
	604	return kref_put(&mgr->kref, &amp_mgr_destroy);
	605	}
	606
	607	static struct amp_mgr amp_mgr_create(struct l2cap_conn conn)
	608	{
	609	struct amp_mgr *mgr;
	610	struct l2cap_chan *chan;
	611
	612	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
	613	if (!mgr)
	614	return NULL;
	615
	616	BT_DBG("conn %p mgr %p", conn, mgr);
	617
	618	mgr->l2cap_conn = conn;
	619
	620	chan = a2mp_chan_open(conn);
	621	if (!chan) {
	622	kfree(mgr);
	623	return NULL;
	624	}
	625
	626	mgr->a2mp_chan = chan;
	627	chan->data = mgr;
	628
	629	conn->hcon->amp_mgr = mgr;
	630
	631	kref_init(&mgr->kref);
	632
f97268fc AE	633	mutex_lock(&amp_mgr_list_lock);
	634	list_add(&mgr->list, &amp_mgr_list);
	635	mutex_unlock(&amp_mgr_list_lock);
	636
9740e49d AE	637	return mgr;
9740e49d AE	638	}
97e8e89d AE	639
	640	struct l2cap_chan a2mp_channel_create(struct l2cap_conn conn,
	641	struct sk_buff *skb)
	642	{
	643	struct amp_mgr *mgr;
	644
	645	mgr = amp_mgr_create(conn);
	646	if (!mgr) {
	647	BT_ERR("Could not create AMP manager");
	648	return NULL;
	649	}
	650
	651	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
	652
	653	return mgr->a2mp_chan;
	654	}
f97268fc AE	655
	656	struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
	657	{
	658	struct amp_mgr *mgr;
	659
	660	mutex_lock(&amp_mgr_list_lock);
	661	list_for_each_entry(mgr, &amp_mgr_list, list) {
	662	if (mgr->state == state) {
	663	amp_mgr_get(mgr);
	664	mutex_unlock(&amp_mgr_list_lock);
	665	return mgr;
	666	}
	667	}
	668	mutex_unlock(&amp_mgr_list_lock);
	669
	670	return NULL;
	671	}
8e2a0d92 AE	672
	673	void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
	674	{
	675	struct amp_mgr *mgr;
	676	struct a2mp_info_rsp rsp;
	677
	678	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
	679	if (!mgr)
	680	return;
	681
	682	BT_DBG("%s mgr %p", hdev->name, mgr);
	683
	684	rsp.id = hdev->id;
	685	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
	686
	687	if (hdev->amp_type != HCI_BREDR) {
	688	rsp.status = 0;
	689	rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
	690	rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
	691	rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
	692	rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
	693	rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
	694	}
	695
	696	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
	697	amp_mgr_put(mgr);
	698	}
903e4541 AE	699
	700	void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
	701	{
	702	struct amp_mgr *mgr;
	703	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
	704	struct a2mp_amp_assoc_rsp *rsp;
	705	size_t len;
	706
	707	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
	708	if (!mgr)
	709	return;
	710
	711	BT_DBG("%s mgr %p", hdev->name, mgr);
	712
	713	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
	714	rsp = kzalloc(len, GFP_KERNEL);
	715	if (!rsp) {
	716	amp_mgr_put(mgr);
	717	return;
	718	}
	719
	720	rsp->id = hdev->id;
	721
	722	if (status) {
	723	rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
	724	} else {
	725	rsp->status = A2MP_STATUS_SUCCESS;
	726	memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
	727	}
	728
	729	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
	730	amp_mgr_put(mgr);
	731	kfree(rsp);
	732	}