Loading...
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
33static 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
59int 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
93int 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
136int 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
184struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
185 struct hci_dev *hdev)
186{
187 struct mgmt_pending_cmd *cmd;
188
189 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
190 if (hci_sock_get_channel(cmd->sk) != channel)
191 continue;
192 if (cmd->opcode == opcode)
193 return cmd;
194 }
195
196 return NULL;
197}
198
199struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
200 u16 opcode,
201 struct hci_dev *hdev,
202 const void *data)
203{
204 struct mgmt_pending_cmd *cmd;
205
206 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
207 if (cmd->user_data != data)
208 continue;
209 if (cmd->opcode == opcode)
210 return cmd;
211 }
212
213 return NULL;
214}
215
216void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
217 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
218 void *data)
219{
220 struct mgmt_pending_cmd *cmd, *tmp;
221
222 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
223 if (opcode > 0 && cmd->opcode != opcode)
224 continue;
225
226 cb(cmd, data);
227 }
228}
229
230struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
231 struct hci_dev *hdev,
232 void *data, u16 len)
233{
234 struct mgmt_pending_cmd *cmd;
235
236 cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
237 if (!cmd)
238 return NULL;
239
240 cmd->opcode = opcode;
241 cmd->index = hdev->id;
242
243 cmd->param = kmemdup(data, len, GFP_KERNEL);
244 if (!cmd->param) {
245 kfree(cmd);
246 return NULL;
247 }
248
249 cmd->param_len = len;
250
251 cmd->sk = sk;
252 sock_hold(sk);
253
254 list_add(&cmd->list, &hdev->mgmt_pending);
255
256 return cmd;
257}
258
259void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
260{
261 sock_put(cmd->sk);
262 kfree(cmd->param);
263 kfree(cmd);
264}
265
266void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
267{
268 list_del(&cmd->list);
269 mgmt_pending_free(cmd);
270}
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
33static 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
59struct 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
75int 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
111int 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
126int 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
169int 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
217struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
218 struct hci_dev *hdev)
219{
220 struct mgmt_pending_cmd *cmd;
221
222 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
223 if (hci_sock_get_channel(cmd->sk) != channel)
224 continue;
225 if (cmd->opcode == opcode)
226 return cmd;
227 }
228
229 return NULL;
230}
231
232struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel,
233 u16 opcode,
234 struct hci_dev *hdev,
235 const void *data)
236{
237 struct mgmt_pending_cmd *cmd;
238
239 list_for_each_entry(cmd, &hdev->mgmt_pending, list) {
240 if (cmd->user_data != data)
241 continue;
242 if (cmd->opcode == opcode)
243 return cmd;
244 }
245
246 return NULL;
247}
248
249void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev,
250 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
251 void *data)
252{
253 struct mgmt_pending_cmd *cmd, *tmp;
254
255 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
256 if (opcode > 0 && cmd->opcode != opcode)
257 continue;
258
259 cb(cmd, data);
260 }
261}
262
263struct 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->index = hdev->id;
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
290struct 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 list_add_tail(&cmd->list, &hdev->mgmt_pending);
301
302 return cmd;
303}
304
305void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
306{
307 sock_put(cmd->sk);
308 kfree(cmd->param);
309 kfree(cmd);
310}
311
312void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
313{
314 list_del(&cmd->list);
315 mgmt_pending_free(cmd);
316}
317
318void mgmt_mesh_foreach(struct hci_dev *hdev,
319 void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data),
320 void *data, struct sock *sk)
321{
322 struct mgmt_mesh_tx *mesh_tx, *tmp;
323
324 list_for_each_entry_safe(mesh_tx, tmp, &hdev->mgmt_pending, list) {
325 if (!sk || mesh_tx->sk == sk)
326 cb(mesh_tx, data);
327 }
328}
329
330struct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk)
331{
332 struct mgmt_mesh_tx *mesh_tx;
333
334 if (list_empty(&hdev->mesh_pending))
335 return NULL;
336
337 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
338 if (!sk || mesh_tx->sk == sk)
339 return mesh_tx;
340 }
341
342 return NULL;
343}
344
345struct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle)
346{
347 struct mgmt_mesh_tx *mesh_tx;
348
349 if (list_empty(&hdev->mesh_pending))
350 return NULL;
351
352 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
353 if (mesh_tx->handle == handle)
354 return mesh_tx;
355 }
356
357 return NULL;
358}
359
360struct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev,
361 void *data, u16 len)
362{
363 struct mgmt_mesh_tx *mesh_tx;
364
365 mesh_tx = kzalloc(sizeof(*mesh_tx), GFP_KERNEL);
366 if (!mesh_tx)
367 return NULL;
368
369 hdev->mesh_send_ref++;
370 if (!hdev->mesh_send_ref)
371 hdev->mesh_send_ref++;
372
373 mesh_tx->handle = hdev->mesh_send_ref;
374 mesh_tx->index = hdev->id;
375 memcpy(mesh_tx->param, data, len);
376 mesh_tx->param_len = len;
377 mesh_tx->sk = sk;
378 sock_hold(sk);
379
380 list_add_tail(&mesh_tx->list, &hdev->mesh_pending);
381
382 return mesh_tx;
383}
384
385void mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx)
386{
387 list_del(&mesh_tx->list);
388 sock_put(mesh_tx->sk);
389 kfree(mesh_tx);
390}