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 709495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenkou8 __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; 1832766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko bool found = false; 184aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 185aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko if (len < sizeof(*rsp)) 186aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko return -EINVAL; 187aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 188aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko len -= sizeof(*rsp); 189aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko skb_pull(skb, sizeof(*rsp)); 190aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 191aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko ext_feat = le16_to_cpu(rsp->ext_feat); 192aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 193aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat); 194aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 195aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko /* check that packet is not broken for now */ 196aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko while (ext_feat & A2MP_FEAT_EXT) { 197aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko if (len < sizeof(ext_feat)) 198aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko return -EINVAL; 199aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 200aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko ext_feat = get_unaligned_le16(skb->data); 201aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko BT_DBG("efm 0x%4.4x", ext_feat); 202aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko len -= sizeof(ext_feat); 203aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko skb_pull(skb, sizeof(ext_feat)); 204aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko } 205aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 206aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko cl = (void *) skb->data; 207aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko while (len >= sizeof(*cl)) { 208aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type, 209aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko cl->status); 210aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 211aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko if (cl->id != HCI_BREDR_ID && cl->type == HCI_AMP) { 212aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko struct a2mp_info_req req; 213aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 2142766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko found = true; 215aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko req.id = cl->id; 216aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr), 217aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko sizeof(req), &req); 218aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko } 219aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 220aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko len -= sizeof(*cl); 221aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko cl = (void *) skb_pull(skb, sizeof(*cl)); 222aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko } 223aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 2242766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko /* Fall back to L2CAP init sequence */ 2252766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko if (!found) { 2262766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko struct l2cap_conn *conn = mgr->l2cap_conn; 2272766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko struct l2cap_chan *chan; 2282766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2292766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko mutex_lock(&conn->chan_lock); 2302766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2312766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko list_for_each_entry(chan, &conn->chan_l, list) { 2322766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2332766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko BT_DBG("chan %p state %s", chan, 2342766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko state_to_string(chan->state)); 2352766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2362766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko if (chan->chan_type == L2CAP_CHAN_CONN_FIX_A2MP) 2372766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko continue; 2382766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2392766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko l2cap_chan_lock(chan); 2402766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2412766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko if (chan->state == BT_CONNECT) 2422766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko l2cap_send_conn_req(chan); 2432766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2442766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko l2cap_chan_unlock(chan); 2452766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko } 2462766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 2472766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko mutex_unlock(&conn->chan_lock); 2482766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko } 2492766be48a7181d7f2a84831ca7e7be248fb6fdb5Andrei Emeltchenko 250aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko return 0; 251aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko} 252aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 253329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenkostatic int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, 254329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko struct a2mp_cmd *hdr) 255329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko{ 256329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko struct a2mp_cl *cl = (void *) skb->data; 257329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 258329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko while (skb->len >= sizeof(*cl)) { 259329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, 260329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko cl->status); 261329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); 262329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko } 263329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 264329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko /* TODO send A2MP_CHANGE_RSP */ 265329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 266329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko return 0; 267329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko} 268329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 26947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenkostatic int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, 27047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct a2mp_cmd *hdr) 27147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko{ 27247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct a2mp_info_req *req = (void *) skb->data; 27347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct hci_dev *hdev; 27447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 27547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 27647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko return -EINVAL; 27747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 27847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko BT_DBG("id %d", req->id); 27947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 28047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko hdev = hci_dev_get(req->id); 281bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko if (!hdev || hdev->dev_type != HCI_AMP) { 2828e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko struct a2mp_info_rsp rsp; 2838e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 2848e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.id = req->id; 2858e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 2868e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 2878e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), 2888e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko &rsp); 28947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 290bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko goto done; 2918e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko } 29247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 293cb6801c640c759fe02c812728c2661bd8ba5a302Andrei Emeltchenko set_bit(READ_LOC_AMP_INFO, &mgr->state); 294bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); 295bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko 296bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenkodone: 297bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko if (hdev) 298bc8dce4f7b4908bac69acac34d19b4234ba19ceeAndrei Emeltchenko hci_dev_put(hdev); 29947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 30047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko skb_pull(skb, sizeof(*req)); 30147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko return 0; 30247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko} 30347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 3040d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenkostatic int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 3050d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko struct a2mp_cmd *hdr) 3060d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko{ 3070d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data; 3080d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko struct a2mp_amp_assoc_req req; 3090d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko struct amp_ctrl *ctrl; 3100d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 3110d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*rsp)) 3120d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko return -EINVAL; 3130d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 3140d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status); 3150d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 3160d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko if (rsp->status) 3170d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko return -EINVAL; 3180d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 319fa4ebc66c432d0e0ec947cb754d4144c4a681f28Andrei Emeltchenko ctrl = amp_ctrl_add(mgr, rsp->id); 3200d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko if (!ctrl) 3210d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko return -ENOMEM; 3220d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 3230d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko req.id = rsp->id; 3240d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req), 3250d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko &req); 3260d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 3270d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko skb_pull(skb, sizeof(*rsp)); 3280d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko return 0; 3290d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko} 3300d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 331a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenkostatic int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, 332a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_cmd *hdr) 333a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko{ 334a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_amp_assoc_req *req = (void *) skb->data; 335a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct hci_dev *hdev; 336903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko struct amp_mgr *tmp; 337a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 338a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 339a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko return -EINVAL; 340a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 341a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko BT_DBG("id %d", req->id); 342a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 343903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko /* Make sure that other request is not processed */ 344903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); 345903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 346a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko hdev = hci_dev_get(req->id); 347903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko if (!hdev || hdev->amp_type == HCI_BREDR || tmp) { 348a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_amp_assoc_rsp rsp; 349a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko rsp.id = req->id; 350903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 351903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko if (tmp) { 352903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp.status = A2MP_STATUS_COLLISION_OCCURED; 353903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko amp_mgr_put(tmp); 354903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko } else { 355903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 356903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko } 357a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 358a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), 359a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko &rsp); 360903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 361903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko goto done; 362a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko } 363a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 364903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko amp_read_loc_assoc(hdev, mgr); 365a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 366903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenkodone: 367a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko if (hdev) 368a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko hci_dev_put(hdev); 369a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 370a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko skb_pull(skb, sizeof(*req)); 371a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko return 0; 372a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko} 373a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 3749a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenkostatic int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 3759a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko struct a2mp_cmd *hdr) 3769a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko{ 3779a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data; 3789a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko u16 len = le16_to_cpu(hdr->len); 3799a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko struct hci_dev *hdev; 3809a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko struct amp_ctrl *ctrl; 3819a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko struct hci_conn *hcon; 38213465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko size_t assoc_len; 3839a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 3849a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (len < sizeof(*rsp)) 3859a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko return -EINVAL; 3869a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 38713465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko assoc_len = len - sizeof(*rsp); 38813465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko 38913465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status, 39013465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko assoc_len); 3919a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 3929a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (rsp->status) 3939a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko return -EINVAL; 3949a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 3959a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko /* Save remote ASSOC data */ 3969a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko ctrl = amp_ctrl_lookup(mgr, rsp->id); 3979a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (ctrl) { 39813465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko u8 *assoc; 3999a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 4005ae327f0efc12d35ea8c98007310c35c143c1e21Alexandru Gheorghiu assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL); 4019a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (!assoc) { 4029a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko amp_ctrl_put(ctrl); 4039a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko return -ENOMEM; 4049a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko } 4059a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 4069a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko ctrl->assoc = assoc; 4079a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko ctrl->assoc_len = assoc_len; 4089a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko ctrl->assoc_rem_len = assoc_len; 4099a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko ctrl->assoc_len_so_far = 0; 4109a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 4119a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko amp_ctrl_put(ctrl); 4129a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko } 4139a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 4149a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko /* Create Phys Link */ 4159a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko hdev = hci_dev_get(rsp->id); 4169a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (!hdev) 4179a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko return -EINVAL; 4189a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 419a0c234fe8972aa6a5afe2db6c27a3f5d5fbd88e7Andrei Emeltchenko hcon = phylink_add(hdev, mgr, rsp->id, true); 4209a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko if (!hcon) 4219a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko goto done; 4229a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 4239a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id); 4249a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 425fffadc08ebf1f4c61bb8f9be0f1d8c3c053d815fAndrei Emeltchenko mgr->bredr_chan->remote_amp_id = rsp->id; 4269495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 427a02226d6ff5098e6b97590cc55aabe7faf0860edAndrei Emeltchenko amp_create_phylink(hdev, mgr, hcon); 428a02226d6ff5098e6b97590cc55aabe7faf0860edAndrei Emeltchenko 4299a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenkodone: 4309a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko hci_dev_put(hdev); 4319a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko skb_pull(skb, len); 4329a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko return 0; 4339a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko} 4349a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 435e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkostatic int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 436e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_cmd *hdr) 437e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko{ 438e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_physlink_req *req = (void *) skb->data; 439e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 440e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_physlink_rsp rsp; 441e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct hci_dev *hdev; 442cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko struct hci_conn *hcon; 4430b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko struct amp_ctrl *ctrl; 444e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 445e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 446e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko return -EINVAL; 447e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 448e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); 449e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 450e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.local_id = req->remote_id; 451e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.remote_id = req->local_id; 452e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 453e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko hdev = hci_dev_get(req->remote_id); 454e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (!hdev || hdev->amp_type != HCI_AMP) { 455e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 456e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko goto send_rsp; 457e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko } 458e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 4590b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko ctrl = amp_ctrl_lookup(mgr, rsp.remote_id); 4600b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko if (!ctrl) { 461fa4ebc66c432d0e0ec947cb754d4144c4a681f28Andrei Emeltchenko ctrl = amp_ctrl_add(mgr, rsp.remote_id); 4620b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko if (ctrl) { 4630b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko amp_ctrl_get(ctrl); 4640b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko } else { 4650b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 4660b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko goto send_rsp; 4670b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko } 4680b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko } 4690b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko 4700b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko if (ctrl) { 47113465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req); 47213465c0aeb9c56a4e4799f25aeff41d05f8a3fabAndrei Emeltchenko u8 *assoc; 4730b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko 4745ae327f0efc12d35ea8c98007310c35c143c1e21Alexandru Gheorghiu assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL); 4750b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko if (!assoc) { 4760b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko amp_ctrl_put(ctrl); 4770b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko return -ENOMEM; 4780b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko } 4790b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko 4800b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko ctrl->assoc = assoc; 4810b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko ctrl->assoc_len = assoc_len; 4820b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko ctrl->assoc_rem_len = assoc_len; 4830b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko ctrl->assoc_len_so_far = 0; 4840b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko 4850b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko amp_ctrl_put(ctrl); 4860b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko } 4870b26ab9dce74f8ac77d7eef0d683ab1d527e45b1Andrei Emeltchenko 488a0c234fe8972aa6a5afe2db6c27a3f5d5fbd88e7Andrei Emeltchenko hcon = phylink_add(hdev, mgr, req->local_id, false); 489cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko if (hcon) { 490dffa387110025801862d7ad09f4e850d06ff55a9Andrei Emeltchenko amp_accept_phylink(hdev, mgr, hcon); 491cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko rsp.status = A2MP_STATUS_SUCCESS; 492cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko } else { 493cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 494cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko } 495e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 496e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkosend_rsp: 497e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (hdev) 498e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko hci_dev_put(hdev); 499e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 5008e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko /* Reply error now and success after HCI Write Remote AMP Assoc 5018e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko command complete with success status 5028e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko */ 5038e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko if (rsp.status != A2MP_STATUS_SUCCESS) { 5048e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, 5058e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko sizeof(rsp), &rsp); 5068e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko } else { 507cb6801c640c759fe02c812728c2661bd8ba5a302Andrei Emeltchenko set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state); 5088e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko mgr->ident = hdr->ident; 5098e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko } 510e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 511e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko skb_pull(skb, le16_to_cpu(hdr->len)); 512e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko return 0; 513e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko} 514e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 5156113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkostatic int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 5166113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_cmd *hdr) 5176113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko{ 5186113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_physlink_req *req = (void *) skb->data; 5196113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_physlink_rsp rsp; 5206113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct hci_dev *hdev; 521cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko struct hci_conn *hcon; 5226113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 5236113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 5246113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko return -EINVAL; 5256113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 5266113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); 5276113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 5286113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.local_id = req->remote_id; 5296113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.remote_id = req->local_id; 5306113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.status = A2MP_STATUS_SUCCESS; 5316113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 532cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko hdev = hci_dev_get(req->remote_id); 5336113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko if (!hdev) { 5346113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 5356113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko goto send_rsp; 5366113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko } 5376113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 538cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, mgr->l2cap_conn->dst); 539cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko if (!hcon) { 540cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko BT_ERR("No phys link exist"); 541cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS; 542cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko goto clean; 543cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko } 544cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenko 5456113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko /* TODO Disconnect Phys Link here */ 5466113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 547cb8488c0b60a947c0ef4a1a94573a7fefd3f20b4Andrei Emeltchenkoclean: 5486113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko hci_dev_put(hdev); 5496113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 5506113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkosend_rsp: 5516113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); 5526113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 5536113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko skb_pull(skb, sizeof(*req)); 5546113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko return 0; 5556113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko} 5566113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 557f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenkostatic inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 558f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko struct a2mp_cmd *hdr) 559f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko{ 5608e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); 561f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 562f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko skb_pull(skb, le16_to_cpu(hdr->len)); 563f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko return 0; 564f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko} 565f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 5666b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko/* Handle A2MP signalling */ 5676b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenkostatic int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) 5686b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko{ 569d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko struct a2mp_cmd *hdr; 5706b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 5716b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko int err = 0; 5726b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5736b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_get(mgr); 5746b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5756b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko while (skb->len >= sizeof(*hdr)) { 576d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko u16 len; 577d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko 578d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko hdr = (void *) skb->data; 579d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko len = le16_to_cpu(hdr->len); 5806b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5818e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); 5826b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5836b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko skb_pull(skb, sizeof(*hdr)); 5846b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5856b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (len > skb->len || !hdr->ident) { 5866b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 5876b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 5886b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 5896b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5906b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko mgr->ident = hdr->ident; 5916b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 5926b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko switch (hdr->code) { 5936b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_COMMAND_REJ: 59421dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko a2mp_command_rej(mgr, skb, hdr); 59521dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko break; 59621dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 5976b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_REQ: 5988598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko err = a2mp_discover_req(mgr, skb, hdr); 5998598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko break; 6008598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 6016b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CHANGE_NOTIFY: 602329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko err = a2mp_change_notify(mgr, skb, hdr); 603329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko break; 604329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 6056b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_REQ: 60647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko err = a2mp_getinfo_req(mgr, skb, hdr); 60747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko break; 60847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 6096b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_REQ: 610a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko err = a2mp_getampassoc_req(mgr, skb, hdr); 611a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko break; 612a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 6136b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_REQ: 614e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko err = a2mp_createphyslink_req(mgr, skb, hdr); 615e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko break; 616e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 6176b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_REQ: 6186113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko err = a2mp_discphyslink_req(mgr, skb, hdr); 6196113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko break; 6206113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 6216b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_RSP: 622aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko err = a2mp_discover_rsp(mgr, skb, hdr); 623aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko break; 624aa09537d80bf7e6282103618eb496f03e76f2953Andrei Emeltchenko 6256b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_RSP: 6260d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko err = a2mp_getinfo_rsp(mgr, skb, hdr); 6270d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko break; 6280d868de9d8760c76f6d4c6c777935c05ef272caaAndrei Emeltchenko 6296b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_RSP: 6309a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko err = a2mp_getampassoc_rsp(mgr, skb, hdr); 6319a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko break; 6329a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko 6339a5e94dbb4aa306742a47cbbcb0a44d4fc77a9e4Andrei Emeltchenko case A2MP_CHANGE_RSP: 6346b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_RSP: 6356b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_RSP: 636f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko err = a2mp_cmd_rsp(mgr, skb, hdr); 637f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko break; 638f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 6396b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko default: 6406b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); 6416b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 6426b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 6436b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 6446b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 6456b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6466b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (err) { 6476b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd_rej rej; 648d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko 6496b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko rej.reason = __constant_cpu_to_le16(0); 650d9fc1d54f6f8123909cdee4fa98ab1ebf6c8651cAndrei Emeltchenko hdr = (void *) skb->data; 6516b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6526b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); 6536b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6546b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), 6556b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko &rej); 6566b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 6576b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6586b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko /* Always free skb and return success error code to prevent 6596b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko from sending L2CAP Disconnect over A2MP channel */ 6606b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko kfree_skb(skb); 6616b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6626b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_put(mgr); 6636b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 6646b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko return 0; 6656b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko} 6666b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 66746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_close_cb(struct l2cap_chan *chan) 66846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 6694af66c691f4e5c2db9bb00793669a548e9db1974Jaganath Kanakkassery l2cap_chan_put(chan); 67046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 67146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 67246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state) 67346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 67446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 67546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 67646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (!mgr) 67746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return; 67846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 67946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_DBG("chan %p state %s", chan, state_to_string(state)); 68046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 68146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko chan->state = state; 68246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 68346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko switch (state) { 68446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko case BT_CLOSED: 68546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (mgr) 68646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko amp_mgr_put(mgr); 68746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko break; 68846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko } 68946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 69046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 69146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, 69246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko unsigned long len, int nb) 69346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 69446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return bt_skb_alloc(len, GFP_KERNEL); 69546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 69646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 697466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_ops a2mp_chan_ops = { 698466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko .name = "L2CAP A2MP channel", 6996b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko .recv = a2mp_chan_recv_cb, 70046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .close = a2mp_chan_close_cb, 70146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .state_change = a2mp_chan_state_change_cb, 70246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .alloc_skb = a2mp_chan_alloc_skb_cb, 70346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 70446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko /* Not implemented for A2MP */ 7057e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .new_connection = l2cap_chan_no_new_connection, 7067e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .teardown = l2cap_chan_no_teardown, 7077e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .ready = l2cap_chan_no_ready, 7082dc4e5105f012bda7eef2f459ed3d5299ded9672Gustavo Padovan .defer = l2cap_chan_no_defer, 709466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko}; 710466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 71193c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenkostatic struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked) 712466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko{ 713466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko struct l2cap_chan *chan; 714466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko int err; 715466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 716466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan = l2cap_chan_create(); 717466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (!chan) 718466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 719466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 720466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko BT_DBG("chan %p", chan); 721466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 722416fa7527d6bf658e5517ea36d2de9270be2c11eAndrei Emeltchenko chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP; 723466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; 724466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 725466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->ops = &a2mp_chan_ops; 726466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 727466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_set_defaults(chan); 728466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_max_tx = chan->max_tx; 729466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_tx_win = chan->tx_win; 730466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 731466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; 732466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; 733466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 734466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko skb_queue_head_init(&chan->tx_q); 735466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 736466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mode = L2CAP_MODE_ERTM; 737466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 738466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko err = l2cap_ertm_init(chan); 739466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (err < 0) { 740466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_del(chan, 0); 741466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 742466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko } 743466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 744466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->conf_state = 0; 745466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 74693c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko if (locked) 74793c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko __l2cap_chan_add(conn, chan); 74893c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko else 74993c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko l2cap_chan_add(conn, chan); 750466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 751466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_mps = chan->omtu; 752466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mps = chan->omtu; 753466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 754466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->state = BT_CONNECTED; 755466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 756466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return chan; 757466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko} 7589740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7599740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko/* AMP Manager functions */ 760f706adfeade767d2194c9f39c0f75e944b0bdd23Andrei Emeltchenkostruct amp_mgr *amp_mgr_get(struct amp_mgr *mgr) 7619740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 762a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 7639740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7649740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_get(&mgr->kref); 765f706adfeade767d2194c9f39c0f75e944b0bdd23Andrei Emeltchenko 766f706adfeade767d2194c9f39c0f75e944b0bdd23Andrei Emeltchenko return mgr; 7679740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 7689740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7699740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic void amp_mgr_destroy(struct kref *kref) 7709740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 7719740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); 7729740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7739740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("mgr %p", mgr); 7749740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 775f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_lock(&_mgr_list_lock); 776f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_del(&mgr->list); 777f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_mgr_list_lock); 778f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko 77952c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko amp_ctrl_list_flush(mgr); 7809740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 7819740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 7829740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7839740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkoint amp_mgr_put(struct amp_mgr *mgr) 7849740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 785a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 7869740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7879740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return kref_put(&mgr->kref, &_mgr_destroy); 7889740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 7899740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 79093c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenkostatic struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked) 7919740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 7929740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr; 7939740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct l2cap_chan *chan; 7949740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7959740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); 7969740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!mgr) 7979740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 7989740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 7999740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("conn %p mgr %p", conn, mgr); 8009740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 8019740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->l2cap_conn = conn; 8029740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 80393c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko chan = a2mp_chan_open(conn, locked); 8049740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!chan) { 8059740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 8069740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 8079740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko } 8089740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 8099740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->a2mp_chan = chan; 8109740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko chan->data = mgr; 8119740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 8129740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko conn->hcon->amp_mgr = mgr; 8139740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 8149740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_init(&mgr->kref); 8159740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 81652c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko /* Remote AMP ctrl list initialization */ 81752c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko INIT_LIST_HEAD(&mgr->amp_ctrls); 81852c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko mutex_init(&mgr->amp_ctrls_lock); 81952c0d6e56b634b195e377192182391d526cdd5e4Andrei Emeltchenko 820f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_lock(&_mgr_list_lock); 821f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_add(&mgr->list, &_mgr_list); 822f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_mgr_list_lock); 823f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko 8249740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return mgr; 8259740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 82697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 82797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenkostruct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, 82897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko struct sk_buff *skb) 82997e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko{ 83097e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko struct amp_mgr *mgr; 83197e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 83293c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko mgr = amp_mgr_create(conn, false); 83397e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko if (!mgr) { 83497e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko BT_ERR("Could not create AMP manager"); 83597e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko return NULL; 83697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko } 83797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 83897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); 83997e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 84097e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko return mgr->a2mp_chan; 84197e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko} 842f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko 843f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenkostruct amp_mgr *amp_mgr_lookup_by_state(u8 state) 844f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko{ 845f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko struct amp_mgr *mgr; 846f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko 847f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_lock(&_mgr_list_lock); 848f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_for_each_entry(mgr, &_mgr_list, list) { 849cb6801c640c759fe02c812728c2661bd8ba5a302Andrei Emeltchenko if (test_and_clear_bit(state, &mgr->state)) { 850f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko amp_mgr_get(mgr); 851f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_mgr_list_lock); 852f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko return mgr; 853f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko } 854f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko } 855f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_mgr_list_lock); 856f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko 857f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko return NULL; 858f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko} 8598e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8608e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenkovoid a2mp_send_getinfo_rsp(struct hci_dev *hdev) 8618e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko{ 8628e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko struct amp_mgr *mgr; 8638e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko struct a2mp_info_rsp rsp; 8648e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8658e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO); 8668e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko if (!mgr) 8678e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko return; 8688e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8698e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko BT_DBG("%s mgr %p", hdev->name, mgr); 8708e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8718e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.id = hdev->id; 8728e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 8738e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8748e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko if (hdev->amp_type != HCI_BREDR) { 8758e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.status = 0; 8768e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.total_bw = cpu_to_le32(hdev->amp_total_bw); 8778e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.max_bw = cpu_to_le32(hdev->amp_max_bw); 8788e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.min_latency = cpu_to_le32(hdev->amp_min_latency); 8798e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap); 8808e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size); 8818e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko } 8828e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko 8838e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp); 8848e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko amp_mgr_put(mgr); 8858e2a0d92c56ec6955526a8b60838c9b00f70540dAndrei Emeltchenko} 886903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 887903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenkovoid a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status) 888903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko{ 889903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko struct amp_mgr *mgr; 890903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko struct amp_assoc *loc_assoc = &hdev->loc_assoc; 891903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko struct a2mp_amp_assoc_rsp *rsp; 892903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko size_t len; 893903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 894903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); 895903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko if (!mgr) 896903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko return; 897903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 898903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko BT_DBG("%s mgr %p", hdev->name, mgr); 899903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 900903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len; 901903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp = kzalloc(len, GFP_KERNEL); 902903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko if (!rsp) { 903903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko amp_mgr_put(mgr); 904903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko return; 905903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko } 906903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 907903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp->id = hdev->id; 908903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 909903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko if (status) { 910903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp->status = A2MP_STATUS_INVALID_CTRL_ID; 911903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko } else { 912903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko rsp->status = A2MP_STATUS_SUCCESS; 913903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len); 914903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko } 915903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko 916903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp); 917903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko amp_mgr_put(mgr); 918903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko kfree(rsp); 919903e45411099ae8292f5ce637ad0c72f6fef61dbAndrei Emeltchenko} 92093c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko 9219495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenkovoid a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status) 9229495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko{ 9239495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko struct amp_mgr *mgr; 9249495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko struct amp_assoc *loc_assoc = &hdev->loc_assoc; 9259495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko struct a2mp_physlink_req *req; 9269495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko struct l2cap_chan *bredr_chan; 9279495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko size_t len; 9289495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9299495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL); 9309495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko if (!mgr) 9319495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko return; 9329495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9339495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko len = sizeof(*req) + loc_assoc->len; 9349495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9359495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len); 9369495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9379495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko req = kzalloc(len, GFP_KERNEL); 9389495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko if (!req) { 9399495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko amp_mgr_put(mgr); 9409495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko return; 9419495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko } 9429495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9439495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko bredr_chan = mgr->bredr_chan; 9449495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko if (!bredr_chan) 9459495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko goto clean; 9469495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9479495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko req->local_id = hdev->id; 948fffadc08ebf1f4c61bb8f9be0f1d8c3c053d815fAndrei Emeltchenko req->remote_id = bredr_chan->remote_amp_id; 9499495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len); 9509495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9519495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req); 9529495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9539495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenkoclean: 9549495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko amp_mgr_put(mgr); 9559495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko kfree(req); 9569495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko} 9579495b2ee757f7747d7c28f9ba8d7edc53005ec2dAndrei Emeltchenko 9588e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenkovoid a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status) 9598e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko{ 9608e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko struct amp_mgr *mgr; 9618e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko struct a2mp_physlink_rsp rsp; 9628e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko struct hci_conn *hs_hcon; 9638e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko 9648e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC); 9658e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko if (!mgr) 9668e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko return; 9678e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko 9688e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT); 9698e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko if (!hs_hcon) { 9708e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; 9718e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko } else { 9728e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko rsp.remote_id = hs_hcon->remote_id; 9738e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko rsp.status = A2MP_STATUS_SUCCESS; 9748e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko } 9758e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko 9768e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon, 9778e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko status); 9788e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko 9798e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko rsp.local_id = hdev->id; 9808e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp); 9818e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko amp_mgr_put(mgr); 9828e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko} 9838e05e3ba88adcf7ac644e6ef26676ea7c048a08cAndrei Emeltchenko 98493c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenkovoid a2mp_discover_amp(struct l2cap_chan *chan) 98593c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko{ 98693c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko struct l2cap_conn *conn = chan->conn; 98793c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko struct amp_mgr *mgr = conn->hcon->amp_mgr; 98893c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko struct a2mp_discov_req req; 98993c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko 99093c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr); 99193c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko 99293c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko if (!mgr) { 99393c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko mgr = amp_mgr_create(conn, true); 99493c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko if (!mgr) 99593c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko return; 99693c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko } 99793c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko 99893c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko mgr->bredr_chan = chan; 99993c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko 100093c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); 100193c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko req.ext_feat = 0; 100293c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req); 100393c3e8f5c9a0e4dc6b6c93108dcf3ec54ab1191aAndrei Emeltchenko} 1004