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
create_monitor_ctrl_event(__le16 index,u32 cookie,u16 opcode,u16 len,void * buf)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
mgmt_send_event(u16 event,struct hci_dev * hdev,unsigned short channel,void * data,u16 data_len,int flag,struct sock * skip_sk)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
mgmt_cmd_status(struct sock * sk,u16 index,u16 cmd,u8 status)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
mgmt_cmd_complete(struct sock * sk,u16 index,u16 cmd,u8 status,void * rp,size_t rp_len)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
mgmt_pending_find(unsigned short channel,u16 opcode,struct hci_dev * hdev)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, *tmp;
188
189 mutex_lock(&hdev->mgmt_pending_lock);
190
191 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
192 if (hci_sock_get_channel(cmd->sk) != channel)
193 continue;
194
195 if (cmd->opcode == opcode) {
196 mutex_unlock(&hdev->mgmt_pending_lock);
197 return cmd;
198 }
199 }
200
201 mutex_unlock(&hdev->mgmt_pending_lock);
202
203 return NULL;
204 }
205
mgmt_pending_find_data(unsigned short channel,u16 opcode,struct hci_dev * hdev,const void * data)206 struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
207 u16 opcode,
208 struct hci_dev *hdev,
209 const void *data)
210 {
211 struct mgmt_pending_cmd *cmd;
212
213 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
214 if (cmd->user_data != data)
215 continue;
216 if (cmd->opcode == opcode)
217 return cmd;
218 }
219
220 return NULL;
221 }
222
mgmt_pending_foreach(u16 opcode,struct hci_dev * hdev,bool remove,void (* cb)(struct mgmt_pending_cmd * cmd,void * data),void * data)223 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, bool remove,
224 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
225 void *data)
226 {
227 struct mgmt_pending_cmd *cmd, *tmp;
228
229 mutex_lock(&hdev->mgmt_pending_lock);
230
231 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
232 if (opcode > 0 && cmd->opcode != opcode)
233 continue;
234
235 if (remove)
236 list_del(&cmd->list);
237
238 cb(cmd, data);
239
240 if (remove)
241 mgmt_pending_free(cmd);
242 }
243
244 mutex_unlock(&hdev->mgmt_pending_lock);
245 }
246
mgmt_pending_add(struct sock * sk,u16 opcode,struct hci_dev * hdev,void * data,u16 len)247 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
248 struct hci_dev *hdev,
249 void *data, u16 len)
250 {
251 struct mgmt_pending_cmd *cmd;
252
253 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
254 if (!cmd)
255 return NULL;
256
257 cmd->opcode = opcode;
258 cmd->hdev = hdev;
259
260 cmd->param = kmemdup(data, len, GFP_KERNEL);
261 if (!cmd->param) {
262 kfree(cmd);
263 return NULL;
264 }
265
266 cmd->param_len = len;
267
268 cmd->sk = sk;
269 sock_hold(sk);
270
271 mutex_lock(&hdev->mgmt_pending_lock);
272 list_add(&cmd->list, &hdev->mgmt_pending);
273 mutex_unlock(&hdev->mgmt_pending_lock);
274
275 return cmd;
276 }
277
mgmt_pending_free(struct mgmt_pending_cmd * cmd)278 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
279 {
280 sock_put(cmd->sk);
281 kfree(cmd->param);
282 kfree(cmd);
283 }
284
mgmt_pending_remove(struct mgmt_pending_cmd * cmd)285 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
286 {
287 mutex_lock(&cmd->hdev->mgmt_pending_lock);
288 list_del(&cmd->list);
289 mutex_unlock(&cmd->hdev->mgmt_pending_lock);
290
291 mgmt_pending_free(cmd);
292 }
293