a2mp.c revision 8e8c7e36fb216d2d072116de3bec6130627ad691
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> 19466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 20f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko/* A2MP build & send command helper functions */ 21f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenkostatic struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) 22f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko{ 23f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko struct a2mp_cmd *cmd; 24f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko int plen; 25f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 26f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko plen = sizeof(*cmd) + len; 27f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko cmd = kzalloc(plen, GFP_KERNEL); 28f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko if (!cmd) 29f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko return NULL; 30f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 31f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko cmd->code = code; 32f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko cmd->ident = ident; 33f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko cmd->len = cpu_to_le16(len); 34f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 35f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko memcpy(cmd->data, data, len); 36f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 37f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko return cmd; 38f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko} 39f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 40f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenkostatic void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, 41f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko void *data) 42f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko{ 43f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko struct l2cap_chan *chan = mgr->a2mp_chan; 44f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko struct a2mp_cmd *cmd; 45f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko u16 total_len = len + sizeof(*cmd); 46f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko struct kvec iv; 47f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko struct msghdr msg; 48f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 49f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko cmd = __a2mp_build(code, ident, len, data); 50f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko if (!cmd) 51f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko return; 52f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 53f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko iv.iov_base = cmd; 54f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko iv.iov_len = total_len; 55f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 56f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko memset(&msg, 0, sizeof(msg)); 57f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 58f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko msg.msg_iov = (struct iovec *) &iv; 59f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko msg.msg_iovlen = 1; 60f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 61f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko l2cap_chan_send(chan, &msg, total_len, 0); 62f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 63f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko kfree(cmd); 64f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko} 65f6d3c6e783b0e9f75b18232f8ff8cd5dbc3f7301Andrei Emeltchenko 668598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic inline void __a2mp_cl_bredr(struct a2mp_cl *cl) 678598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{ 688598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl->id = 0; 698598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl->type = 0; 708598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl->status = 1; 718598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko} 728598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 738598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko/* hci_dev_list shall be locked */ 748598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl, u8 num_ctrl) 758598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{ 768598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko int i = 0; 778598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko struct hci_dev *hdev; 788598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 798598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko __a2mp_cl_bredr(cl); 808598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 818598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko list_for_each_entry(hdev, &hci_dev_list, list) { 828598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko /* Iterate through AMP controllers */ 838598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko if (hdev->id == HCI_BREDR_ID) 848598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko continue; 858598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 868598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko /* Starting from second entry */ 878598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko if (++i >= num_ctrl) 888598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko return; 898598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 908598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl[i].id = hdev->id; 918598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl[i].type = hdev->amp_type; 928598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko cl[i].status = hdev->amp_status; 938598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko } 948598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko} 958598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 9621dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko/* Processing A2MP messages */ 9721dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenkostatic int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, 9821dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko struct a2mp_cmd *hdr) 9921dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko{ 10021dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko struct a2mp_cmd_rej *rej = (void *) skb->data; 10121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 10221dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*rej)) 10321dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko return -EINVAL; 10421dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 10521dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); 10621dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 10721dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko skb_pull(skb, sizeof(*rej)); 10821dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 10921dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko return 0; 11021dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko} 11121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 1128598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenkostatic int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, 1138598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko struct a2mp_cmd *hdr) 1148598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko{ 1158598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko struct a2mp_discov_req *req = (void *) skb->data; 1168598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko u16 len = le16_to_cpu(hdr->len); 1178598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko struct a2mp_discov_rsp *rsp; 1188598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko u16 ext_feat; 1198598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko u8 num_ctrl; 1208598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1218598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko if (len < sizeof(*req)) 1228598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko return -EINVAL; 1238598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1248598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko skb_pull(skb, sizeof(*req)); 1258598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1268598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko ext_feat = le16_to_cpu(req->ext_feat); 1278598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1288598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); 1298598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1308598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko /* check that packet is not broken for now */ 1318598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko while (ext_feat & A2MP_FEAT_EXT) { 1328598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko if (len < sizeof(ext_feat)) 1338598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko return -EINVAL; 1348598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1358598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko ext_feat = get_unaligned_le16(skb->data); 1368598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko BT_DBG("efm 0x%4.4x", ext_feat); 1378598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko len -= sizeof(ext_feat); 1388598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko skb_pull(skb, sizeof(ext_feat)); 1398598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko } 1408598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1418598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko read_lock(&hci_dev_list_lock); 1428598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1438598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko num_ctrl = __hci_num_ctrl(); 1448598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); 1458598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko rsp = kmalloc(len, GFP_ATOMIC); 1468598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko if (!rsp) { 1478598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko read_unlock(&hci_dev_list_lock); 1488598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko return -ENOMEM; 1498598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko } 1508598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1518598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); 1528598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko rsp->ext_feat = 0; 1538598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1548598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko __a2mp_add_cl(mgr, rsp->cl, num_ctrl); 1558598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1568598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko read_unlock(&hci_dev_list_lock); 1578598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1588598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); 1598598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 1608598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko kfree(rsp); 1618598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko return 0; 1628598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko} 1638598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 164329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenkostatic int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, 165329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko struct a2mp_cmd *hdr) 166329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko{ 167329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko struct a2mp_cl *cl = (void *) skb->data; 168329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 169329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko while (skb->len >= sizeof(*cl)) { 170329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, 171329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko cl->status); 172329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); 173329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko } 174329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 175329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko /* TODO send A2MP_CHANGE_RSP */ 176329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 177329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko return 0; 178329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko} 179329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 18047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenkostatic int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, 18147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct a2mp_cmd *hdr) 18247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko{ 18347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct a2mp_info_req *req = (void *) skb->data; 18447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct a2mp_info_rsp rsp; 18547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko struct hci_dev *hdev; 18647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 18747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 18847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko return -EINVAL; 18947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 19047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko BT_DBG("id %d", req->id); 19147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 19247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.id = req->id; 19347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 19447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 19547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko hdev = hci_dev_get(req->id); 19647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko if (hdev && hdev->amp_type != HCI_BREDR) { 19747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.status = 0; 19847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.total_bw = cpu_to_le32(hdev->amp_total_bw); 19947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.max_bw = cpu_to_le32(hdev->amp_max_bw); 20047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.min_latency = cpu_to_le32(hdev->amp_min_latency); 20147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap); 20247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size); 20347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko } 20447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 20547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko if (hdev) 20647f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko hci_dev_put(hdev); 20747f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 20847f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), &rsp); 20947f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 21047f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko skb_pull(skb, sizeof(*req)); 21147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko return 0; 21247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko} 21347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 214a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenkostatic int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, 215a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_cmd *hdr) 216a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko{ 217a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_amp_assoc_req *req = (void *) skb->data; 218a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct hci_dev *hdev; 219a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 220a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 221a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko return -EINVAL; 222a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 223a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko BT_DBG("id %d", req->id); 224a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 225a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko hdev = hci_dev_get(req->id); 226a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko if (!hdev || hdev->amp_type == HCI_BREDR) { 227a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko struct a2mp_amp_assoc_rsp rsp; 228a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko rsp.id = req->id; 229a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 230a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 231a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), 232a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko &rsp); 233a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko goto clean; 234a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko } 235a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 236a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko /* Placeholder for HCI Read AMP Assoc */ 237a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 238a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenkoclean: 239a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko if (hdev) 240a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko hci_dev_put(hdev); 241a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 242a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko skb_pull(skb, sizeof(*req)); 243a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko return 0; 244a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko} 245a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 246e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkostatic int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 247e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_cmd *hdr) 248e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko{ 249e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_physlink_req *req = (void *) skb->data; 250e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 251e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct a2mp_physlink_rsp rsp; 252e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko struct hci_dev *hdev; 253e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 254e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 255e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko return -EINVAL; 256e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 257e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); 258e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 259e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.local_id = req->remote_id; 260e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.remote_id = req->local_id; 261e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 262e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko hdev = hci_dev_get(req->remote_id); 263e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (!hdev || hdev->amp_type != HCI_AMP) { 264e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 265e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko goto send_rsp; 266e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko } 267e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 268e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko /* TODO process physlink create */ 269e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 270e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko rsp.status = A2MP_STATUS_SUCCESS; 271e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 272e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenkosend_rsp: 273e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko if (hdev) 274e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko hci_dev_put(hdev); 275e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 276e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, sizeof(rsp), 277e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko &rsp); 278e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 279e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko skb_pull(skb, le16_to_cpu(hdr->len)); 280e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko return 0; 281e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko} 282e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 2836113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkostatic int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, 2846113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_cmd *hdr) 2856113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko{ 2866113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_physlink_req *req = (void *) skb->data; 2876113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct a2mp_physlink_rsp rsp; 2886113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko struct hci_dev *hdev; 2896113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 2906113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko if (le16_to_cpu(hdr->len) < sizeof(*req)) 2916113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko return -EINVAL; 2926113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 2936113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); 2946113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 2956113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.local_id = req->remote_id; 2966113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.remote_id = req->local_id; 2976113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.status = A2MP_STATUS_SUCCESS; 2986113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 2996113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko hdev = hci_dev_get(req->local_id); 3006113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko if (!hdev) { 3016113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko rsp.status = A2MP_STATUS_INVALID_CTRL_ID; 3026113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko goto send_rsp; 3036113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko } 3046113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 3056113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko /* TODO Disconnect Phys Link here */ 3066113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 3076113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko hci_dev_put(hdev); 3086113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 3096113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenkosend_rsp: 3106113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); 3116113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 3126113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko skb_pull(skb, sizeof(*req)); 3136113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko return 0; 3146113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko} 3156113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 316f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenkostatic inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, 317f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko struct a2mp_cmd *hdr) 318f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko{ 3198e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); 320f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 321f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko skb_pull(skb, le16_to_cpu(hdr->len)); 322f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko return 0; 323f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko} 324f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 3256b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko/* Handle A2MP signalling */ 3266b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenkostatic int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) 3276b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko{ 3286b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd *hdr = (void *) skb->data; 3296b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 3306b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko int err = 0; 3316b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3326b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_get(mgr); 3336b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3346b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko while (skb->len >= sizeof(*hdr)) { 3356b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd *hdr = (void *) skb->data; 3366b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko u16 len = le16_to_cpu(hdr->len); 3376b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3388e8c7e36fb216d2d072116de3bec6130627ad691Andrei Emeltchenko BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); 3396b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3406b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko skb_pull(skb, sizeof(*hdr)); 3416b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3426b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (len > skb->len || !hdr->ident) { 3436b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 3446b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 3456b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3466b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3476b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko mgr->ident = hdr->ident; 3486b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3496b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko switch (hdr->code) { 3506b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_COMMAND_REJ: 35121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko a2mp_command_rej(mgr, skb, hdr); 35221dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko break; 35321dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 3546b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_REQ: 3558598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko err = a2mp_discover_req(mgr, skb, hdr); 3568598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko break; 3578598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 3586b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CHANGE_NOTIFY: 359329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko err = a2mp_change_notify(mgr, skb, hdr); 360329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko break; 361329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 3626b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_REQ: 36347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko err = a2mp_getinfo_req(mgr, skb, hdr); 36447f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko break; 36547f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 3666b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_REQ: 367a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko err = a2mp_getampassoc_req(mgr, skb, hdr); 368a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko break; 369a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 3706b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_REQ: 371e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko err = a2mp_createphyslink_req(mgr, skb, hdr); 372e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko break; 373e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 3746b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_REQ: 3756113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko err = a2mp_discphyslink_req(mgr, skb, hdr); 3766113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko break; 3776113f84fc1a8962aed25f54a115b196e9aea151fAndrei Emeltchenko 3786b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CHANGE_RSP: 3796b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_RSP: 3806b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_RSP: 3816b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_RSP: 3826b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_RSP: 3836b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_RSP: 384f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko err = a2mp_cmd_rsp(mgr, skb, hdr); 385f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko break; 386f6410a849b76f56c78d989786eb427b85a559b9fAndrei Emeltchenko 3876b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko default: 3886b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); 3896b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 3906b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 3916b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3926b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3936b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3946b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (err) { 3956b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd_rej rej; 3966b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko rej.reason = __constant_cpu_to_le16(0); 3976b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3986b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); 3996b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 4006b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), 4016b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko &rej); 4026b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 4036b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 4046b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko /* Always free skb and return success error code to prevent 4056b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko from sending L2CAP Disconnect over A2MP channel */ 4066b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko kfree_skb(skb); 4076b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 4086b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_put(mgr); 4096b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 4106b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko return 0; 4116b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko} 4126b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 41346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_close_cb(struct l2cap_chan *chan) 41446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 41546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko l2cap_chan_destroy(chan); 41646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 41746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 41846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state) 41946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 42046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 42146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 42246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (!mgr) 42346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return; 42446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 42546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_DBG("chan %p state %s", chan, state_to_string(state)); 42646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 42746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko chan->state = state; 42846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 42946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko switch (state) { 43046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko case BT_CLOSED: 43146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (mgr) 43246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko amp_mgr_put(mgr); 43346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko break; 43446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko } 43546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 43646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 43746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, 43846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko unsigned long len, int nb) 43946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 44046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return bt_skb_alloc(len, GFP_KERNEL); 44146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 44246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 443466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_ops a2mp_chan_ops = { 444466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko .name = "L2CAP A2MP channel", 4456b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko .recv = a2mp_chan_recv_cb, 44646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .close = a2mp_chan_close_cb, 44746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .state_change = a2mp_chan_state_change_cb, 44846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .alloc_skb = a2mp_chan_alloc_skb_cb, 44946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 45046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko /* Not implemented for A2MP */ 4517e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .new_connection = l2cap_chan_no_new_connection, 4527e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .teardown = l2cap_chan_no_teardown, 4537e1af8a3a51dbf5dc7392fb294a0830f7e853aa8Gustavo Padovan .ready = l2cap_chan_no_ready, 454466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko}; 455466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 456466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) 457466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko{ 458466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko struct l2cap_chan *chan; 459466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko int err; 460466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 461466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan = l2cap_chan_create(); 462466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (!chan) 463466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 464466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 465466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko BT_DBG("chan %p", chan); 466466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 467416fa7527d6bf658e5517ea36d2de9270be2c11eAndrei Emeltchenko chan->chan_type = L2CAP_CHAN_CONN_FIX_A2MP; 468466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; 469466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 470466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->ops = &a2mp_chan_ops; 471466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 472466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_set_defaults(chan); 473466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_max_tx = chan->max_tx; 474466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_tx_win = chan->tx_win; 475466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 476466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; 477466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; 478466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 479466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko skb_queue_head_init(&chan->tx_q); 480466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 481466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mode = L2CAP_MODE_ERTM; 482466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 483466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko err = l2cap_ertm_init(chan); 484466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (err < 0) { 485466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_del(chan, 0); 486466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 487466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko } 488466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 489466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->conf_state = 0; 490466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 491466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_add(conn, chan); 492466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 493466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_mps = chan->omtu; 494466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mps = chan->omtu; 495466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 496466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->state = BT_CONNECTED; 497466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 498466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return chan; 499466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko} 5009740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5019740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko/* AMP Manager functions */ 5029740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkovoid amp_mgr_get(struct amp_mgr *mgr) 5039740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 504a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 5059740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5069740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_get(&mgr->kref); 5079740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 5089740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5099740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic void amp_mgr_destroy(struct kref *kref) 5109740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 5119740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); 5129740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5139740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("mgr %p", mgr); 5149740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5159740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 5169740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 5179740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5189740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkoint amp_mgr_put(struct amp_mgr *mgr) 5199740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 520a0dfe0ab6bf194805ce9d6a2dc81efab7a4a7fdaAndrei Emeltchenko BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); 5219740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5229740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return kref_put(&mgr->kref, &_mgr_destroy); 5239740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 5249740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5259740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn) 5269740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 5279740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr; 5289740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct l2cap_chan *chan; 5299740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5309740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); 5319740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!mgr) 5329740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 5339740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5349740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("conn %p mgr %p", conn, mgr); 5359740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5369740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->l2cap_conn = conn; 5379740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5389740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko chan = a2mp_chan_open(conn); 5399740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!chan) { 5409740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 5419740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 5429740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko } 5439740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5449740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->a2mp_chan = chan; 5459740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko chan->data = mgr; 5469740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5479740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko conn->hcon->amp_mgr = mgr; 5489740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5499740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_init(&mgr->kref); 5509740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5519740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return mgr; 5529740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 55397e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 55497e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenkostruct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, 55597e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko struct sk_buff *skb) 55697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko{ 55797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko struct amp_mgr *mgr; 55897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 55997e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko mgr = amp_mgr_create(conn); 56097e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko if (!mgr) { 56197e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko BT_ERR("Could not create AMP manager"); 56297e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko return NULL; 56397e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko } 56497e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 56597e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); 56697e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko 56797e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko return mgr->a2mp_chan; 56897e8e89d2d8185b7644c9941636d3682eedc517bAndrei Emeltchenko} 569