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(&_mgr_list_lock); 594f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_del(&mgr->list); 595f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_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, &_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(&_mgr_list_lock); 639f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_add(&mgr->list, &_mgr_list); 640f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_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(&_mgr_list_lock); 666f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko list_for_each_entry(mgr, &_mgr_list, list) { 667f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko if (mgr->state == state) { 668f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko amp_mgr_get(mgr); 669f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_mgr_list_lock); 670f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko return mgr; 671f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko } 672f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko } 673f97268fccdd4e76462195216fcab621b8d4a6cd1Andrei Emeltchenko mutex_unlock(&_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