1 /* 2 BlueZ - Bluetooth protocol stack for Linux 3 4 Copyright (C) 2015 Intel Corporation 5 6 This program is free software; you can redistribute it and/or modify 7 it under the terms of the GNU General Public License version 2 as 8 published by the Free Software Foundation; 9 10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 11 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 12 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 13 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 14 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 15 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18 19 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 20 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 21 SOFTWARE IS DISCLAIMED. 22 */ 23 24 #include <asm/unaligned.h> 25 26 #include <net/bluetooth/bluetooth.h> 27 #include <net/bluetooth/hci_core.h> 28 #include <net/bluetooth/hci_mon.h> 29 #include <net/bluetooth/mgmt.h> 30 31 #include "mgmt_util.h" 32 33 static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie, 34 u16 opcode, u16 len, void *buf) 35 { 36 struct hci_mon_hdr *hdr; 37 struct sk_buff *skb; 38 39 skb = bt_skb_alloc(6 + len, GFP_ATOMIC); 40 if (!skb) 41 return NULL; 42 43 put_unaligned_le32(cookie, skb_put(skb, 4)); 44 put_unaligned_le16(opcode, skb_put(skb, 2)); 45 46 if (buf) 47 skb_put_data(skb, buf, len); 48 49 __net_timestamp(skb); 50 51 hdr = skb_push(skb, HCI_MON_HDR_SIZE); 52 hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT); 53 hdr->index = index; 54 hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE); 55 56 return skb; 57 } 58 59 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 60 void *data, u16 data_len, int flag, struct sock *skip_sk) 61 { 62 struct sk_buff *skb; 63 struct mgmt_hdr *hdr; 64 65 skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL); 66 if (!skb) 67 return -ENOMEM; 68 69 hdr = skb_put(skb, sizeof(*hdr)); 70 hdr->opcode = cpu_to_le16(event); 71 if (hdev) 72 hdr->index = cpu_to_le16(hdev->id); 73 else 74 hdr->index = cpu_to_le16(MGMT_INDEX_NONE); 75 hdr->len = cpu_to_le16(data_len); 76 77 if (data) 78 skb_put_data(skb, data, data_len); 79 80 /* Time stamp */ 81 __net_timestamp(skb); 82 83 hci_send_to_channel(channel, skb, flag, skip_sk); 84 85 if (channel == HCI_CHANNEL_CONTROL) 86 hci_send_monitor_ctrl_event(hdev, event, data, data_len, 87 skb_get_ktime(skb), flag, skip_sk); 88 89 kfree_skb(skb); 90 return 0; 91 } 92 93 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 94 { 95 struct sk_buff *skb, *mskb; 96 struct mgmt_hdr *hdr; 97 struct mgmt_ev_cmd_status *ev; 98 int err; 99 100 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); 101 102 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); 103 if (!skb) 104 return -ENOMEM; 105 106 hdr = skb_put(skb, sizeof(*hdr)); 107 108 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); 109 hdr->index = cpu_to_le16(index); 110 hdr->len = cpu_to_le16(sizeof(*ev)); 111 112 ev = skb_put(skb, sizeof(*ev)); 113 ev->status = status; 114 ev->opcode = cpu_to_le16(cmd); 115 116 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 117 MGMT_EV_CMD_STATUS, sizeof(*ev), ev); 118 if (mskb) 119 skb->tstamp = mskb->tstamp; 120 else 121 __net_timestamp(skb); 122 123 err = sock_queue_rcv_skb(sk, skb); 124 if (err < 0) 125 kfree_skb(skb); 126 127 if (mskb) { 128 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 129 HCI_SOCK_TRUSTED, NULL); 130 kfree_skb(mskb); 131 } 132 133 return err; 134 } 135 136 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 137 void *rp, size_t rp_len) 138 { 139 struct sk_buff *skb, *mskb; 140 struct mgmt_hdr *hdr; 141 struct mgmt_ev_cmd_complete *ev; 142 int err; 143 144 BT_DBG("sock %p", sk); 145 146 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); 147 if (!skb) 148 return -ENOMEM; 149 150 hdr = skb_put(skb, sizeof(*hdr)); 151 152 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); 153 hdr->index = cpu_to_le16(index); 154 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); 155 156 ev = skb_put(skb, sizeof(*ev) + rp_len); 157 ev->opcode = cpu_to_le16(cmd); 158 ev->status = status; 159 160 if (rp) 161 memcpy(ev->data, rp, rp_len); 162 163 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 164 MGMT_EV_CMD_COMPLETE, 165 sizeof(*ev) + rp_len, ev); 166 if (mskb) 167 skb->tstamp = mskb->tstamp; 168 else 169 __net_timestamp(skb); 170 171 err = sock_queue_rcv_skb(sk, skb); 172 if (err < 0) 173 kfree_skb(skb); 174 175 if (mskb) { 176 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 177 HCI_SOCK_TRUSTED, NULL); 178 kfree_skb(mskb); 179 } 180 181 return err; 182 } 183 184 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, 185 struct hci_dev *hdev) 186 { 187 struct mgmt_pending_cmd *cmd; 188 189 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 190 if (hci_sock_get_channel(cmd->sk) != channel) 191 continue; 192 if (cmd->opcode == opcode) 193 return cmd; 194 } 195 196 return NULL; 197 } 198 199 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, 200 u16 opcode, 201 struct hci_dev *hdev, 202 const void *data) 203 { 204 struct mgmt_pending_cmd *cmd; 205 206 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 207 if (cmd->user_data != data) 208 continue; 209 if (cmd->opcode == opcode) 210 return cmd; 211 } 212 213 return NULL; 214 } 215 216 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, 217 void (*cb)(struct mgmt_pending_cmd *cmd, void *data), 218 void *data) 219 { 220 struct mgmt_pending_cmd *cmd, *tmp; 221 222 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 223 if (opcode > 0 && cmd->opcode != opcode) 224 continue; 225 226 cb(cmd, data); 227 } 228 } 229 230 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, 231 struct hci_dev *hdev, 232 void *data, u16 len) 233 { 234 struct mgmt_pending_cmd *cmd; 235 236 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 237 if (!cmd) 238 return NULL; 239 240 cmd->opcode = opcode; 241 cmd->index = hdev->id; 242 243 cmd->param = kmemdup(data, len, GFP_KERNEL); 244 if (!cmd->param) { 245 kfree(cmd); 246 return NULL; 247 } 248 249 cmd->param_len = len; 250 251 cmd->sk = sk; 252 sock_hold(sk); 253 254 list_add(&cmd->list, &hdev->mgmt_pending); 255 256 return cmd; 257 } 258 259 void mgmt_pending_free(struct mgmt_pending_cmd *cmd) 260 { 261 sock_put(cmd->sk); 262 kfree(cmd->param); 263 kfree(cmd); 264 } 265 266 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) 267 { 268 list_del(&cmd->list); 269 mgmt_pending_free(cmd); 270 } 271