projects/linux·blob master · refs

438 lines · 9.4 KiB

1// SPDX-License-Identifier: GPL-2.0
2/*
3 BlueZ - Bluetooth protocol stack for Linux
4
5 Copyright (C) 2015 Intel Corporation
6
7 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
8 OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
9 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS.
10 IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY
11 CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES
12 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
13 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
14 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
15
16 ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS,
17 COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS
18 SOFTWARE IS DISCLAIMED.
19*/
20
21#include <linux/unaligned.h>
22
23#include <net/bluetooth/bluetooth.h>
24#include <net/bluetooth/hci_core.h>
25#include <net/bluetooth/hci_mon.h>
26#include <net/bluetooth/mgmt.h>
27
28#include "mgmt_util.h"
29
30static struct sk_buff *create_monitor_ctrl_event(__le16 index, u32 cookie,
31 u16 opcode, u16 len, void *buf)
32{
33 struct hci_mon_hdr *hdr;
34 struct sk_buff *skb;
35
36 skb = bt_skb_alloc(6 + len, GFP_ATOMIC);
37 if (!skb)
38 return NULL;
39
40 put_unaligned_le32(cookie, skb_put(skb, 4));
41 put_unaligned_le16(opcode, skb_put(skb, 2));
42
43 if (buf)
44 skb_put_data(skb, buf, len);
45
46 __net_timestamp(skb);
47
48 hdr = skb_push(skb, HCI_MON_HDR_SIZE);
49 hdr->opcode = cpu_to_le16(HCI_MON_CTRL_EVENT);
50 hdr->index = index;
51 hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE);
52
53 return skb;
54}
55
56struct sk_buff *mgmt_alloc_skb(struct hci_dev *hdev, u16 opcode,
57 unsigned int size)
58{
59 struct sk_buff *skb;
60
61 skb = alloc_skb(sizeof(struct mgmt_hdr) + size, GFP_KERNEL);
62 if (!skb)
63 return skb;
64
65 skb_reserve(skb, sizeof(struct mgmt_hdr));
66 bt_cb(skb)->mgmt.hdev = hdev;
67 bt_cb(skb)->mgmt.opcode = opcode;
68
69 return skb;
70}
71
72int mgmt_send_event_skb(unsigned short channel, struct sk_buff *skb, int flag,
73 struct sock *skip_sk)
74{
75 struct hci_dev *hdev;
76 struct mgmt_hdr *hdr;
77 int len;
78
79 if (!skb)
80 return -EINVAL;
81
82 len = skb->len;
83 hdev = bt_cb(skb)->mgmt.hdev;
84
85 /* Time stamp */
86 __net_timestamp(skb);
87
88 /* Send just the data, without headers, to the monitor */
89 if (channel == HCI_CHANNEL_CONTROL)
90 hci_send_monitor_ctrl_event(hdev, bt_cb(skb)->mgmt.opcode,
91 skb->data, skb->len,
92 skb_get_ktime(skb), flag, skip_sk);
93
94 hdr = skb_push(skb, sizeof(*hdr));
95 hdr->opcode = cpu_to_le16(bt_cb(skb)->mgmt.opcode);
96 if (hdev)
97 hdr->index = cpu_to_le16(hdev->id);
98 else
99 hdr->index = cpu_to_le16(MGMT_INDEX_NONE);
100 hdr->len = cpu_to_le16(len);
101
102 hci_send_to_channel(channel, skb, flag, skip_sk);
103
104 kfree_skb(skb);
105 return 0;
106}
107
108int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel,
109 void *data, u16 data_len, int flag, struct sock *skip_sk)
110{
111 struct sk_buff *skb;
112
113 skb = mgmt_alloc_skb(hdev, event, data_len);
114 if (!skb)
115 return -ENOMEM;
116
117 if (data)
118 skb_put_data(skb, data, data_len);
119
120 return mgmt_send_event_skb(channel, skb, flag, skip_sk);
121}
122
123int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status)
124{
125 struct sk_buff *skb, *mskb;
126 struct mgmt_hdr *hdr;
127 struct mgmt_ev_cmd_status *ev;
128 int err;
129
130 BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status);
131
132 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL);
133 if (!skb)
134 return -ENOMEM;
135
136 hdr = skb_put(skb, sizeof(*hdr));
137
138 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS);
139 hdr->index = cpu_to_le16(index);
140 hdr->len = cpu_to_le16(sizeof(*ev));
141
142 ev = skb_put(skb, sizeof(*ev));
143 ev->status = status;
144 ev->opcode = cpu_to_le16(cmd);
145
146 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
147 MGMT_EV_CMD_STATUS, sizeof(*ev), ev);
148 if (mskb)
149 skb->tstamp = mskb->tstamp;
150 else
151 __net_timestamp(skb);
152
153 err = sock_queue_rcv_skb(sk, skb);
154 if (err < 0)
155 kfree_skb(skb);
156
157 if (mskb) {
158 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
159 HCI_SOCK_TRUSTED, NULL);
160 kfree_skb(mskb);
161 }
162
163 return err;
164}
165
166int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status,
167 void *rp, size_t rp_len)
168{
169 struct sk_buff *skb, *mskb;
170 struct mgmt_hdr *hdr;
171 struct mgmt_ev_cmd_complete *ev;
172 int err;
173
174 BT_DBG("sock %p", sk);
175
176 skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL);
177 if (!skb)
178 return -ENOMEM;
179
180 hdr = skb_put(skb, sizeof(*hdr));
181
182 hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE);
183 hdr->index = cpu_to_le16(index);
184 hdr->len = cpu_to_le16(sizeof(*ev) + rp_len);
185
186 ev = skb_put(skb, sizeof(*ev) + rp_len);
187 ev->opcode = cpu_to_le16(cmd);
188 ev->status = status;
189
190 if (rp)
191 memcpy(ev->data, rp, rp_len);
192
193 mskb = create_monitor_ctrl_event(hdr->index, hci_sock_get_cookie(sk),
194 MGMT_EV_CMD_COMPLETE,
195 sizeof(*ev) + rp_len, ev);
196 if (mskb)
197 skb->tstamp = mskb->tstamp;
198 else
199 __net_timestamp(skb);
200
201 err = sock_queue_rcv_skb(sk, skb);
202 if (err < 0)
203 kfree_skb(skb);
204
205 if (mskb) {
206 hci_send_to_channel(HCI_CHANNEL_MONITOR, mskb,
207 HCI_SOCK_TRUSTED, NULL);
208 kfree_skb(mskb);
209 }
210
211 return err;
212}
213
214struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode,
215 struct hci_dev *hdev)
216{
217 struct mgmt_pending_cmd *cmd, *tmp;
218
219 mutex_lock(&hdev->mgmt_pending_lock);
220
221 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
222 if (hci_sock_get_channel(cmd->sk) != channel)
223 continue;
224
225 if (cmd->opcode == opcode) {
226 mutex_unlock(&hdev->mgmt_pending_lock);
227 return cmd;
228 }
229 }
230
231 mutex_unlock(&hdev->mgmt_pending_lock);
232
233 return NULL;
234}
235
236void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, bool remove,
237 void (*cb)(struct mgmt_pending_cmd *cmd, void *data),
238 void *data)
239{
240 struct mgmt_pending_cmd *cmd, *tmp;
241
242 mutex_lock(&hdev->mgmt_pending_lock);
243
244 list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) {
245 if (opcode > 0 && cmd->opcode != opcode)
246 continue;
247
248 if (remove)
249 list_del(&cmd->list);
250
251 cb(cmd, data);
252
253 if (remove)
254 mgmt_pending_free(cmd);
255 }
256
257 mutex_unlock(&hdev->mgmt_pending_lock);
258}
259
260struct mgmt_pending_cmd *mgmt_pending_new(struct sock *sk, u16 opcode,
261 struct hci_dev *hdev,
262 void *data, u16 len)
263{
264 struct mgmt_pending_cmd *cmd;
265
266 cmd = kzalloc_obj(*cmd);
267 if (!cmd)
268 return NULL;
269
270 cmd->opcode = opcode;
271 cmd->hdev = hdev;
272
273 cmd->param = kmemdup(data, len, GFP_KERNEL);
274 if (!cmd->param) {
275 kfree(cmd);
276 return NULL;
277 }
278
279 cmd->param_len = len;
280
281 cmd->sk = sk;
282 sock_hold(sk);
283
284 return cmd;
285}
286
287struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode,
288 struct hci_dev *hdev,
289 void *data, u16 len)
290{
291 struct mgmt_pending_cmd *cmd;
292
293 cmd = mgmt_pending_new(sk, opcode, hdev, data, len);
294 if (!cmd)
295 return NULL;
296
297 mutex_lock(&hdev->mgmt_pending_lock);
298 list_add_tail(&cmd->list, &hdev->mgmt_pending);
299 mutex_unlock(&hdev->mgmt_pending_lock);
300
301 return cmd;
302}
303
304void mgmt_pending_free(struct mgmt_pending_cmd *cmd)
305{
306 sock_put(cmd->sk);
307 kfree(cmd->param);
308 kfree(cmd);
309}
310
311void mgmt_pending_remove(struct mgmt_pending_cmd *cmd)
312{
313 mutex_lock(&cmd->hdev->mgmt_pending_lock);
314 list_del(&cmd->list);
315 mutex_unlock(&cmd->hdev->mgmt_pending_lock);
316
317 mgmt_pending_free(cmd);
318}
319
320bool __mgmt_pending_listed(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd)
321{
322 struct mgmt_pending_cmd *tmp;
323
324 lockdep_assert_held(&hdev->mgmt_pending_lock);
325
326 if (!cmd)
327 return false;
328
329 list_for_each_entry(tmp, &hdev->mgmt_pending, list) {
330 if (cmd == tmp)
331 return true;
332 }
333
334 return false;
335}
336
337bool mgmt_pending_listed(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd)
338{
339 bool listed;
340
341 mutex_lock(&hdev->mgmt_pending_lock);
342 listed = __mgmt_pending_listed(hdev, cmd);
343 mutex_unlock(&hdev->mgmt_pending_lock);
344
345 return listed;
346}
347
348bool mgmt_pending_valid(struct hci_dev *hdev, struct mgmt_pending_cmd *cmd)
349{
350 bool listed;
351
352 if (!cmd)
353 return false;
354
355 mutex_lock(&hdev->mgmt_pending_lock);
356
357 listed = __mgmt_pending_listed(hdev, cmd);
358 if (listed)
359 list_del(&cmd->list);
360
361 mutex_unlock(&hdev->mgmt_pending_lock);
362
363 return listed;
364}
365
366void mgmt_mesh_foreach(struct hci_dev *hdev,
367 void (*cb)(struct mgmt_mesh_tx *mesh_tx, void *data),
368 void *data, struct sock *sk)
369{
370 struct mgmt_mesh_tx *mesh_tx, *tmp;
371
372 list_for_each_entry_safe(mesh_tx, tmp, &hdev->mesh_pending, list) {
373 if (!sk || mesh_tx->sk == sk)
374 cb(mesh_tx, data);
375 }
376}
377
378struct mgmt_mesh_tx *mgmt_mesh_next(struct hci_dev *hdev, struct sock *sk)
379{
380 struct mgmt_mesh_tx *mesh_tx;
381
382 if (list_empty(&hdev->mesh_pending))
383 return NULL;
384
385 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
386 if (!sk || mesh_tx->sk == sk)
387 return mesh_tx;
388 }
389
390 return NULL;
391}
392
393struct mgmt_mesh_tx *mgmt_mesh_find(struct hci_dev *hdev, u8 handle)
394{
395 struct mgmt_mesh_tx *mesh_tx;
396
397 if (list_empty(&hdev->mesh_pending))
398 return NULL;
399
400 list_for_each_entry(mesh_tx, &hdev->mesh_pending, list) {
401 if (mesh_tx->handle == handle)
402 return mesh_tx;
403 }
404
405 return NULL;
406}
407
408struct mgmt_mesh_tx *mgmt_mesh_add(struct sock *sk, struct hci_dev *hdev,
409 void *data, u16 len)
410{
411 struct mgmt_mesh_tx *mesh_tx;
412
413 mesh_tx = kzalloc_obj(*mesh_tx);
414 if (!mesh_tx)
415 return NULL;
416
417 hdev->mesh_send_ref++;
418 if (!hdev->mesh_send_ref)
419 hdev->mesh_send_ref++;
420
421 mesh_tx->handle = hdev->mesh_send_ref;
422 mesh_tx->index = hdev->id;
423 memcpy(mesh_tx->param, data, len);
424 mesh_tx->param_len = len;
425 mesh_tx->sk = sk;
426 sock_hold(sk);
427
428 list_add_tail(&mesh_tx->list, &hdev->mesh_pending);
429
430 return mesh_tx;
431}
432
433void mgmt_mesh_remove(struct mgmt_mesh_tx *mesh_tx)
434{
435 list_del(&mesh_tx->list);
436 sock_put(mesh_tx->sk);
437 kfree(mesh_tx);
438}