1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 BlueZ - Bluetooth protocol stack for Linux 4 5 Copyright (C) 2015 Intel Corporation 6 7 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 8 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 9 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 10 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 11 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 12 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 13 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 14 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 15 16 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 17 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 18 SOFTWARE IS DISCLAIMED. 19 */ 20 21 #include <linux/unaligned.h> 22 23 #include <net/bluetooth/bluetooth.h> 24 #include <net/bluetooth/hci_core.h> 25 #include <net/bluetooth/hci_mon.h> 26 #include <net/bluetooth/mgmt.h> 27 28 #include "mgmt_util.h" 29 30 static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie, 31 u16 opcode, u16 len, void *buf) 32 { 33 struct hci_mon_hdr *hdr; 34 struct sk_buff *skb; 35 36 skb = bt_skb_alloc(6 + len, GFP_ATOMIC); 37 if (!skb) 38 return NULL; 39 40 put_unaligned_le32(cookie, skb_put(skb, 4)); 41 put_unaligned_le16(opcode, skb_put(skb, 2)); 42 43 if (buf) 44 skb_put_data(skb, buf, len); 45 46 __net_timestamp(skb); 47 48 hdr = skb_push(skb, HCI_MON_HDR_SIZE); 49 hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT); 50 hdr->index = index; 51 hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE); 52 53 return skb; 54 } 55 56 struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, 57 unsigned int size) 58 { 59 struct sk_buff *skb; 60 61 skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL); 62 if (!skb) 63 return skb; 64 65 skb_reserve(skb, sizeof(struct mgmt_hdr)); 66 bt_cb(skb)->mgmt.hdev = hdev; 67 bt_cb(skb)->mgmt.opcode = opcode; 68 69 return skb; 70 } 71 72 int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, 73 struct sock *skip_sk) 74 { 75 struct hci_dev *hdev; 76 struct mgmt_hdr *hdr; 77 int len; 78 79 if (!skb) 80 return -EINVAL; 81 82 len = skb->len; 83 hdev = bt_cb(skb)->mgmt.hdev; 84 85 /* Time stamp */ 86 __net_timestamp(skb); 87 88 /* Send just the data, without headers, to the monitor */ 89 if (channel == HCI_CHANNEL_CONTROL) 90 hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode, 91 skb->data, skb->len, 92 skb_get_ktime(skb), flag, skip_sk); 93 94 hdr = skb_push(skb, sizeof(*hdr)); 95 hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode); 96 if (hdev) 97 hdr->index = cpu_to_le16(hdev->id); 98 else 99 hdr->index = cpu_to_le16(MGMT_INDEX_NONE); 100 hdr->len = cpu_to_le16(len); 101 102 hci_send_to_channel(channel, skb, flag, skip_sk); 103 104 kfree_skb(skb); 105 return 0; 106 } 107 108 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 109 void *data, u16 data_len, int flag, struct sock *skip_sk) 110 { 111 struct sk_buff *skb; 112 113 skb = mgmt_alloc_skb(hdev, event, data_len); 114 if (!skb) 115 return -ENOMEM; 116 117 if (data) 118 skb_put_data(skb, data, data_len); 119 120 return mgmt_send_event_skb(channel, skb, flag, skip_sk); 121 } 122 123 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 124 { 125 struct sk_buff *skb, *mskb; 126 struct mgmt_hdr *hdr; 127 struct mgmt_ev_cmd_status *ev; 128 int err; 129 130 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); 131 132 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); 133 if (!skb) 134 return -ENOMEM; 135 136 hdr = skb_put(skb, sizeof(*hdr)); 137 138 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); 139 hdr->index = cpu_to_le16(index); 140 hdr->len = cpu_to_le16(sizeof(*ev)); 141 142 ev = skb_put(skb, sizeof(*ev)); 143 ev->status = status; 144 ev->opcode = cpu_to_le16(cmd); 145 146 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 147 MGMT_EV_CMD_STATUS, sizeof(*ev), ev); 148 if (mskb) 149 skb->tstamp = mskb->tstamp; 150 else 151 __net_timestamp(skb); 152 153 err = sock_queue_rcv_skb(sk, skb); 154 if (err < 0) 155 kfree_skb(skb); 156 157 if (mskb) { 158 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 159 HCI_SOCK_TRUSTED, NULL); 160 kfree_skb(mskb); 161 } 162 163 return err; 164 } 165 166 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 167 void *rp, size_t rp_len) 168 { 169 struct sk_buff *skb, *mskb; 170 struct mgmt_hdr *hdr; 171 struct mgmt_ev_cmd_complete *ev; 172 int err; 173 174 BT_DBG("sock %p", sk); 175 176 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); 177 if (!skb) 178 return -ENOMEM; 179 180 hdr = skb_put(skb, sizeof(*hdr)); 181 182 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); 183 hdr->index = cpu_to_le16(index); 184 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); 185 186 ev = skb_put(skb, sizeof(*ev) + rp_len); 187 ev->opcode = cpu_to_le16(cmd); 188 ev->status = status; 189 190 if (rp) 191 memcpy(ev->data, rp, rp_len); 192 193 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 194 MGMT_EV_CMD_COMPLETE, 195 sizeof(*ev) + rp_len, ev); 196 if (mskb) 197 skb->tstamp = mskb->tstamp; 198 else 199 __net_timestamp(skb); 200 201 err = sock_queue_rcv_skb(sk, skb); 202 if (err < 0) 203 kfree_skb(skb); 204 205 if (mskb) { 206 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 207 HCI_SOCK_TRUSTED, NULL); 208 kfree_skb(mskb); 209 } 210 211 return err; 212 } 213 214 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, 215 struct hci_dev *hdev) 216 { 217 struct mgmt_pending_cmd *cmd, *tmp; 218 219 mutex_lock(&hdev->mgmt_pending_lock); 220 221 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 222 if (hci_sock_get_channel(cmd->sk) != channel) 223 continue; 224 225 if (cmd->opcode == opcode) { 226 mutex_unlock(&hdev->mgmt_pending_lock); 227 return cmd; 228 } 229 } 230 231 mutex_unlock(&hdev->mgmt_pending_lock); 232 233 return NULL; 234 } 235 236 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, bool remove, 237 void (*cb)(struct mgmt_pending_cmd *cmd, void *data), 238 void *data) 239 { 240 struct mgmt_pending_cmd *cmd, *tmp; 241 242 mutex_lock(&hdev->mgmt_pending_lock); 243 244 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 245 if (opcode > 0 && cmd->opcode != opcode) 246 continue; 247 248 if (remove) 249 list_del(&cmd->list); 250 251 cb(cmd, data); 252 253 if (remove) 254 mgmt_pending_free(cmd); 255 } 256 257 mutex_unlock(&hdev->mgmt_pending_lock); 258 } 259 260 struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode, 261 struct hci_dev *hdev, 262 void *data, u16 len) 263 { 264 struct mgmt_pending_cmd *cmd; 265 266 cmd = kzalloc_obj(*cmd); 267 if (!cmd) 268 return NULL; 269 270 cmd->opcode = opcode; 271 cmd->hdev = hdev; 272 273 cmd->param = kmemdup(data, len, GFP_KERNEL); 274 if (!cmd->param) { 275 kfree(cmd); 276 return NULL; 277 } 278 279 cmd->param_len = len; 280 281 cmd->sk = sk; 282 sock_hold(sk); 283 284 return cmd; 285 } 286 287 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, 288 struct hci_dev *hdev, 289 void *data, u16 len) 290 { 291 struct mgmt_pending_cmd *cmd; 292 293 cmd = mgmt_pending_new(sk, opcode, hdev, data, len); 294 if (!cmd) 295 return NULL; 296 297 mutex_lock(&hdev->mgmt_pending_lock); 298 list_add_tail(&cmd->list, &hdev->mgmt_pending); 299 mutex_unlock(&hdev->mgmt_pending_lock); 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 mutex_lock(&cmd->hdev->mgmt_pending_lock); 314 list_del(&cmd->list); 315 mutex_unlock(&cmd->hdev->mgmt_pending_lock); 316 317 mgmt_pending_free(cmd); 318 } 319 320 bool __mgmt_pending_listed(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd) 321 { 322 struct mgmt_pending_cmd *tmp; 323 324 lockdep_assert_held(&hdev->mgmt_pending_lock); 325 326 if (!cmd) 327 return false; 328 329 list_for_each_entry(tmp, &hdev->mgmt_pending, list) { 330 if (cmd == tmp) 331 return true; 332 } 333 334 return false; 335 } 336 337 bool mgmt_pending_listed(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd) 338 { 339 bool listed; 340 341 mutex_lock(&hdev->mgmt_pending_lock); 342 listed = __mgmt_pending_listed(hdev, cmd); 343 mutex_unlock(&hdev->mgmt_pending_lock); 344 345 return listed; 346 } 347 348 bool mgmt_pending_valid(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd) 349 { 350 bool listed; 351 352 if (!cmd) 353 return false; 354 355 mutex_lock(&hdev->mgmt_pending_lock); 356 357 listed = __mgmt_pending_listed(hdev, cmd); 358 if (listed) 359 list_del(&cmd->list); 360 361 mutex_unlock(&hdev->mgmt_pending_lock); 362 363 return listed; 364 } 365 366 void mgmt_mesh_foreach(struct hci_dev *hdev, 367 void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data), 368 void *data, struct sock *sk) 369 { 370 struct mgmt_mesh_tx *mesh_tx, *tmp; 371 372 list_for_each_entry_safe(mesh_tx, tmp, &hdev->mesh_pending, list) { 373 if (!sk || mesh_tx->sk == sk) 374 cb(mesh_tx, data); 375 } 376 } 377 378 struct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk) 379 { 380 struct mgmt_mesh_tx *mesh_tx; 381 382 if (list_empty(&hdev->mesh_pending)) 383 return NULL; 384 385 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { 386 if (!sk || mesh_tx->sk == sk) 387 return mesh_tx; 388 } 389 390 return NULL; 391 } 392 393 struct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle) 394 { 395 struct mgmt_mesh_tx *mesh_tx; 396 397 if (list_empty(&hdev->mesh_pending)) 398 return NULL; 399 400 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { 401 if (mesh_tx->handle == handle) 402 return mesh_tx; 403 } 404 405 return NULL; 406 } 407 408 struct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev, 409 void *data, u16 len) 410 { 411 struct mgmt_mesh_tx *mesh_tx; 412 413 mesh_tx = kzalloc_obj(*mesh_tx); 414 if (!mesh_tx) 415 return NULL; 416 417 hdev->mesh_send_ref++; 418 if (!hdev->mesh_send_ref) 419 hdev->mesh_send_ref++; 420 421 mesh_tx->handle = hdev->mesh_send_ref; 422 mesh_tx->index = hdev->id; 423 memcpy(mesh_tx->param, data, len); 424 mesh_tx->param_len = len; 425 mesh_tx->sk = sk; 426 sock_hold(sk); 427 428 list_add_tail(&mesh_tx->list, &hdev->mesh_pending); 429 430 return mesh_tx; 431 } 432 433 void mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx) 434 { 435 list_del(&mesh_tx->list); 436 sock_put(mesh_tx->sk); 437 kfree(mesh_tx); 438 } 439