1133819Stjr/* 2133819Stjr BlueZ - Bluetooth protocol stack for Linux 3133819Stjr 4133819Stjr Copyright (C) 2015 Intel Corporation 5133819Stjr 6133819Stjr This program is free software; you can redistribute it and/or modify 7159581Snetchild it under the terms of the GNU General Public License version 2 as 8133819Stjr published by the Free Software Foundation; 9146806Srwatson 10146806Srwatson THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 11146806Srwatson OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 12146806Srwatson FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 13146806Srwatson IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 14156842Snetchild CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 15133819Stjr WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 16133819Stjr ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 17133819Stjr OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 18133819Stjr 19133819Stjr ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 20133819Stjr COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 21133819Stjr SOFTWARE IS DISCLAIMED. 22133819Stjr*/ 23160797Sjhb 24133819Stjr#include <asm/unaligned.h> 25160797Sjhb 26133819Stjr#include <net/bluetooth/bluetooth.h> 27156874Sru#include <net/bluetooth/hci_core.h> 28133819Stjr#include <net/bluetooth/hci_mon.h> 29133819Stjr#include <net/bluetooth/mgmt.h> 30133819Stjr 31143197Ssobomax#include "mgmt_util.h" 32133819Stjr 33133819Stjrstatic struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie, 34133819Stjr u16 opcode, u16 len, void *buf) 35143197Ssobomax{ 36143197Ssobomax struct hci_mon_hdr *hdr; 37143197Ssobomax struct sk_buff *skb; 38133819Stjr 39133819Stjr skb = bt_skb_alloc(6 + len, GFP_ATOMIC); 40146806Srwatson if (!skb) 41160798Sjhb return NULL; 42146806Srwatson 43160798Sjhb put_unaligned_le32(cookie, skb_put(skb, 4)); 44160798Sjhb put_unaligned_le16(opcode, skb_put(skb, 2)); 45146806Srwatson 46160798Sjhb if (buf) 47146806Srwatson skb_put_data(skb, buf, len); 48160798Sjhb 49146806Srwatson __net_timestamp(skb); 50160798Sjhb 51160798Sjhb hdr = skb_push(skb, HCI_MON_HDR_SIZE); 52146806Srwatson hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT); 53165609Srwatson hdr->index = index; 54159581Snetchild hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE); 55160798Sjhb 56160798Sjhb return skb; 57160798Sjhb} 58146806Srwatson 59160798Sjhbstruct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode, 60160798Sjhb unsigned int size) 61160798Sjhb{ 62146806Srwatson struct sk_buff *skb; 63160798Sjhb 64146806Srwatson skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL); 65160798Sjhb if (!skb) 66146806Srwatson return skb; 67146806Srwatson 68160798Sjhb skb_reserve(skb, sizeof(struct mgmt_hdr)); 69159581Snetchild bt_cb(skb)->mgmt.hdev = hdev; 70160798Sjhb bt_cb(skb)->mgmt.opcode = opcode; 71146806Srwatson 72160798Sjhb return skb; 73160798Sjhb} 74146806Srwatson 75146806Srwatsonint mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag, 76160798Sjhb struct sock *skip_sk) 77160798Sjhb{ 78160798Sjhb struct hci_dev *hdev; 79160798Sjhb struct mgmt_hdr *hdr; 80160798Sjhb int len; 81146806Srwatson 82160798Sjhb if (!skb) 83165609Srwatson return -EINVAL; 84160798Sjhb 85160798Sjhb len = skb->len; 86146806Srwatson hdev = bt_cb(skb)->mgmt.hdev; 87146806Srwatson 88146806Srwatson /* Time stamp */ 89160798Sjhb __net_timestamp(skb); 90160798Sjhb 91146806Srwatson /* Send just the data, without headers, to the monitor */ 92160798Sjhb if (channel == HCI_CHANNEL_CONTROL) 93160798Sjhb hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode, 94160798Sjhb skb->data, skb->len, 95160798Sjhb skb_get_ktime(skb), flag, skip_sk); 96160798Sjhb 97160798Sjhb hdr = skb_push(skb, sizeof(*hdr)); 98160798Sjhb hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode); 99160798Sjhb if (hdev) 100146806Srwatson hdr->index = cpu_to_le16(hdev->id); 101160798Sjhb else 102160798Sjhb hdr->index = cpu_to_le16(MGMT_INDEX_NONE); 103160798Sjhb hdr->len = cpu_to_le16(len); 104160798Sjhb 105146806Srwatson hci_send_to_channel(channel, skb, flag, skip_sk); 106160798Sjhb 107160798Sjhb kfree_skb(skb); 108160798Sjhb return 0; 109160798Sjhb} 110146806Srwatson 111160798Sjhbint mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 112146806Srwatson void *data, u16 data_len, int flag, struct sock *skip_sk) 113160798Sjhb{ 114146806Srwatson struct sk_buff *skb; 115146806Srwatson 116160798Sjhb skb = mgmt_alloc_skb(hdev, event, data_len); 117146806Srwatson if (!skb) 118160798Sjhb return -ENOMEM; 119160798Sjhb 120160798Sjhb if (data) 121160798Sjhb skb_put_data(skb, data, data_len); 122146806Srwatson 123160798Sjhb return mgmt_send_event_skb(channel, skb, flag, skip_sk); 124161305Snetchild} 125160798Sjhb 126160798Sjhbint mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 127160798Sjhb{ 128146806Srwatson struct sk_buff *skb, *mskb; 129146806Srwatson struct mgmt_hdr *hdr; 130160798Sjhb struct mgmt_ev_cmd_status *ev; 131160798Sjhb int err; 132160798Sjhb 133146806Srwatson BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); 134160798Sjhb 135146806Srwatson skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); 136160798Sjhb if (!skb) 137146806Srwatson return -ENOMEM; 138160798Sjhb 139160798Sjhb hdr = skb_put(skb, sizeof(*hdr)); 140156842Snetchild 141160798Sjhb hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); 142146806Srwatson hdr->index = cpu_to_le16(index); 143160798Sjhb hdr->len = cpu_to_le16(sizeof(*ev)); 144146806Srwatson 145160798Sjhb ev = skb_put(skb, sizeof(*ev)); 146146806Srwatson ev->status = status; 147160798Sjhb ev->opcode = cpu_to_le16(cmd); 148146806Srwatson 149146806Srwatson mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 150160798Sjhb MGMT_EV_CMD_STATUS, sizeof(*ev), ev); 151146806Srwatson if (mskb) 152146806Srwatson skb->tstamp = mskb->tstamp; 153160798Sjhb else 154146806Srwatson __net_timestamp(skb); 155160798Sjhb 156146806Srwatson err = sock_queue_rcv_skb(sk, skb); 157160798Sjhb if (err < 0) 158146806Srwatson kfree_skb(skb); 159160798Sjhb 160160798Sjhb if (mskb) { 161160798Sjhb hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 162159581Snetchild HCI_SOCK_TRUSTED, NULL); 163165609Srwatson kfree_skb(mskb); 164160798Sjhb } 165160798Sjhb 166159581Snetchild return err; 167165609Srwatson} 168146806Srwatson 169160798Sjhbint mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 170160798Sjhb void *rp, size_t rp_len) 171160798Sjhb{ 172146806Srwatson struct sk_buff *skb, *mskb; 173160798Sjhb struct mgmt_hdr *hdr; 174160798Sjhb struct mgmt_ev_cmd_complete *ev; 175160798Sjhb int err; 176160798Sjhb 177160798Sjhb BT_DBG("sock %p", sk); 178146806Srwatson 179165609Srwatson skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); 180160798Sjhb if (!skb) 181146806Srwatson return -ENOMEM; 182160798Sjhb 183146806Srwatson hdr = skb_put(skb, sizeof(*hdr)); 184146806Srwatson 185160798Sjhb hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); 186146806Srwatson hdr->index = cpu_to_le16(index); 187160798Sjhb hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); 188146806Srwatson 189160798Sjhb ev = skb_put(skb, sizeof(*ev) + rp_len); 190146806Srwatson ev->opcode = cpu_to_le16(cmd); 191146806Srwatson ev->status = status; 192160798Sjhb 193146806Srwatson if (rp) 194160798Sjhb memcpy(ev->data, rp, rp_len); 195146806Srwatson 196160798Sjhb mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk), 197146806Srwatson MGMT_EV_CMD_COMPLETE, 198160798Sjhb sizeof(*ev) + rp_len, ev); 199146806Srwatson if (mskb) 200160798Sjhb skb->tstamp = mskb->tstamp; 201146806Srwatson else 202160798Sjhb __net_timestamp(skb); 203146806Srwatson 204146806Srwatson err = sock_queue_rcv_skb(sk, skb); 205160798Sjhb if (err < 0) 206146806Srwatson kfree_skb(skb); 207146806Srwatson 208160798Sjhb if (mskb) { 209160798Sjhb hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb, 210160798Sjhb HCI_SOCK_TRUSTED, NULL); 211146806Srwatson kfree_skb(mskb); 212146806Srwatson } 213160798Sjhb 214160798Sjhb return err; 215146806Srwatson} 216161305Snetchild 217161305Snetchildstruct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, 218161305Snetchild struct hci_dev *hdev) 219160798Sjhb{ 220159581Snetchild struct mgmt_pending_cmd *cmd; 221160798Sjhb 222146806Srwatson list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 223146806Srwatson if (hci_sock_get_channel(cmd->sk) != channel) 224160798Sjhb continue; 225160798Sjhb if (cmd->opcode == opcode) 226146806Srwatson return cmd; 227160798Sjhb } 228146806Srwatson 229160798Sjhb return NULL; 230160798Sjhb} 231160798Sjhb 232160798Sjhbstruct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, 233160798Sjhb u16 opcode, 234160798Sjhb struct hci_dev *hdev, 235160798Sjhb const void *data) 236160798Sjhb{ 237160798Sjhb struct mgmt_pending_cmd *cmd; 238146806Srwatson 239160798Sjhb list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 240146806Srwatson if (cmd->user_data != data) 241160798Sjhb continue; 242160798Sjhb if (cmd->opcode == opcode) 243160798Sjhb return cmd; 244146806Srwatson } 245146806Srwatson 246165609Srwatson return NULL; 247146806Srwatson} 248160798Sjhb 249146806Srwatsonvoid mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, 250146806Srwatson void (*cb)(struct mgmt_pending_cmd *cmd, void *data), 251146806Srwatson void *data) 252160798Sjhb{ 253160798Sjhb struct mgmt_pending_cmd *cmd, *tmp; 254146806Srwatson 255165609Srwatson list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 256159581Snetchild if (opcode > 0 && cmd->opcode != opcode) 257165609Srwatson continue; 258159581Snetchild 259160798Sjhb cb(cmd, data); 260160798Sjhb } 261160798Sjhb} 262146806Srwatson 263160798Sjhbstruct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode, 264160798Sjhb struct hci_dev *hdev, 265160798Sjhb void *data, u16 len) 266160798Sjhb{ 267160798Sjhb struct mgmt_pending_cmd *cmd; 268146806Srwatson 269160798Sjhb cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 270146806Srwatson if (!cmd) 271165609Srwatson return NULL; 272165609Srwatson 273146806Srwatson cmd->opcode = opcode; 274165609Srwatson cmd->index = hdev->id; 275165609Srwatson 276160798Sjhb cmd->param = kmemdup(data, len, GFP_KERNEL); 277165609Srwatson if (!cmd->param) { 278146806Srwatson kfree(cmd); 279165609Srwatson return NULL; 280146806Srwatson } 281165609Srwatson 282146806Srwatson cmd->param_len = len; 283160798Sjhb 284146806Srwatson cmd->sk = sk; 285146806Srwatson sock_hold(sk); 286160798Sjhb 287146806Srwatson return cmd; 288146806Srwatson} 289160798Sjhb 290146806Srwatsonstruct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, 291160798Sjhb struct hci_dev *hdev, 292146806Srwatson void *data, u16 len) 293146806Srwatson{ 294160798Sjhb struct mgmt_pending_cmd *cmd; 295160798Sjhb 296146806Srwatson cmd = mgmt_pending_new(sk, opcode, hdev, data, len); 297160798Sjhb if (!cmd) 298160798Sjhb return NULL; 299146806Srwatson 300160798Sjhb list_add_tail(&cmd->list, &hdev->mgmt_pending); 301146806Srwatson 302163734Snetchild return cmd; 303163734Snetchild} 304160798Sjhb 305146806Srwatsonvoid mgmt_pending_free(struct mgmt_pending_cmd *cmd) 306160798Sjhb{ 307146806Srwatson sock_put(cmd->sk); 308146806Srwatson kfree(cmd->param); 309160798Sjhb kfree(cmd); 310146806Srwatson} 311146806Srwatson 312160798Sjhbvoid mgmt_pending_remove(struct mgmt_pending_cmd *cmd) 313158406Snetchild{ 314160798Sjhb list_del(&cmd->list); 315160798Sjhb mgmt_pending_free(cmd); 316160798Sjhb} 317146806Srwatson 318146806Srwatsonvoid mgmt_mesh_foreach(struct hci_dev *hdev, 319160798Sjhb void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data), 320146806Srwatson void *data, struct sock *sk) 321160798Sjhb{ 322146806Srwatson struct mgmt_mesh_tx *mesh_tx, *tmp; 323160798Sjhb 324146806Srwatson list_for_each_entry_safe(mesh_tx, tmp, &hdev->mgmt_pending, list) { 325160798Sjhb if (!sk || mesh_tx->sk == sk) 326146806Srwatson cb(mesh_tx, data); 327160798Sjhb } 328160798Sjhb} 329160798Sjhb 330146806Srwatsonstruct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk) 331160798Sjhb{ 332159581Snetchild struct mgmt_mesh_tx *mesh_tx; 333159581Snetchild 334160798Sjhb if (list_empty(&hdev->mesh_pending)) 335160798Sjhb return NULL; 336146806Srwatson 337160798Sjhb list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { 338146806Srwatson if (!sk || mesh_tx->sk == sk) 339146806Srwatson return mesh_tx; 340160798Sjhb } 341146806Srwatson 342160798Sjhb return NULL; 343146806Srwatson} 344160798Sjhb 345146806Srwatsonstruct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle) 346160798Sjhb{ 347146806Srwatson struct mgmt_mesh_tx *mesh_tx; 348160798Sjhb 349146806Srwatson if (list_empty(&hdev->mesh_pending)) 350160798Sjhb return NULL; 351146806Srwatson 352160798Sjhb list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) { 353160798Sjhb if (mesh_tx->handle == handle) 354160798Sjhb return mesh_tx; 355160798Sjhb } 356160798Sjhb 357160798Sjhb return NULL; 358160798Sjhb} 359146806Srwatson 360160798Sjhbstruct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev, 361146806Srwatson void *data, u16 len) 362159581Snetchild{ 363160798Sjhb struct mgmt_mesh_tx *mesh_tx; 364146806Srwatson 365160798Sjhb mesh_tx = kzalloc(sizeof(*mesh_tx), GFP_KERNEL); 366146806Srwatson if (!mesh_tx) 367160798Sjhb return NULL; 368146806Srwatson 369160798Sjhb hdev->mesh_send_ref++; 370146806Srwatson if (!hdev->mesh_send_ref) 371160798Sjhb hdev->mesh_send_ref++; 372146806Srwatson 373160798Sjhb mesh_tx->handle = hdev->mesh_send_ref; 374160798Sjhb mesh_tx->index = hdev->id; 375160798Sjhb memcpy(mesh_tx->param, data, len); 376160798Sjhb mesh_tx->param_len = len; 377160798Sjhb mesh_tx->sk = sk; 378146806Srwatson sock_hold(sk); 379160798Sjhb 380146806Srwatson list_add_tail(&mesh_tx->list, &hdev->mesh_pending); 381160798Sjhb 382146806Srwatson return mesh_tx; 383165609Srwatson} 384146806Srwatson 385160798Sjhbvoid mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx) 386146806Srwatson{ 387146806Srwatson list_del(&mesh_tx->list); 388146806Srwatson sock_put(mesh_tx->sk); 389161305Snetchild kfree(mesh_tx); 390146806Srwatson} 391160798Sjhb