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 <net/bluetooth/bluetooth.h> 25 #include <net/bluetooth/hci_core.h> 26 #include <net/bluetooth/mgmt.h> 27 28 #include "mgmt_util.h" 29 30 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, 31 void *data, u16 data_len, int flag, struct sock *skip_sk) 32 { 33 struct sk_buff *skb; 34 struct mgmt_hdr *hdr; 35 36 skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL); 37 if (!skb) 38 return -ENOMEM; 39 40 hdr = (void *) skb_put(skb, sizeof(*hdr)); 41 hdr->opcode = cpu_to_le16(event); 42 if (hdev) 43 hdr->index = cpu_to_le16(hdev->id); 44 else 45 hdr->index = cpu_to_le16(MGMT_INDEX_NONE); 46 hdr->len = cpu_to_le16(data_len); 47 48 if (data) 49 memcpy(skb_put(skb, data_len), data, data_len); 50 51 /* Time stamp */ 52 __net_timestamp(skb); 53 54 hci_send_to_channel(channel, skb, flag, skip_sk); 55 kfree_skb(skb); 56 57 return 0; 58 } 59 60 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) 61 { 62 struct sk_buff *skb; 63 struct mgmt_hdr *hdr; 64 struct mgmt_ev_cmd_status *ev; 65 int err; 66 67 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); 68 69 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); 70 if (!skb) 71 return -ENOMEM; 72 73 hdr = (void *) skb_put(skb, sizeof(*hdr)); 74 75 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); 76 hdr->index = cpu_to_le16(index); 77 hdr->len = cpu_to_le16(sizeof(*ev)); 78 79 ev = (void *) skb_put(skb, sizeof(*ev)); 80 ev->status = status; 81 ev->opcode = cpu_to_le16(cmd); 82 83 err = sock_queue_rcv_skb(sk, skb); 84 if (err < 0) 85 kfree_skb(skb); 86 87 return err; 88 } 89 90 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, 91 void *rp, size_t rp_len) 92 { 93 struct sk_buff *skb; 94 struct mgmt_hdr *hdr; 95 struct mgmt_ev_cmd_complete *ev; 96 int err; 97 98 BT_DBG("sock %p", sk); 99 100 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); 101 if (!skb) 102 return -ENOMEM; 103 104 hdr = (void *) skb_put(skb, sizeof(*hdr)); 105 106 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); 107 hdr->index = cpu_to_le16(index); 108 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); 109 110 ev = (void *) skb_put(skb, sizeof(*ev) + rp_len); 111 ev->opcode = cpu_to_le16(cmd); 112 ev->status = status; 113 114 if (rp) 115 memcpy(ev->data, rp, rp_len); 116 117 err = sock_queue_rcv_skb(sk, skb); 118 if (err < 0) 119 kfree_skb(skb); 120 121 return err; 122 } 123 124 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, 125 struct hci_dev *hdev) 126 { 127 struct mgmt_pending_cmd *cmd; 128 129 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 130 if (hci_sock_get_channel(cmd->sk) != channel) 131 continue; 132 if (cmd->opcode == opcode) 133 return cmd; 134 } 135 136 return NULL; 137 } 138 139 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, 140 u16 opcode, 141 struct hci_dev *hdev, 142 const void *data) 143 { 144 struct mgmt_pending_cmd *cmd; 145 146 list_for_each_entry(cmd, &hdev->mgmt_pending, list) { 147 if (cmd->user_data != data) 148 continue; 149 if (cmd->opcode == opcode) 150 return cmd; 151 } 152 153 return NULL; 154 } 155 156 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, 157 void (*cb)(struct mgmt_pending_cmd *cmd, void *data), 158 void *data) 159 { 160 struct mgmt_pending_cmd *cmd, *tmp; 161 162 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { 163 if (opcode > 0 && cmd->opcode != opcode) 164 continue; 165 166 cb(cmd, data); 167 } 168 } 169 170 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, 171 struct hci_dev *hdev, 172 void *data, u16 len) 173 { 174 struct mgmt_pending_cmd *cmd; 175 176 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); 177 if (!cmd) 178 return NULL; 179 180 cmd->opcode = opcode; 181 cmd->index = hdev->id; 182 183 cmd->param = kmemdup(data, len, GFP_KERNEL); 184 if (!cmd->param) { 185 kfree(cmd); 186 return NULL; 187 } 188 189 cmd->param_len = len; 190 191 cmd->sk = sk; 192 sock_hold(sk); 193 194 list_add(&cmd->list, &hdev->mgmt_pending); 195 196 return cmd; 197 } 198 199 void mgmt_pending_free(struct mgmt_pending_cmd *cmd) 200 { 201 sock_put(cmd->sk); 202 kfree(cmd->param); 203 kfree(cmd); 204 } 205 206 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) 207 { 208 list_del(&cmd->list); 209 mgmt_pending_free(cmd); 210 } 211