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 <linux/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_alloc_skb(struct hci_dev * hdev,u16 opcode,unsigned int size)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
mgmt_send_event_skb(unsigned short channel,struct sk_buff * skb,int flag,struct sock * skip_sk)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;
81
82 if (!skb)
83 return -EINVAL;
84
85 len = skb->len;
86 hdev = bt_cb(skb)->mgmt.hdev;
87
88 /* Time stamp */
89 __net_timestamp(skb);
90
91 /* Send just the data, without headers, to the monitor */
92 if (channel == HCI_CHANNEL_CONTROL)
93 hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
94 skb->data, skb->len,
95 skb_get_ktime(skb), flag, skip_sk);
96
97 hdr = skb_push(skb, sizeof(*hdr));
98 hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
99 if (hdev)
100 hdr->index = cpu_to_le16(hdev->id);
101 else
102 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
103 hdr->len = cpu_to_le16(len);
104
105 hci_send_to_channel(channel, skb, flag, skip_sk);
106
107 kfree_skb(skb);
108 return 0;
109 }
110
mgmt_send_event(u16 event,struct hci_dev * hdev,unsigned short channel,void * data,u16 data_len,int flag,struct sock * skip_sk)111 int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
112 void *data, u16 data_len, int flag, struct sock *skip_sk)
113 {
114 struct sk_buff *skb;
115
116 skb = mgmt_alloc_skb(hdev, event, data_len);
117 if (!skb)
118 return -ENOMEM;
119
120 if (data)
121 skb_put_data(skb, data, data_len);
122
123 return mgmt_send_event_skb(channel, skb, flag, skip_sk);
124 }
125
mgmt_cmd_status(struct sock * sk,u16 index,u16 cmd,u8 status)126 int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
127 {
128 struct sk_buff *skb, *mskb;
129 struct mgmt_hdr *hdr;
130 struct mgmt_ev_cmd_status *ev;
131 int err;
132
133 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
134
135 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
136 if (!skb)
137 return -ENOMEM;
138
139 hdr = skb_put(skb, sizeof(*hdr));
140
141 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
142 hdr->index = cpu_to_le16(index);
143 hdr->len = cpu_to_le16(sizeof(*ev));
144
145 ev = skb_put(skb, sizeof(*ev));
146 ev->status = status;
147 ev->opcode = cpu_to_le16(cmd);
148
149 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
150 MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
151 if (mskb)
152 skb->tstamp = mskb->tstamp;
153 else
154 __net_timestamp(skb);
155
156 err = sock_queue_rcv_skb(sk, skb);
157 if (err < 0)
158 kfree_skb(skb);
159
160 if (mskb) {
161 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
162 HCI_SOCK_TRUSTED, NULL);
163 kfree_skb(mskb);
164 }
165
166 return err;
167 }
168
mgmt_cmd_complete(struct sock * sk,u16 index,u16 cmd,u8 status,void * rp,size_t rp_len)169 int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
170 void *rp, size_t rp_len)
171 {
172 struct sk_buff *skb, *mskb;
173 struct mgmt_hdr *hdr;
174 struct mgmt_ev_cmd_complete *ev;
175 int err;
176
177 BT_DBG("sock %p", sk);
178
179 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
180 if (!skb)
181 return -ENOMEM;
182
183 hdr = skb_put(skb, sizeof(*hdr));
184
185 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
186 hdr->index = cpu_to_le16(index);
187 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
188
189 ev = skb_put(skb, sizeof(*ev) + rp_len);
190 ev->opcode = cpu_to_le16(cmd);
191 ev->status = status;
192
193 if (rp)
194 memcpy(ev->data, rp, rp_len);
195
196 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
197 MGMT_EV_CMD_COMPLETE,
198 sizeof(*ev) + rp_len, ev);
199 if (mskb)
200 skb->tstamp = mskb->tstamp;
201 else
202 __net_timestamp(skb);
203
204 err = sock_queue_rcv_skb(sk, skb);
205 if (err < 0)
206 kfree_skb(skb);
207
208 if (mskb) {
209 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
210 HCI_SOCK_TRUSTED, NULL);
211 kfree_skb(mskb);
212 }
213
214 return err;
215 }
216
mgmt_pending_find(unsigned short channel,u16 opcode,struct hci_dev * hdev)217 struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
218 struct hci_dev *hdev)
219 {
220 struct mgmt_pending_cmd *cmd, *tmp;
221
222 mutex_lock(&hdev->mgmt_pending_lock);
223
224 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
225 if (hci_sock_get_channel(cmd->sk) != channel)
226 continue;
227
228 if (cmd->opcode == opcode) {
229 mutex_unlock(&hdev->mgmt_pending_lock);
230 return cmd;
231 }
232 }
233
234 mutex_unlock(&hdev->mgmt_pending_lock);
235
236 return NULL;
237 }
238
mgmt_pending_foreach(u16 opcode,struct hci_dev * hdev,bool remove,void (* cb)(struct mgmt_pending_cmd * cmd,void * data),void * data)239 void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, bool remove,
240 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
241 void *data)
242 {
243 struct mgmt_pending_cmd *cmd, *tmp;
244
245 mutex_lock(&hdev->mgmt_pending_lock);
246
247 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
248 if (opcode > 0 && cmd->opcode != opcode)
249 continue;
250
251 if (remove)
252 list_del(&cmd->list);
253
254 cb(cmd, data);
255
256 if (remove)
257 mgmt_pending_free(cmd);
258 }
259
260 mutex_unlock(&hdev->mgmt_pending_lock);
261 }
262
mgmt_pending_new(struct sock * sk,u16 opcode,struct hci_dev * hdev,void * data,u16 len)263 struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode,
264 struct hci_dev *hdev,
265 void *data, u16 len)
266 {
267 struct mgmt_pending_cmd *cmd;
268
269 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
270 if (!cmd)
271 return NULL;
272
273 cmd->opcode = opcode;
274 cmd->hdev = hdev;
275
276 cmd->param = kmemdup(data, len, GFP_KERNEL);
277 if (!cmd->param) {
278 kfree(cmd);
279 return NULL;
280 }
281
282 cmd->param_len = len;
283
284 cmd->sk = sk;
285 sock_hold(sk);
286
287 return cmd;
288 }
289
mgmt_pending_add(struct sock * sk,u16 opcode,struct hci_dev * hdev,void * data,u16 len)290 struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
291 struct hci_dev *hdev,
292 void *data, u16 len)
293 {
294 struct mgmt_pending_cmd *cmd;
295
296 cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
297 if (!cmd)
298 return NULL;
299
300 mutex_lock(&hdev->mgmt_pending_lock);
301 list_add_tail(&cmd->list, &hdev->mgmt_pending);
302 mutex_unlock(&hdev->mgmt_pending_lock);
303
304 return cmd;
305 }
306
mgmt_pending_free(struct mgmt_pending_cmd * cmd)307 void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
308 {
309 sock_put(cmd->sk);
310 kfree(cmd->param);
311 kfree(cmd);
312 }
313
mgmt_pending_remove(struct mgmt_pending_cmd * cmd)314 void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
315 {
316 mutex_lock(&cmd->hdev->mgmt_pending_lock);
317 list_del(&cmd->list);
318 mutex_unlock(&cmd->hdev->mgmt_pending_lock);
319
320 mgmt_pending_free(cmd);
321 }
322
mgmt_mesh_foreach(struct hci_dev * hdev,void (* cb)(struct mgmt_mesh_tx * mesh_tx,void * data),void * data,struct sock * sk)323 void mgmt_mesh_foreach(struct hci_dev *hdev,
324 void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data),
325 void *data, struct sock *sk)
326 {
327 struct mgmt_mesh_tx *mesh_tx, *tmp;
328
329 list_for_each_entry_safe(mesh_tx, tmp, &hdev->mesh_pending, list) {
330 if (!sk || mesh_tx->sk == sk)
331 cb(mesh_tx, data);
332 }
333 }
334
mgmt_mesh_next(struct hci_dev * hdev,struct sock * sk)335 struct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk)
336 {
337 struct mgmt_mesh_tx *mesh_tx;
338
339 if (list_empty(&hdev->mesh_pending))
340 return NULL;
341
342 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
343 if (!sk || mesh_tx->sk == sk)
344 return mesh_tx;
345 }
346
347 return NULL;
348 }
349
mgmt_mesh_find(struct hci_dev * hdev,u8 handle)350 struct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle)
351 {
352 struct mgmt_mesh_tx *mesh_tx;
353
354 if (list_empty(&hdev->mesh_pending))
355 return NULL;
356
357 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
358 if (mesh_tx->handle == handle)
359 return mesh_tx;
360 }
361
362 return NULL;
363 }
364
mgmt_mesh_add(struct sock * sk,struct hci_dev * hdev,void * data,u16 len)365 struct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev,
366 void *data, u16 len)
367 {
368 struct mgmt_mesh_tx *mesh_tx;
369
370 mesh_tx = kzalloc(sizeof(*mesh_tx), GFP_KERNEL);
371 if (!mesh_tx)
372 return NULL;
373
374 hdev->mesh_send_ref++;
375 if (!hdev->mesh_send_ref)
376 hdev->mesh_send_ref++;
377
378 mesh_tx->handle = hdev->mesh_send_ref;
379 mesh_tx->index = hdev->id;
380 memcpy(mesh_tx->param, data, len);
381 mesh_tx->param_len = len;
382 mesh_tx->sk = sk;
383 sock_hold(sk);
384
385 list_add_tail(&mesh_tx->list, &hdev->mesh_pending);
386
387 return mesh_tx;
388 }
389
mgmt_mesh_remove(struct mgmt_mesh_tx * mesh_tx)390 void mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx)
391 {
392 list_del(&mesh_tx->list);
393 sock_put(mesh_tx->sk);
394 kfree(mesh_tx);
395 }
396