a2mp.c revision e072f5dab22e7bf0a10daf854acc0fc271396ee7
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 2836b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko/* Handle A2MP signalling */ 2846b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenkostatic int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) 2856b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko{ 2866b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd *hdr = (void *) skb->data; 2876b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 2886b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko int err = 0; 2896b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 2906b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_get(mgr); 2916b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 2926b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko while (skb->len >= sizeof(*hdr)) { 2936b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd *hdr = (void *) skb->data; 2946b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko u16 len = le16_to_cpu(hdr->len); 2956b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 2966b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_DBG("code 0x%02x id %d len %d", hdr->code, hdr->ident, len); 2976b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 2986b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko skb_pull(skb, sizeof(*hdr)); 2996b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3006b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (len > skb->len || !hdr->ident) { 3016b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 3026b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 3036b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3046b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3056b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko mgr->ident = hdr->ident; 3066b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3076b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko switch (hdr->code) { 3086b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_COMMAND_REJ: 30921dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko a2mp_command_rej(mgr, skb, hdr); 31021dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko break; 31121dbd2ce35f6d2b4aa5363be6c839cdb50644e11Andrei Emeltchenko 3126b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_REQ: 3138598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko err = a2mp_discover_req(mgr, skb, hdr); 3148598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko break; 3158598d064cbf22b2d84c7cd8a9fcb97138baffe3fAndrei Emeltchenko 3166b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CHANGE_NOTIFY: 317329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko err = a2mp_change_notify(mgr, skb, hdr); 318329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko break; 319329d81af29344a2ad2f9595310be74644421797aAndrei Emeltchenko 3206b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_REQ: 32147f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko err = a2mp_getinfo_req(mgr, skb, hdr); 32247f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko break; 32347f2d97d38816aaca94c9b6961c6eff1cfcd0bd6Andrei Emeltchenko 3246b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_REQ: 325a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko err = a2mp_getampassoc_req(mgr, skb, hdr); 326a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko break; 327a28381dc9ca3e54b0678e2cd7c68c1afb2d7cc76Andrei Emeltchenko 3286b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_REQ: 329e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko err = a2mp_createphyslink_req(mgr, skb, hdr); 330e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko break; 331e072f5dab22e7bf0a10daf854acc0fc271396ee7Andrei Emeltchenko 3326b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_REQ: 3336b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CHANGE_RSP: 3346b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCOVER_RSP: 3356b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETINFO_RSP: 3366b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_GETAMPASSOC_RSP: 3376b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_CREATEPHYSLINK_RSP: 3386b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko case A2MP_DISCONNPHYSLINK_RSP: 3396b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko default: 3406b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); 3416b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko err = -EINVAL; 3426b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko break; 3436b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3446b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3456b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3466b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko if (err) { 3476b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko struct a2mp_cmd_rej rej; 3486b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko rej.reason = __constant_cpu_to_le16(0); 3496b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3506b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); 3516b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3526b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), 3536b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko &rej); 3546b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko } 3556b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3566b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko /* Always free skb and return success error code to prevent 3576b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko from sending L2CAP Disconnect over A2MP channel */ 3586b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko kfree_skb(skb); 3596b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3606b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko amp_mgr_put(mgr); 3616b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 3626b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko return 0; 3636b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko} 3646b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko 36546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_close_cb(struct l2cap_chan *chan) 36646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 36746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko l2cap_chan_destroy(chan); 36846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 36946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 37046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state) 37146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 37246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko struct amp_mgr *mgr = chan->data; 37346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 37446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (!mgr) 37546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return; 37646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 37746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_DBG("chan %p state %s", chan, state_to_string(state)); 37846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 37946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko chan->state = state; 38046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 38146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko switch (state) { 38246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko case BT_CLOSED: 38346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko if (mgr) 38446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko amp_mgr_put(mgr); 38546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko break; 38646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko } 38746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 38846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 38946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, 39046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko unsigned long len, int nb) 39146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 39246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return bt_skb_alloc(len, GFP_KERNEL); 39346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 39446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 39546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic struct l2cap_chan *a2mp_chan_no_new_conn_cb(struct l2cap_chan *chan) 39646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 39746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_ERR("new_connection for chan %p not implemented", chan); 39846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 39946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko return NULL; 40046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 40146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 40246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_no_teardown_cb(struct l2cap_chan *chan, int err) 40346d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 40446d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_ERR("teardown for chan %p not implemented", chan); 40546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 40646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 40746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenkostatic void a2mp_chan_no_ready(struct l2cap_chan *chan) 40846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko{ 40946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko BT_ERR("ready for chan %p not implemented", chan); 41046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko} 41146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 412466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_ops a2mp_chan_ops = { 413466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko .name = "L2CAP A2MP channel", 4146b44d9b8d96b37f72ccd7335b32f386a67b7f1f4Andrei Emeltchenko .recv = a2mp_chan_recv_cb, 41546d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .close = a2mp_chan_close_cb, 41646d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .state_change = a2mp_chan_state_change_cb, 41746d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .alloc_skb = a2mp_chan_alloc_skb_cb, 41846d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko 41946d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko /* Not implemented for A2MP */ 42046d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .new_connection = a2mp_chan_no_new_conn_cb, 42146d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .teardown = a2mp_chan_no_teardown_cb, 42246d5c9088fbcc8a570bc271f77940973d9cae074Andrei Emeltchenko .ready = a2mp_chan_no_ready, 423466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko}; 424466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 425466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenkostatic struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn) 426466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko{ 427466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko struct l2cap_chan *chan; 428466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko int err; 429466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 430466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan = l2cap_chan_create(); 431466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (!chan) 432466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 433466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 434466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko BT_DBG("chan %p", chan); 435466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 436466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko hci_conn_hold(conn->hcon); 437466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 438466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->omtu = L2CAP_A2MP_DEFAULT_MTU; 439466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->imtu = L2CAP_A2MP_DEFAULT_MTU; 440466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; 441466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 442466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->ops = &a2mp_chan_ops; 443466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 444466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_set_defaults(chan); 445466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_max_tx = chan->max_tx; 446466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_tx_win = chan->tx_win; 447466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 448466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; 449466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; 450466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 451466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko skb_queue_head_init(&chan->tx_q); 452466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 453466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mode = L2CAP_MODE_ERTM; 454466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 455466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko err = l2cap_ertm_init(chan); 456466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko if (err < 0) { 457466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_del(chan, 0); 458466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return NULL; 459466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko } 460466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 461466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->conf_state = 0; 462466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 463466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko l2cap_chan_add(conn, chan); 464466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 465466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->remote_mps = chan->omtu; 466466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->mps = chan->omtu; 467466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 468466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko chan->state = BT_CONNECTED; 469466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko 470466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko return chan; 471466f8004f364e9cb46d9124109972489eccfb404Andrei Emeltchenko} 4729740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4739740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko/* AMP Manager functions */ 4749740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkovoid amp_mgr_get(struct amp_mgr *mgr) 4759740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 4769740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("mgr %p", mgr); 4779740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4789740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_get(&mgr->kref); 4799740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 4809740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4819740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic void amp_mgr_destroy(struct kref *kref) 4829740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 4839740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); 4849740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4859740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("mgr %p", mgr); 4869740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4879740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 4889740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 4899740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4909740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkoint amp_mgr_put(struct amp_mgr *mgr) 4919740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 4929740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("mgr %p", mgr); 4939740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4949740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return kref_put(&mgr->kref, &_mgr_destroy); 4959740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 4969740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 4979740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenkostatic struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn) 4989740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko{ 4999740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct amp_mgr *mgr; 5009740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko struct l2cap_chan *chan; 5019740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5029740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); 5039740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!mgr) 5049740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 5059740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5069740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko BT_DBG("conn %p mgr %p", conn, mgr); 5079740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5089740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->l2cap_conn = conn; 5099740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5109740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko chan = a2mp_chan_open(conn); 5119740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko if (!chan) { 5129740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kfree(mgr); 5139740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return NULL; 5149740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko } 5159740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5169740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko mgr->a2mp_chan = chan; 5179740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko chan->data = mgr; 5189740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5199740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko conn->hcon->amp_mgr = mgr; 5209740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5219740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko kref_init(&mgr->kref); 5229740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko 5239740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko return mgr; 5249740e49d17e55f3832661fd99a8e0a17e921a82eAndrei Emeltchenko} 525