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 struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, 60 unsigned int size) 61 { 62 struct sk_buff *skb; 63 64 skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL); 65 if (!skb) 66 return skb; 67 68 skb_reserve(skb, sizeof(struct mgmt_hdr)); 69 bt_cb(skb)->mgmt.hdev = hdev; 70 bt_cb(skb)->mgmt.opcode = opcode; 71 72 return skb; 73 } 74 75 int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, 76 struct sock *skip_sk) 77 { 78 struct hci_dev *hdev; 79 struct mgmt_hdr *hdr; 80 int len = skb->len; 81 82 if (!skb) 83 return -EINVAL; 84 85 hdev = bt_cb(skb)->mgmt.hdev; 86 87 /* Time stamp */ 88 __net_timestamp(skb); 89 90 /* Send just the data, without headers, to the monitor */ 91 if (channel == HCI_CHANNEL_CONTROL) 92 hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode, 93 skb->data, skb->len, 94 skb_get_ktime(skb), flag, skip_sk); 95 96 hdr = skb_push(skb, sizeof(*hdr)); 97 hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode); 98 if (hdev) 99 hdr->index = cpu_to_le16(hdev->id); 100 else 101 hdr->index = cpu_to_le16(MGMT_INDEX_NONE); 102 hdr->len = cpu_to_le16(len); 103 104 hci_send_to_channel(channel, skb, flag, skip_sk); 105 106 kfree_skb(skb); 107 return 0; 108 } 109 110 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 111 void *data, u16 data_len, int flag, struct sock *skip_sk) 112 { 113 struct sk_buff *skb; 114 115 skb = mgmt_alloc_skb(hdev, event, data_len); 116 if (!skb) 117 return -ENOMEM; 118 119 if (data) 120 skb_put_data(skb, data, data_len); 121 122 return mgmt_send_event_skb(channel, skb, flag, skip_sk); 123 } 124 125 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 126 { 127 struct sk_buff *skb, *mskb; 128 struct mgmt_hdr *hdr; 129 struct mgmt_ev_cmd_status *ev; 130 int err; 131 132 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); 133 134 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); 135 if (!skb) 136 return -ENOMEM; 137 138 hdr = skb_put(skb, sizeof(*hdr)); 139 140 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); 141 hdr->index = cpu_to_le16(index); 142 hdr->len = cpu_to_le16(sizeof(*ev)); 143 144 ev = skb_put(skb, sizeof(*ev)); 145 ev->status = status; 146 ev->opcode = cpu_to_le16(cmd); 147 148 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 149 MGMT_EV_CMD_STATUS, sizeof(*ev), ev); 150 if (mskb) 151 skb->tstamp = mskb->tstamp; 152 else 153 __net_timestamp(skb); 154 155 err = sock_queue_rcv_skb(sk, skb); 156 if (err < 0) 157 kfree_skb(skb); 158 159 if (mskb) { 160 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 161 HCI_SOCK_TRUSTED, NULL); 162 kfree_skb(mskb); 163 } 164 165 return err; 166 } 167 168 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 169 void *rp, size_t rp_len) 170 { 171 struct sk_buff *skb, *mskb; 172 struct mgmt_hdr *hdr; 173 struct mgmt_ev_cmd_complete *ev; 174 int err; 175 176 BT_DBG("sock %p", sk); 177 178 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); 179 if (!skb) 180 return -ENOMEM; 181 182 hdr = skb_put(skb, sizeof(*hdr)); 183 184 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); 185 hdr->index = cpu_to_le16(index); 186 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); 187 188 ev = skb_put(skb, sizeof(*ev) + rp_len); 189 ev->opcode = cpu_to_le16(cmd); 190 ev->status = status; 191 192 if (rp) 193 memcpy(ev->data, rp, rp_len); 194 195 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 196 MGMT_EV_CMD_COMPLETE, 197 sizeof(*ev) + rp_len, ev); 198 if (mskb) 199 skb->tstamp = mskb->tstamp; 200 else 201 __net_timestamp(skb); 202 203 err = sock_queue_rcv_skb(sk, skb); 204 if (err < 0) 205 kfree_skb(skb); 206 207 if (mskb) { 208 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 209 HCI_SOCK_TRUSTED, NULL); 210 kfree_skb(mskb); 211 } 212 213 return err; 214 } 215 216 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, 217 struct hci_dev *hdev) 218 { 219 struct mgmt_pending_cmd *cmd; 220 221 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 222 if (hci_sock_get_channel(cmd->sk) != channel) 223 continue; 224 if (cmd->opcode == opcode) 225 return cmd; 226 } 227 228 return NULL; 229 } 230 231 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, 232 u16 opcode, 233 struct hci_dev *hdev, 234 const void *data) 235 { 236 struct mgmt_pending_cmd *cmd; 237 238 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 239 if (cmd->user_data != data) 240 continue; 241 if (cmd->opcode == opcode) 242 return cmd; 243 } 244 245 return NULL; 246 } 247 248 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, 249 void (*cb)(struct mgmt_pending_cmd *cmd, void *data), 250 void *data) 251 { 252 struct mgmt_pending_cmd *cmd, *tmp; 253 254 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 255 if (opcode > 0 && cmd->opcode != opcode) 256 continue; 257 258 cb(cmd, data); 259 } 260 } 261 262 struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode, 263 struct hci_dev *hdev, 264 void *data, u16 len) 265 { 266 struct mgmt_pending_cmd *cmd; 267 268 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 269 if (!cmd) 270 return NULL; 271 272 cmd->opcode = opcode; 273 cmd->index = hdev->id; 274 275 cmd->param = kmemdup(data, len, GFP_KERNEL); 276 if (!cmd->param) { 277 kfree(cmd); 278 return NULL; 279 } 280 281 cmd->param_len = len; 282 283 cmd->sk = sk; 284 sock_hold(sk); 285 286 return cmd; 287 } 288 289 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, 290 struct hci_dev *hdev, 291 void *data, u16 len) 292 { 293 struct mgmt_pending_cmd *cmd; 294 295 cmd = mgmt_pending_new(sk, opcode, hdev, data, len); 296 if (!cmd) 297 return NULL; 298 299 list_add(&cmd->list, &hdev->mgmt_pending); 300 301 return cmd; 302 } 303 304 void mgmt_pending_free(struct mgmt_pending_cmd *cmd) 305 { 306 sock_put(cmd->sk); 307 kfree(cmd->param); 308 kfree(cmd); 309 } 310 311 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) 312 { 313 list_del(&cmd->list); 314 mgmt_pending_free(cmd); 315 } 316