a2mp.c revision 52c0d6e56b634b195e377192182391d526cdd5e4
1466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko/*
2466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   Copyright (c) 2010,2011 Code Aurora Forum.  All rights reserved.
3466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   Copyright (c) 2011,2012 Intel Corp.
4466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
5466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   This program is free software; you can redistribute it and/or modify
6466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   it under the terms of the GNU General Public License version 2 and
7466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   only version 2 as published by the Free Software Foundation.
8466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
9466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   This program is distributed in the hope that it will be useful,
10466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   but WITHOUT ANY WARRANTY; without even the implied warranty of
11466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko   GNU General Public License for more details.
13466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko*/
14466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
15466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko#include <net/bluetooth/bluetooth.h>
16466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko#include <net/bluetooth/hci_core.h>
17466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko#include <net/bluetooth/l2cap.h>
189740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko#include <net/bluetooth/a2mp.h>
19903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko#include <net/bluetooth/amp.h>
20466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
21f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko/* Global AMP Manager list */
22f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei EmeltchenkoLIST_HEAD(amp_mgr_list);
23f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei EmeltchenkoDEFINE_MUTEX(amp_mgr_list_lock);
24f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
25f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko/* A2MP build & send command helper functions */
26f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenkostatic struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
27f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko{
28f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	struct a2mp_cmd *cmd;
29f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	int plen;
30f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
31f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	plen = sizeof(*cmd) + len;
32f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	cmd = kzalloc(plen, GFP_KERNEL);
33f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	if (!cmd)
34f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko		return NULL;
35f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
36f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	cmd->code = code;
37f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	cmd->ident = ident;
38f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	cmd->len = cpu_to_le16(len);
39f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
40f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	memcpy(cmd->data, data, len);
41f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
42f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	return cmd;
43f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko}
44f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
458e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenkovoid a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
46f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko{
47f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	struct l2cap_chan *chan = mgr->a2mp_chan;
48f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	struct a2mp_cmd *cmd;
49f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	u16 total_len = len + sizeof(*cmd);
50f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	struct kvec iv;
51f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	struct msghdr msg;
52f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
53f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	cmd = __a2mp_build(code, ident, len, data);
54f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	if (!cmd)
55f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko		return;
56f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
57f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	iv.iov_base = cmd;
58f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	iv.iov_len = total_len;
59f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
60f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	memset(&msg, 0, sizeof(msg));
61f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
62f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	msg.msg_iov = (struct iovec *) &iv;
63f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	msg.msg_iovlen = 1;
64f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
65f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	l2cap_chan_send(chan, &msg, total_len, 0);
66f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
67f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko	kfree(cmd);
68f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko}
69f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko
70aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenkostatic u8 __next_ident(struct amp_mgr *mgr)
71aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko{
72aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	if (++mgr->ident == 0)
73aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		mgr->ident = 1;
74aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
75aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	return mgr->ident;
76aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko}
77aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
788598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
798598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{
808598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	cl->id = 0;
818598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	cl->type = 0;
828598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	cl->status = 1;
838598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko}
848598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
858598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko/* hci_dev_list shall be locked */
868598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl)
878598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{
888598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	int i = 0;
898598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	struct hci_dev *hdev;
908598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
918598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	__a2mp_cl_bredr(cl);
928598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
938598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	list_for_each_entry(hdev, &hci_dev_list, list) {
948598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		/* Iterate through AMP controllers */
958598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		if (hdev->id == HCI_BREDR_ID)
968598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			continue;
978598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
988598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		/* Starting from second entry */
998598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		if (++i >= num_ctrl)
1008598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			return;
1018598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1028598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		cl[i].id = hdev->id;
1038598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		cl[i].type = hdev->amp_type;
1048598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		cl[i].status = hdev->amp_status;
1058598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	}
1068598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko}
1078598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
10821dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko/* Processing A2MP messages */
10921dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenkostatic int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
11021dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko			    struct a2mp_cmd *hdr)
11121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko{
11221dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko	struct a2mp_cmd_rej *rej = (void *) skb->data;
11321dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
11421dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko	if (le16_to_cpu(hdr->len) < sizeof(*rej))
11521dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko		return -EINVAL;
11621dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
11721dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko	BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
11821dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
11921dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko	skb_pull(skb, sizeof(*rej));
12021dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
12121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko	return 0;
12221dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko}
12321dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
1248598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
1258598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			     struct a2mp_cmd *hdr)
1268598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{
1278598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	struct a2mp_discov_req *req = (void *) skb->data;
1288598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	u16 len = le16_to_cpu(hdr->len);
1298598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	struct a2mp_discov_rsp *rsp;
1308598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	u16 ext_feat;
1318598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	u8 num_ctrl;
1328598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1338598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	if (len < sizeof(*req))
1348598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		return -EINVAL;
1358598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1368598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	skb_pull(skb, sizeof(*req));
1378598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1388598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	ext_feat = le16_to_cpu(req->ext_feat);
1398598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1408598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
1418598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1428598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	/* check that packet is not broken for now */
1438598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	while (ext_feat & A2MP_FEAT_EXT) {
1448598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		if (len < sizeof(ext_feat))
1458598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			return -EINVAL;
1468598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1478598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		ext_feat = get_unaligned_le16(skb->data);
1488598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		BT_DBG("efm 0x%4.4x", ext_feat);
1498598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		len -= sizeof(ext_feat);
1508598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		skb_pull(skb, sizeof(ext_feat));
1518598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	}
1528598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1538598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	read_lock(&hci_dev_list_lock);
1548598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1558598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	num_ctrl = __hci_num_ctrl();
1568598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
1578598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	rsp = kmalloc(len, GFP_ATOMIC);
1588598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	if (!rsp) {
1598598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		read_unlock(&hci_dev_list_lock);
1608598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko		return -ENOMEM;
1618598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	}
1628598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1638598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
1648598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	rsp->ext_feat = 0;
1658598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1668598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	__a2mp_add_cl(mgr, rsp->cl, num_ctrl);
1678598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1688598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	read_unlock(&hci_dev_list_lock);
1698598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1708598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
1718598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
1728598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	kfree(rsp);
1738598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko	return 0;
1748598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko}
1758598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
176aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenkostatic int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
177aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			     struct a2mp_cmd *hdr)
178aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko{
179aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	struct a2mp_discov_rsp *rsp = (void *) skb->data;
180aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	u16 len = le16_to_cpu(hdr->len);
181aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	struct a2mp_cl *cl;
182aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	u16 ext_feat;
183aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
184aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	if (len < sizeof(*rsp))
185aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		return -EINVAL;
186aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
187aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	len -= sizeof(*rsp);
188aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	skb_pull(skb, sizeof(*rsp));
189aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
190aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	ext_feat = le16_to_cpu(rsp->ext_feat);
191aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
192aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
193aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
194aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	/* check that packet is not broken for now */
195aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	while (ext_feat & A2MP_FEAT_EXT) {
196aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		if (len < sizeof(ext_feat))
197aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			return -EINVAL;
198aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
199aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		ext_feat = get_unaligned_le16(skb->data);
200aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		BT_DBG("efm 0x%4.4x", ext_feat);
201aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		len -= sizeof(ext_feat);
202aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		skb_pull(skb, sizeof(ext_feat));
203aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	}
204aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
205aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	cl = (void *) skb->data;
206aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	while (len >= sizeof(*cl)) {
207aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
208aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		       cl->status);
209aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
210aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) {
211aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			struct a2mp_info_req req;
212aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
213aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			req.id = cl->id;
214aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
215aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko				  sizeof(req), &req);
216aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		}
217aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
218aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		len -= sizeof(*cl);
219aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		cl = (void *) skb_pull(skb, sizeof(*cl));
220aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	}
221aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
222aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko	return 0;
223aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko}
224aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
225329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenkostatic int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
226329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko			      struct a2mp_cmd *hdr)
227329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko{
228329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko	struct a2mp_cl *cl = (void *) skb->data;
229329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko
230329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko	while (skb->len >= sizeof(*cl)) {
231329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko		BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
232329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko		       cl->status);
233329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko		cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl));
234329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko	}
235329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko
236329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko	/* TODO send A2MP_CHANGE_RSP */
237329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko
238329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko	return 0;
239329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko}
240329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko
24147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenkostatic int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
24247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko			    struct a2mp_cmd *hdr)
24347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko{
24447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	struct a2mp_info_req *req  = (void *) skb->data;
24547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	struct hci_dev *hdev;
24647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
24747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	if (le16_to_cpu(hdr->len) < sizeof(*req))
24847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko		return -EINVAL;
24947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
25047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	BT_DBG("id %d", req->id);
25147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
25247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	hdev = hci_dev_get(req->id);
2538e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	if (!hdev) {
2548e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		struct a2mp_info_rsp rsp;
2558e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
2568e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.id = req->id;
2578e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
2588e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
2598e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
2608e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko			  &rsp);
26147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	}
26247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
2638e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	if (hdev->dev_type != HCI_BREDR) {
2648e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		mgr->state = READ_LOC_AMP_INFO;
2658e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
2668e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	}
26747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
2688e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	hci_dev_put(hdev);
26947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
27047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	skb_pull(skb, sizeof(*req));
27147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko	return 0;
27247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko}
27347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
274a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenkostatic int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
275a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko				struct a2mp_cmd *hdr)
276a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko{
277a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	struct a2mp_amp_assoc_req *req = (void *) skb->data;
278a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	struct hci_dev *hdev;
279903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	struct amp_mgr *tmp;
280a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
281a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	if (le16_to_cpu(hdr->len) < sizeof(*req))
282a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko		return -EINVAL;
283a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
284a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	BT_DBG("id %d", req->id);
285a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
286903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	/* Make sure that other request is not processed */
287903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
288903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
289a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	hdev = hci_dev_get(req->id);
290903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	if (!hdev || hdev->amp_type == HCI_BREDR || tmp) {
291a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko		struct a2mp_amp_assoc_rsp rsp;
292a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko		rsp.id = req->id;
293903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
294903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		if (tmp) {
295903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko			rsp.status = A2MP_STATUS_COLLISION_OCCURED;
296903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko			amp_mgr_put(tmp);
297903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		} else {
298903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko			rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
299903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		}
300a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
301a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko		a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
302a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko			  &rsp);
303903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
304903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		goto done;
305a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	}
306a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
307903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	amp_read_loc_assoc(hdev, mgr);
308a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
309903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenkodone:
310a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	if (hdev)
311a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko		hci_dev_put(hdev);
312a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
313a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	skb_pull(skb, sizeof(*req));
314a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko	return 0;
315a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko}
316a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
317e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkostatic int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
318e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko				   struct a2mp_cmd *hdr)
319e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko{
320e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	struct a2mp_physlink_req *req = (void *) skb->data;
321e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
322e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	struct a2mp_physlink_rsp rsp;
323e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	struct hci_dev *hdev;
324e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
325e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	if (le16_to_cpu(hdr->len) < sizeof(*req))
326e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko		return -EINVAL;
327e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
328e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
329e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
330e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	rsp.local_id = req->remote_id;
331e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	rsp.remote_id = req->local_id;
332e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
333e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	hdev = hci_dev_get(req->remote_id);
334e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	if (!hdev || hdev->amp_type != HCI_AMP) {
335e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
336e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko		goto send_rsp;
337e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	}
338e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
339e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	/* TODO process physlink create */
340e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
341e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	rsp.status = A2MP_STATUS_SUCCESS;
342e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
343e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkosend_rsp:
344e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	if (hdev)
345e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko		hci_dev_put(hdev);
346e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
347e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp),
348e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko		  &rsp);
349e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
350e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	skb_pull(skb, le16_to_cpu(hdr->len));
351e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko	return 0;
352e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko}
353e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
3546113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkostatic int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
3556113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko				 struct a2mp_cmd *hdr)
3566113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko{
3576113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	struct a2mp_physlink_req *req = (void *) skb->data;
3586113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	struct a2mp_physlink_rsp rsp;
3596113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	struct hci_dev *hdev;
3606113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3616113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	if (le16_to_cpu(hdr->len) < sizeof(*req))
3626113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko		return -EINVAL;
3636113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3646113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
3656113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3666113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	rsp.local_id = req->remote_id;
3676113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	rsp.remote_id = req->local_id;
3686113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	rsp.status = A2MP_STATUS_SUCCESS;
3696113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3706113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	hdev = hci_dev_get(req->local_id);
3716113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	if (!hdev) {
3726113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko		rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
3736113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko		goto send_rsp;
3746113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	}
3756113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3766113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	/* TODO Disconnect Phys Link here */
3776113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3786113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	hci_dev_put(hdev);
3796113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3806113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkosend_rsp:
3816113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
3826113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
3836113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	skb_pull(skb, sizeof(*req));
3846113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko	return 0;
3856113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko}
3866113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
387f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenkostatic inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
388f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko			       struct a2mp_cmd *hdr)
389f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko{
3908e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko	BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
391f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko
392f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko	skb_pull(skb, le16_to_cpu(hdr->len));
393f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko	return 0;
394f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko}
395f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko
3966b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko/* Handle A2MP signalling */
3976b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenkostatic int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
3986b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko{
399d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko	struct a2mp_cmd *hdr;
4006b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	struct amp_mgr *mgr = chan->data;
4016b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	int err = 0;
4026b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4036b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	amp_mgr_get(mgr);
4046b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4056b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	while (skb->len >= sizeof(*hdr)) {
406d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko		u16 len;
407d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko
408d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko		hdr = (void *) skb->data;
409d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko		len = le16_to_cpu(hdr->len);
4106b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4118e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko		BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
4126b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4136b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		skb_pull(skb, sizeof(*hdr));
4146b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4156b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		if (len > skb->len || !hdr->ident) {
4166b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			err = -EINVAL;
4176b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			break;
4186b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		}
4196b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4206b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		mgr->ident = hdr->ident;
4216b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4226b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		switch (hdr->code) {
4236b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_COMMAND_REJ:
42421dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko			a2mp_command_rej(mgr, skb, hdr);
42521dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko			break;
42621dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko
4276b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_DISCOVER_REQ:
4288598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			err = a2mp_discover_req(mgr, skb, hdr);
4298598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko			break;
4308598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko
4316b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_CHANGE_NOTIFY:
432329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko			err = a2mp_change_notify(mgr, skb, hdr);
433329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko			break;
434329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko
4356b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_GETINFO_REQ:
43647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko			err = a2mp_getinfo_req(mgr, skb, hdr);
43747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko			break;
43847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko
4396b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_GETAMPASSOC_REQ:
440a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko			err = a2mp_getampassoc_req(mgr, skb, hdr);
441a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko			break;
442a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko
4436b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_CREATEPHYSLINK_REQ:
444e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko			err = a2mp_createphyslink_req(mgr, skb, hdr);
445e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko			break;
446e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko
4476b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_DISCONNPHYSLINK_REQ:
4486113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko			err = a2mp_discphyslink_req(mgr, skb, hdr);
4496113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko			break;
4506113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko
4516b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_DISCOVER_RSP:
452aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			err = a2mp_discover_rsp(mgr, skb, hdr);
453aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko			break;
454aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko
455aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko		case A2MP_CHANGE_RSP:
4566b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_GETINFO_RSP:
4576b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_GETAMPASSOC_RSP:
4586b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_CREATEPHYSLINK_RSP:
4596b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		case A2MP_DISCONNPHYSLINK_RSP:
460f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko			err = a2mp_cmd_rsp(mgr, skb, hdr);
461f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko			break;
462f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko
4636b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		default:
4646b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
4656b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			err = -EINVAL;
4666b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			break;
4676b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		}
4686b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	}
4696b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4706b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	if (err) {
4716b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		struct a2mp_cmd_rej rej;
472d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko
4736b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		rej.reason = __constant_cpu_to_le16(0);
474d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko		hdr = (void *) skb->data;
4756b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4766b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
4776b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4786b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko		a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
4796b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko			  &rej);
4806b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	}
4816b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4826b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	/* Always free skb and return success error code to prevent
4836b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	   from sending L2CAP Disconnect over A2MP channel */
4846b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	kfree_skb(skb);
4856b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4866b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	amp_mgr_put(mgr);
4876b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
4886b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	return 0;
4896b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko}
4906b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko
49146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_close_cb(struct l2cap_chan *chan)
49246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{
4934af66c691f4e5c2db9bb00793669a548e9db1974Jaganath Kanakkassery	l2cap_chan_put(chan);
49446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko}
49546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
49646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state)
49746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{
49846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	struct amp_mgr *mgr = chan->data;
49946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
50046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	if (!mgr)
50146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko		return;
50246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
50346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	BT_DBG("chan %p state %s", chan, state_to_string(state));
50446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
50546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	chan->state = state;
50646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
50746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	switch (state) {
50846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	case BT_CLOSED:
50946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko		if (mgr)
51046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko			amp_mgr_put(mgr);
51146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko		break;
51246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	}
51346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko}
51446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
51546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
51646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko					      unsigned long len, int nb)
51746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{
51846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	return bt_skb_alloc(len, GFP_KERNEL);
51946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko}
52046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
521466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_ops a2mp_chan_ops = {
522466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	.name = "L2CAP A2MP channel",
5236b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko	.recv = a2mp_chan_recv_cb,
52446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	.close = a2mp_chan_close_cb,
52546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	.state_change = a2mp_chan_state_change_cb,
52646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	.alloc_skb = a2mp_chan_alloc_skb_cb,
52746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko
52846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko	/* Not implemented for A2MP */
5297e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan	.new_connection = l2cap_chan_no_new_connection,
5307e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan	.teardown = l2cap_chan_no_teardown,
5317e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan	.ready = l2cap_chan_no_ready,
532466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko};
533466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
534466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn)
535466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko{
536466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	struct l2cap_chan *chan;
537466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	int err;
538466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
539466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan = l2cap_chan_create();
540466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	if (!chan)
541466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko		return NULL;
542466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
543466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	BT_DBG("chan %p", chan);
544466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
545416fa7527d6bf658e5517ea36d2de9270be2c11eAndrei Emeltchenko	chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP;
546466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
547466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
548466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->ops = &a2mp_chan_ops;
549466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
550466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	l2cap_chan_set_defaults(chan);
551466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->remote_max_tx = chan->max_tx;
552466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->remote_tx_win = chan->tx_win;
553466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
554466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
555466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
556466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
557466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	skb_queue_head_init(&chan->tx_q);
558466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
559466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->mode = L2CAP_MODE_ERTM;
560466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
561466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	err = l2cap_ertm_init(chan);
562466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	if (err < 0) {
563466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko		l2cap_chan_del(chan, 0);
564466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko		return NULL;
565466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	}
566466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
567466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->conf_state = 0;
568466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
569466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	l2cap_chan_add(conn, chan);
570466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
571466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->remote_mps = chan->omtu;
572466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->mps = chan->omtu;
573466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
574466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	chan->state = BT_CONNECTED;
575466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko
576466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko	return chan;
577466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko}
5789740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
5799740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko/* AMP Manager functions */
5809740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkovoid amp_mgr_get(struct amp_mgr *mgr)
5819740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{
582a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
5839740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
5849740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	kref_get(&mgr->kref);
5859740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko}
5869740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
5879740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic void amp_mgr_destroy(struct kref *kref)
5889740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{
5899740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
5909740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
5919740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	BT_DBG("mgr %p", mgr);
5929740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
593f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_lock(&amp_mgr_list_lock);
594f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	list_del(&mgr->list);
595f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_unlock(&amp_mgr_list_lock);
596f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
59752c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko	amp_ctrl_list_flush(mgr);
5989740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	kfree(mgr);
5999740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko}
6009740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6019740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkoint amp_mgr_put(struct amp_mgr *mgr)
6029740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{
603a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko	BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount));
6049740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6059740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	return kref_put(&mgr->kref, &amp_mgr_destroy);
6069740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko}
6079740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6089740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn)
6099740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{
6109740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	struct amp_mgr *mgr;
6119740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	struct l2cap_chan *chan;
6129740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6139740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
6149740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	if (!mgr)
6159740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko		return NULL;
6169740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6179740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	BT_DBG("conn %p mgr %p", conn, mgr);
6189740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6199740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	mgr->l2cap_conn = conn;
6209740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6219740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	chan = a2mp_chan_open(conn);
6229740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	if (!chan) {
6239740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko		kfree(mgr);
6249740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko		return NULL;
6259740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	}
6269740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6279740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	mgr->a2mp_chan = chan;
6289740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	chan->data = mgr;
6299740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6309740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	conn->hcon->amp_mgr = mgr;
6319740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
6329740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	kref_init(&mgr->kref);
6339740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko
63452c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko	/* Remote AMP ctrl list initialization */
63552c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko	INIT_LIST_HEAD(&mgr->amp_ctrls);
63652c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko	mutex_init(&mgr->amp_ctrls_lock);
63752c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko
638f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_lock(&amp_mgr_list_lock);
639f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	list_add(&mgr->list, &amp_mgr_list);
640f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_unlock(&amp_mgr_list_lock);
641f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
6429740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko	return mgr;
6439740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko}
64497e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko
64597e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenkostruct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
64697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko				       struct sk_buff *skb)
64797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko{
64897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	struct amp_mgr *mgr;
64997e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko
65097e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	mgr = amp_mgr_create(conn);
65197e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	if (!mgr) {
65297e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko		BT_ERR("Could not create AMP manager");
65397e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko		return NULL;
65497e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	}
65597e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko
65697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
65797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko
65897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko	return mgr->a2mp_chan;
65997e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko}
660f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
661f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenkostruct amp_mgr *amp_mgr_lookup_by_state(u8 state)
662f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko{
663f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	struct amp_mgr *mgr;
664f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
665f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_lock(&amp_mgr_list_lock);
666f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	list_for_each_entry(mgr, &amp_mgr_list, list) {
667f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko		if (mgr->state == state) {
668f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko			amp_mgr_get(mgr);
669f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko			mutex_unlock(&amp_mgr_list_lock);
670f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko			return mgr;
671f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko		}
672f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	}
673f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	mutex_unlock(&amp_mgr_list_lock);
674f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko
675f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko	return NULL;
676f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko}
6778e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
6788e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenkovoid a2mp_send_getinfo_rsp(struct hci_dev *hdev)
6798e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko{
6808e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	struct amp_mgr *mgr;
6818e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	struct a2mp_info_rsp rsp;
6828e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
6838e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
6848e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	if (!mgr)
6858e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		return;
6868e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
6878e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	BT_DBG("%s mgr %p", hdev->name, mgr);
6888e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
6898e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	rsp.id = hdev->id;
6908e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
6918e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
6928e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	if (hdev->amp_type != HCI_BREDR) {
6938e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.status = 0;
6948e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
6958e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
6968e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
6978e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
6988e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko		rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
6998e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	}
7008e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko
7018e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
7028e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko	amp_mgr_put(mgr);
7038e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko}
704903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
705903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenkovoid a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
706903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko{
707903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	struct amp_mgr *mgr;
708903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	struct amp_assoc *loc_assoc = &hdev->loc_assoc;
709903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	struct a2mp_amp_assoc_rsp *rsp;
710903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	size_t len;
711903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
712903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
713903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	if (!mgr)
714903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		return;
715903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
716903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	BT_DBG("%s mgr %p", hdev->name, mgr);
717903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
718903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
719903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	rsp = kzalloc(len, GFP_KERNEL);
720903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	if (!rsp) {
721903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		amp_mgr_put(mgr);
722903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		return;
723903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	}
724903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
725903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	rsp->id = hdev->id;
726903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
727903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	if (status) {
728903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
729903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	} else {
730903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		rsp->status = A2MP_STATUS_SUCCESS;
731903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko		memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
732903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	}
733903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko
734903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
735903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	amp_mgr_put(mgr);
736903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko	kfree(rsp);
737903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko}
738