^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1) // SPDX-License-Identifier: GPL-2.0-only
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 2) /*
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 3) Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 4) Copyright (c) 2011,2012 Intel Corp.
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 5)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 6) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 7)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 8) #include <net/bluetooth/bluetooth.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 9) #include <net/bluetooth/hci_core.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 10) #include <net/bluetooth/l2cap.h>
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 11)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 12) #include "hci_request.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 13) #include "a2mp.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 14) #include "amp.h"
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 15)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 16) #define A2MP_FEAT_EXT 0x8000
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 17)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 18) /* Global AMP Manager list */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 19) static LIST_HEAD(amp_mgr_list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 20) static DEFINE_MUTEX(amp_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 21)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 22) /* A2MP build & send command helper functions */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 23) static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 24) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 25) struct a2mp_cmd *cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 26) int plen;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 27)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 28) plen = sizeof(*cmd) + len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 29) cmd = kzalloc(plen, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 30) if (!cmd)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 31) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 32)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 33) cmd->code = code;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 34) cmd->ident = ident;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 35) cmd->len = cpu_to_le16(len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 36)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 37) memcpy(cmd->data, data, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 38)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 39) return cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 40) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 41)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 42) static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 43) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 44) struct l2cap_chan *chan = mgr->a2mp_chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 45) struct a2mp_cmd *cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 46) u16 total_len = len + sizeof(*cmd);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 47) struct kvec iv;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 48) struct msghdr msg;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 49)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 50) cmd = __a2mp_build(code, ident, len, data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 51) if (!cmd)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 52) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 53)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 54) iv.iov_base = cmd;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 55) iv.iov_len = total_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 56)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 57) memset(&msg, 0, sizeof(msg));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 58)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 59) iov_iter_kvec(&msg.msg_iter, WRITE, &iv, 1, total_len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 60)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 61) l2cap_chan_send(chan, &msg, total_len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 62)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 63) kfree(cmd);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 64) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 65)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 66) static u8 __next_ident(struct amp_mgr *mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 67) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 68) if (++mgr->ident == 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 69) mgr->ident = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 70)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 71) return mgr->ident;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 72) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 73)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 74) static struct amp_mgr *amp_mgr_lookup_by_state(u8 state)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 75) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 76) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 77)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 78) mutex_lock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 79) list_for_each_entry(mgr, &_mgr_list, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 80) if (test_and_clear_bit(state, &mgr->state)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 81) amp_mgr_get(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 82) mutex_unlock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 83) return mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 84) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 85) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 86) mutex_unlock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 87)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 88) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 89) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 90)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 91) /* hci_dev_list shall be locked */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 92) static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 93) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 94) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 95) int i = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 96)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 97) cl[0].id = AMP_ID_BREDR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 98) cl[0].type = AMP_TYPE_BREDR;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 99) cl[0].status = AMP_STATUS_BLUETOOTH_ONLY;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 100)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 101) list_for_each_entry(hdev, &hci_dev_list, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 102) if (hdev->dev_type == HCI_AMP) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 103) cl[i].id = hdev->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 104) cl[i].type = hdev->amp_type;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 105) if (test_bit(HCI_UP, &hdev->flags))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 106) cl[i].status = hdev->amp_status;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 107) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 108) cl[i].status = AMP_STATUS_POWERED_DOWN;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 109) i++;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 110) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 111) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 112) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 113)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 114) /* Processing A2MP messages */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 115) static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 116) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 117) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 118) struct a2mp_cmd_rej *rej = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 119)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 120) if (le16_to_cpu(hdr->len) < sizeof(*rej))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 121) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 122)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 123) BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 124)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 125) skb_pull(skb, sizeof(*rej));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 126)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 127) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 128) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 129)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 130) static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 131) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 132) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 133) struct a2mp_discov_req *req = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 134) u16 len = le16_to_cpu(hdr->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 135) struct a2mp_discov_rsp *rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 136) u16 ext_feat;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 137) u8 num_ctrl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 138) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 139)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 140) if (len < sizeof(*req))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 141) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 142)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 143) skb_pull(skb, sizeof(*req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 144)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 145) ext_feat = le16_to_cpu(req->ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 146)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 147) BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 148)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 149) /* check that packet is not broken for now */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 150) while (ext_feat & A2MP_FEAT_EXT) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 151) if (len < sizeof(ext_feat))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 152) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 153)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 154) ext_feat = get_unaligned_le16(skb->data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 155) BT_DBG("efm 0x%4.4x", ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 156) len -= sizeof(ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 157) skb_pull(skb, sizeof(ext_feat));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 158) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 159)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 160) read_lock(&hci_dev_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 161)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 162) /* at minimum the BR/EDR needs to be listed */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 163) num_ctrl = 1;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 164)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 165) list_for_each_entry(hdev, &hci_dev_list, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 166) if (hdev->dev_type == HCI_AMP)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 167) num_ctrl++;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 168) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 169)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 170) len = struct_size(rsp, cl, num_ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 171) rsp = kmalloc(len, GFP_ATOMIC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 172) if (!rsp) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 173) read_unlock(&hci_dev_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 174) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 175) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 176)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 177) rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 178) rsp->ext_feat = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 179)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 180) __a2mp_add_cl(mgr, rsp->cl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 181)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 182) read_unlock(&hci_dev_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 183)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 184) a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 185)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 186) kfree(rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 187) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 188) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 189)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 190) static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 191) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 192) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 193) struct a2mp_discov_rsp *rsp = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 194) u16 len = le16_to_cpu(hdr->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 195) struct a2mp_cl *cl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 196) u16 ext_feat;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 197) bool found = false;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 198)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 199) if (len < sizeof(*rsp))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 200) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 201)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 202) len -= sizeof(*rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 203) skb_pull(skb, sizeof(*rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 204)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 205) ext_feat = le16_to_cpu(rsp->ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 206)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 207) BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 208)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 209) /* check that packet is not broken for now */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 210) while (ext_feat & A2MP_FEAT_EXT) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 211) if (len < sizeof(ext_feat))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 212) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 213)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 214) ext_feat = get_unaligned_le16(skb->data);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 215) BT_DBG("efm 0x%4.4x", ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 216) len -= sizeof(ext_feat);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 217) skb_pull(skb, sizeof(ext_feat));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 218) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 219)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 220) cl = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 221) while (len >= sizeof(*cl)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 222) BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 223) cl->status);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 224)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 225) if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 226) struct a2mp_info_req req;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 227)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 228) found = true;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 229)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 230) memset(&req, 0, sizeof(req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 231)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 232) req.id = cl->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 233) a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 234) sizeof(req), &req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 235) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 236)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 237) len -= sizeof(*cl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 238) cl = skb_pull(skb, sizeof(*cl));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 239) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 240)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 241) /* Fall back to L2CAP init sequence */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 242) if (!found) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 243) struct l2cap_conn *conn = mgr->l2cap_conn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 244) struct l2cap_chan *chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 245)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 246) mutex_lock(&conn->chan_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 247)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 248) list_for_each_entry(chan, &conn->chan_l, list) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 249)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 250) BT_DBG("chan %p state %s", chan,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 251) state_to_string(chan->state));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 252)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 253) if (chan->scid == L2CAP_CID_A2MP)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 254) continue;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 255)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 256) l2cap_chan_lock(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 257)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 258) if (chan->state == BT_CONNECT)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 259) l2cap_send_conn_req(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 260)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 261) l2cap_chan_unlock(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 262) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 263)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 264) mutex_unlock(&conn->chan_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 265) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 266)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 267) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 268) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 269)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 270) static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 271) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 272) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 273) struct a2mp_cl *cl = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 274)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 275) while (skb->len >= sizeof(*cl)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 276) BT_DBG("Controller id %d type %d status %d", cl->id, cl->type,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 277) cl->status);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 278) cl = skb_pull(skb, sizeof(*cl));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 279) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 280)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 281) /* TODO send A2MP_CHANGE_RSP */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 282)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 283) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 284) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 285)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 286) static void read_local_amp_info_complete(struct hci_dev *hdev, u8 status,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 287) u16 opcode)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 288) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 289) BT_DBG("%s status 0x%2.2x", hdev->name, status);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 290)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 291) a2mp_send_getinfo_rsp(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 292) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 293)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 294) static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 295) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 296) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 297) struct a2mp_info_req *req = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 298) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 299) struct hci_request hreq;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 300) int err = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 301)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 302) if (le16_to_cpu(hdr->len) < sizeof(*req))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 303) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 304)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 305) BT_DBG("id %d", req->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 306)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 307) hdev = hci_dev_get(req->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 308) if (!hdev || hdev->dev_type != HCI_AMP) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 309) struct a2mp_info_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 310)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 311) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 312)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 313) rsp.id = req->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 314) rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 315)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 316) a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 317) &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 318)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 319) goto done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 320) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 321)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 322) set_bit(READ_LOC_AMP_INFO, &mgr->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 323) hci_req_init(&hreq, hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 324) hci_req_add(&hreq, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 325) err = hci_req_run(&hreq, read_local_amp_info_complete);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 326) if (err < 0)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 327) a2mp_send_getinfo_rsp(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 328)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 329) done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 330) if (hdev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 331) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 332)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 333) skb_pull(skb, sizeof(*req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 334) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 335) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 336)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 337) static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 338) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 339) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 340) struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 341) struct a2mp_amp_assoc_req req;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 342) struct amp_ctrl *ctrl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 343)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 344) if (le16_to_cpu(hdr->len) < sizeof(*rsp))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 345) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 346)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 347) BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 348)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 349) if (rsp->status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 350) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 351)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 352) ctrl = amp_ctrl_add(mgr, rsp->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 353) if (!ctrl)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 354) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 355)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 356) memset(&req, 0, sizeof(req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 357)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 358) req.id = rsp->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 359) a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 360) &req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 361)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 362) skb_pull(skb, sizeof(*rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 363) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 364) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 365)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 366) static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 367) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 368) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 369) struct a2mp_amp_assoc_req *req = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 370) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 371) struct amp_mgr *tmp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 372)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 373) if (le16_to_cpu(hdr->len) < sizeof(*req))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 374) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 375)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 376) BT_DBG("id %d", req->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 377)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 378) /* Make sure that other request is not processed */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 379) tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 380)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 381) hdev = hci_dev_get(req->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 382) if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 383) struct a2mp_amp_assoc_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 384)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 385) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 386) rsp.id = req->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 387)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 388) if (tmp) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 389) rsp.status = A2MP_STATUS_COLLISION_OCCURED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 390) amp_mgr_put(tmp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 391) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 392) rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 393) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 394)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 395) a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 396) &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 397)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 398) goto done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 399) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 400)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 401) amp_read_loc_assoc(hdev, mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 402)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 403) done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 404) if (hdev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 405) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 406)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 407) skb_pull(skb, sizeof(*req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 408) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 409) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 410)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 411) static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 412) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 413) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 414) struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 415) u16 len = le16_to_cpu(hdr->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 416) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 417) struct amp_ctrl *ctrl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 418) struct hci_conn *hcon;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 419) size_t assoc_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 420)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 421) if (len < sizeof(*rsp))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 422) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 423)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 424) assoc_len = len - sizeof(*rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 425)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 426) BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 427) assoc_len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 428)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 429) if (rsp->status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 430) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 431)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 432) /* Save remote ASSOC data */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 433) ctrl = amp_ctrl_lookup(mgr, rsp->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 434) if (ctrl) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 435) u8 *assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 436)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 437) assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 438) if (!assoc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 439) amp_ctrl_put(ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 440) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 441) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 442)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 443) ctrl->assoc = assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 444) ctrl->assoc_len = assoc_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 445) ctrl->assoc_rem_len = assoc_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 446) ctrl->assoc_len_so_far = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 447)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 448) amp_ctrl_put(ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 449) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 450)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 451) /* Create Phys Link */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 452) hdev = hci_dev_get(rsp->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 453) if (!hdev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 454) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 455)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 456) hcon = phylink_add(hdev, mgr, rsp->id, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 457) if (!hcon)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 458) goto done;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 459)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 460) BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 461)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 462) mgr->bredr_chan->remote_amp_id = rsp->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 463)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 464) amp_create_phylink(hdev, mgr, hcon);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 465)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 466) done:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 467) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 468) skb_pull(skb, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 469) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 470) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 471)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 472) static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 473) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 474) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 475) struct a2mp_physlink_req *req = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 476) struct a2mp_physlink_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 477) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 478) struct hci_conn *hcon;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 479) struct amp_ctrl *ctrl;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 480)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 481) if (le16_to_cpu(hdr->len) < sizeof(*req))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 482) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 483)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 484) BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 485)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 486) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 487)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 488) rsp.local_id = req->remote_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 489) rsp.remote_id = req->local_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 490)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 491) hdev = hci_dev_get(req->remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 492) if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 493) rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 494) goto send_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 495) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 496)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 497) ctrl = amp_ctrl_lookup(mgr, rsp.remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 498) if (!ctrl) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 499) ctrl = amp_ctrl_add(mgr, rsp.remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 500) if (ctrl) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 501) amp_ctrl_get(ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 502) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 503) rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 504) goto send_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 505) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 506) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 507)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 508) if (ctrl) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 509) size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 510) u8 *assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 511)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 512) assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 513) if (!assoc) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 514) amp_ctrl_put(ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 515) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 516) return -ENOMEM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 517) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 518)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 519) ctrl->assoc = assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 520) ctrl->assoc_len = assoc_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 521) ctrl->assoc_rem_len = assoc_len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 522) ctrl->assoc_len_so_far = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 523)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 524) amp_ctrl_put(ctrl);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 525) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 526)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 527) hcon = phylink_add(hdev, mgr, req->local_id, false);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 528) if (hcon) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 529) amp_accept_phylink(hdev, mgr, hcon);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 530) rsp.status = A2MP_STATUS_SUCCESS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 531) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 532) rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 533) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 534)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 535) send_rsp:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 536) if (hdev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 537) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 538)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 539) /* Reply error now and success after HCI Write Remote AMP Assoc
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 540) command complete with success status
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 541) */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 542) if (rsp.status != A2MP_STATUS_SUCCESS) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 543) a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 544) sizeof(rsp), &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 545) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 546) set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 547) mgr->ident = hdr->ident;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 548) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 549)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 550) skb_pull(skb, le16_to_cpu(hdr->len));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 551) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 552) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 553)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 554) static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 555) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 556) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 557) struct a2mp_physlink_req *req = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 558) struct a2mp_physlink_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 559) struct hci_dev *hdev;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 560) struct hci_conn *hcon;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 561)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 562) if (le16_to_cpu(hdr->len) < sizeof(*req))
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 563) return -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 564)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 565) BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 566)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 567) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 568)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 569) rsp.local_id = req->remote_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 570) rsp.remote_id = req->local_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 571) rsp.status = A2MP_STATUS_SUCCESS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 572)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 573) hdev = hci_dev_get(req->remote_id);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 574) if (!hdev) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 575) rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 576) goto send_rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 577) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 578)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 579) hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 580) &mgr->l2cap_conn->hcon->dst);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 581) if (!hcon) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 582) bt_dev_err(hdev, "no phys link exist");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 583) rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 584) goto clean;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 585) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 586)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 587) /* TODO Disconnect Phys Link here */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 588)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 589) clean:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 590) hci_dev_put(hdev);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 591)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 592) send_rsp:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 593) a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 594)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 595) skb_pull(skb, sizeof(*req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 596) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 597) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 598)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 599) static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 600) struct a2mp_cmd *hdr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 601) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 602) BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 603)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 604) skb_pull(skb, le16_to_cpu(hdr->len));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 605) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 606) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 607)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 608) /* Handle A2MP signalling */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 609) static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 610) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 611) struct a2mp_cmd *hdr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 612) struct amp_mgr *mgr = chan->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 613) int err = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 614)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 615) amp_mgr_get(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 616)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 617) while (skb->len >= sizeof(*hdr)) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 618) u16 len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 619)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 620) hdr = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 621) len = le16_to_cpu(hdr->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 622)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 623) BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 624)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 625) skb_pull(skb, sizeof(*hdr));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 626)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 627) if (len > skb->len || !hdr->ident) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 628) err = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 629) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 630) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 631)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 632) mgr->ident = hdr->ident;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 633)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 634) switch (hdr->code) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 635) case A2MP_COMMAND_REJ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 636) a2mp_command_rej(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 637) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 638)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 639) case A2MP_DISCOVER_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 640) err = a2mp_discover_req(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 641) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 642)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 643) case A2MP_CHANGE_NOTIFY:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 644) err = a2mp_change_notify(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 645) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 646)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 647) case A2MP_GETINFO_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 648) err = a2mp_getinfo_req(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 649) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 650)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 651) case A2MP_GETAMPASSOC_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 652) err = a2mp_getampassoc_req(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 653) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 654)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 655) case A2MP_CREATEPHYSLINK_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 656) err = a2mp_createphyslink_req(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 657) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 658)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 659) case A2MP_DISCONNPHYSLINK_REQ:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 660) err = a2mp_discphyslink_req(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 661) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 662)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 663) case A2MP_DISCOVER_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 664) err = a2mp_discover_rsp(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 665) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 666)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 667) case A2MP_GETINFO_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 668) err = a2mp_getinfo_rsp(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 669) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 670)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 671) case A2MP_GETAMPASSOC_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 672) err = a2mp_getampassoc_rsp(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 673) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 674)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 675) case A2MP_CHANGE_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 676) case A2MP_CREATEPHYSLINK_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 677) case A2MP_DISCONNPHYSLINK_RSP:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 678) err = a2mp_cmd_rsp(mgr, skb, hdr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 679) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 680)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 681) default:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 682) BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 683) err = -EINVAL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 684) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 685) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 686) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 687)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 688) if (err) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 689) struct a2mp_cmd_rej rej;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 690)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 691) memset(&rej, 0, sizeof(rej));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 692)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 693) rej.reason = cpu_to_le16(0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 694) hdr = (void *) skb->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 695)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 696) BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 697)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 698) a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej),
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 699) &rej);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 700) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 701)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 702) /* Always free skb and return success error code to prevent
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 703) from sending L2CAP Disconnect over A2MP channel */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 704) kfree_skb(skb);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 705)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 706) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 707)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 708) return 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 709) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 710)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 711) static void a2mp_chan_close_cb(struct l2cap_chan *chan)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 712) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 713) l2cap_chan_put(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 714) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 715)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 716) static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 717) int err)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 718) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 719) struct amp_mgr *mgr = chan->data;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 720)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 721) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 722) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 723)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 724) BT_DBG("chan %p state %s", chan, state_to_string(state));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 725)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 726) chan->state = state;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 727)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 728) switch (state) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 729) case BT_CLOSED:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 730) if (mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 731) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 732) break;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 733) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 734) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 735)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 736) static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 737) unsigned long hdr_len,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 738) unsigned long len, int nb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 739) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 740) struct sk_buff *skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 741)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 742) skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 743) if (!skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 744) return ERR_PTR(-ENOMEM);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 745)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 746) return skb;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 747) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 748)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 749) static const struct l2cap_ops a2mp_chan_ops = {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 750) .name = "L2CAP A2MP channel",
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 751) .recv = a2mp_chan_recv_cb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 752) .close = a2mp_chan_close_cb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 753) .state_change = a2mp_chan_state_change_cb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 754) .alloc_skb = a2mp_chan_alloc_skb_cb,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 755)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 756) /* Not implemented for A2MP */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 757) .new_connection = l2cap_chan_no_new_connection,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 758) .teardown = l2cap_chan_no_teardown,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 759) .ready = l2cap_chan_no_ready,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 760) .defer = l2cap_chan_no_defer,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 761) .resume = l2cap_chan_no_resume,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 762) .set_shutdown = l2cap_chan_no_set_shutdown,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 763) .get_sndtimeo = l2cap_chan_no_get_sndtimeo,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 764) };
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 765)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 766) static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 767) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 768) struct l2cap_chan *chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 769) int err;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 770)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 771) chan = l2cap_chan_create();
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 772) if (!chan)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 773) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 774)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 775) BT_DBG("chan %p", chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 776)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 777) chan->chan_type = L2CAP_CHAN_FIXED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 778) chan->scid = L2CAP_CID_A2MP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 779) chan->dcid = L2CAP_CID_A2MP;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 780) chan->omtu = L2CAP_A2MP_DEFAULT_MTU;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 781) chan->imtu = L2CAP_A2MP_DEFAULT_MTU;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 782) chan->flush_to = L2CAP_DEFAULT_FLUSH_TO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 783)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 784) chan->ops = &a2mp_chan_ops;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 785)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 786) l2cap_chan_set_defaults(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 787) chan->remote_max_tx = chan->max_tx;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 788) chan->remote_tx_win = chan->tx_win;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 789)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 790) chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 791) chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 792)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 793) skb_queue_head_init(&chan->tx_q);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 794)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 795) chan->mode = L2CAP_MODE_ERTM;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 796)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 797) err = l2cap_ertm_init(chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 798) if (err < 0) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 799) l2cap_chan_del(chan, 0);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 800) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 801) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 802)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 803) chan->conf_state = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 804)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 805) if (locked)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 806) __l2cap_chan_add(conn, chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 807) else
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 808) l2cap_chan_add(conn, chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 809)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 810) chan->remote_mps = chan->omtu;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 811) chan->mps = chan->omtu;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 812)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 813) chan->state = BT_CONNECTED;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 814)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 815) return chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 816) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 817)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 818) /* AMP Manager functions */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 819) struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 820) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 821) BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 822)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 823) kref_get(&mgr->kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 824)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 825) return mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 826) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 827)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 828) static void amp_mgr_destroy(struct kref *kref)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 829) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 830) struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 831)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 832) BT_DBG("mgr %p", mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 833)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 834) mutex_lock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 835) list_del(&mgr->list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 836) mutex_unlock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 837)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 838) amp_ctrl_list_flush(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 839) kfree(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 840) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 841)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 842) int amp_mgr_put(struct amp_mgr *mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 843) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 844) BT_DBG("mgr %p orig refcnt %d", mgr, kref_read(&mgr->kref));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 845)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 846) return kref_put(&mgr->kref, &_mgr_destroy);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 847) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 848)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 849) static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 850) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 851) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 852) struct l2cap_chan *chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 853)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 854) mgr = kzalloc(sizeof(*mgr), GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 855) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 856) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 857)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 858) BT_DBG("conn %p mgr %p", conn, mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 859)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 860) mgr->l2cap_conn = conn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 861)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 862) chan = a2mp_chan_open(conn, locked);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 863) if (!chan) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 864) kfree(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 865) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 866) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 867)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 868) mgr->a2mp_chan = chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 869) chan->data = mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 870)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 871) conn->hcon->amp_mgr = mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 872)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 873) kref_init(&mgr->kref);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 874)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 875) /* Remote AMP ctrl list initialization */
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 876) INIT_LIST_HEAD(&mgr->amp_ctrls);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 877) mutex_init(&mgr->amp_ctrls_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 878)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 879) mutex_lock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 880) list_add(&mgr->list, &_mgr_list);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 881) mutex_unlock(&_mgr_list_lock);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 882)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 883) return mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 884) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 885)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 886) struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 887) struct sk_buff *skb)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 888) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 889) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 890)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 891) if (conn->hcon->type != ACL_LINK)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 892) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 893)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 894) mgr = amp_mgr_create(conn, false);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 895) if (!mgr) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 896) BT_ERR("Could not create AMP manager");
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 897) return NULL;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 898) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 899)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 900) BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 901)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 902) return mgr->a2mp_chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 903) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 904)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 905) void a2mp_send_getinfo_rsp(struct hci_dev *hdev)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 906) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 907) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 908) struct a2mp_info_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 909)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 910) mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 911) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 912) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 913)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 914) BT_DBG("%s mgr %p", hdev->name, mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 915)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 916) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 917)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 918) rsp.id = hdev->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 919) rsp.status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 920)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 921) if (hdev->amp_type != AMP_TYPE_BREDR) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 922) rsp.status = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 923) rsp.total_bw = cpu_to_le32(hdev->amp_total_bw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 924) rsp.max_bw = cpu_to_le32(hdev->amp_max_bw);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 925) rsp.min_latency = cpu_to_le32(hdev->amp_min_latency);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 926) rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 927) rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 928) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 929)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 930) a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 931) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 932) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 933)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 934) void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 935) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 936) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 937) struct amp_assoc *loc_assoc = &hdev->loc_assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 938) struct a2mp_amp_assoc_rsp *rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 939) size_t len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 940)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 941) mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 942) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 943) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 944)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 945) BT_DBG("%s mgr %p", hdev->name, mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 946)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 947) len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 948) rsp = kzalloc(len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 949) if (!rsp) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 950) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 951) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 952) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 953)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 954) rsp->id = hdev->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 955)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 956) if (status) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 957) rsp->status = A2MP_STATUS_INVALID_CTRL_ID;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 958) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 959) rsp->status = A2MP_STATUS_SUCCESS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 960) memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 961) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 962)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 963) a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 964) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 965) kfree(rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 966) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 967)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 968) void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 969) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 970) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 971) struct amp_assoc *loc_assoc = &hdev->loc_assoc;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 972) struct a2mp_physlink_req *req;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 973) struct l2cap_chan *bredr_chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 974) size_t len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 975)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 976) mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 977) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 978) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 979)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 980) len = sizeof(*req) + loc_assoc->len;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 981)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 982) BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 983)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 984) req = kzalloc(len, GFP_KERNEL);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 985) if (!req) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 986) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 987) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 988) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 989)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 990) bredr_chan = mgr->bredr_chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 991) if (!bredr_chan)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 992) goto clean;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 993)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 994) req->local_id = hdev->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 995) req->remote_id = bredr_chan->remote_amp_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 996) memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 997)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 998) a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 999)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1000) clean:
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1001) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1002) kfree(req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1003) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1004)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1005) void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1006) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1007) struct amp_mgr *mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1008) struct a2mp_physlink_rsp rsp;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1009) struct hci_conn *hs_hcon;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1010)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1011) mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1012) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1013) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1014)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1015) memset(&rsp, 0, sizeof(rsp));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1016)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1017) hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1018) if (!hs_hcon) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1019) rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1020) } else {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1021) rsp.remote_id = hs_hcon->remote_id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1022) rsp.status = A2MP_STATUS_SUCCESS;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1023) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1024)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1025) BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon,
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1026) status);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1027)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1028) rsp.local_id = hdev->id;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1029) a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1030) amp_mgr_put(mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1031) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1032)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1033) void a2mp_discover_amp(struct l2cap_chan *chan)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1034) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1035) struct l2cap_conn *conn = chan->conn;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1036) struct amp_mgr *mgr = conn->hcon->amp_mgr;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1037) struct a2mp_discov_req req;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1038)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1039) BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1040)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1041) if (!mgr) {
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1042) mgr = amp_mgr_create(conn, true);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1043) if (!mgr)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1044) return;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1045) }
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1046)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1047) mgr->bredr_chan = chan;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1048)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1049) memset(&req, 0, sizeof(req));
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1050)
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1051) req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1052) req.ext_feat = 0;
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1053) a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req);
^8f3ce5b39 (kx 2023-10-28 12:00:06 +0300 1054) }
Orange Pi5 kernel
Deprecated Linux kernel 5.10.110 for OrangePi 5/5B/5+ boards
3 Commits
0 Branches
0 Tags