3 * BlueZ - Bluetooth protocol stack for Linux
5 * Copyright (C) 2012-2014 Intel Corporation. All rights reserved.
8 * This library is free software; you can redistribute it and/or
9 * modify it under the terms of the GNU Lesser General Public
10 * License as published by the Free Software Foundation; either
11 * version 2.1 of the License, or (at your option) any later version.
13 * This library is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 * Lesser General Public License for more details.
18 * You should have received a copy of the GNU Lesser General Public
19 * License along with this library; if not, write to the Free Software
20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
33 #include "lib/bluetooth.h"
37 #include "src/shared/io.h"
38 #include "src/shared/queue.h"
39 #include "src/shared/util.h"
40 #include "src/shared/mgmt.h"
48 struct queue *request_queue;
49 struct queue *reply_queue;
50 struct queue *pending_list;
51 struct queue *notify_list;
52 unsigned int next_request_id;
53 unsigned int next_notify_id;
56 mgmt_debug_func_t debug_callback;
57 mgmt_destroy_func_t debug_destroy;
67 mgmt_request_func_t callback;
68 mgmt_destroy_func_t destroy;
76 mgmt_notify_func_t callback;
77 mgmt_destroy_func_t destroy;
81 static void destroy_request(void *data)
83 struct mgmt_request *request = data;
86 request->destroy(request->user_data);
92 static bool match_request_id(const void *a, const void *b)
94 const struct mgmt_request *request = a;
95 unsigned int id = PTR_TO_UINT(b);
97 return request->id == id;
100 static bool match_request_index(const void *a, const void *b)
102 const struct mgmt_request *request = a;
103 uint16_t index = PTR_TO_UINT(b);
105 return request->index == index;
108 static void destroy_notify(void *data)
110 struct mgmt_notify *notify = data;
113 notify->destroy(notify->user_data);
118 static bool match_notify_id(const void *a, const void *b)
120 const struct mgmt_notify *notify = a;
121 unsigned int id = PTR_TO_UINT(b);
123 return notify->id == id;
126 static bool match_notify_index(const void *a, const void *b)
128 const struct mgmt_notify *notify = a;
129 uint16_t index = PTR_TO_UINT(b);
131 return notify->index == index;
134 static void write_watch_destroy(void *user_data)
136 struct mgmt *mgmt = user_data;
138 mgmt->writer_active = false;
141 static bool send_request(struct mgmt *mgmt, struct mgmt_request *request)
146 iov.iov_base = request->buf;
147 iov.iov_len = request->len;
149 ret = io_send(mgmt->io, &iov, 1);
151 util_debug(mgmt->debug_callback, mgmt->debug_data,
152 "write failed: %s", strerror(-ret));
153 if (request->callback)
154 request->callback(MGMT_STATUS_FAILED, 0, NULL,
156 destroy_request(request);
160 util_debug(mgmt->debug_callback, mgmt->debug_data,
161 "[0x%04x] command 0x%04x",
162 request->index, request->opcode);
164 util_hexdump('<', request->buf, ret, mgmt->debug_callback,
167 queue_push_tail(mgmt->pending_list, request);
172 static bool can_write_data(struct io *io, void *user_data)
174 struct mgmt *mgmt = user_data;
175 struct mgmt_request *request;
178 request = queue_pop_head(mgmt->reply_queue);
180 /* only reply commands can jump the queue */
181 if (!queue_isempty(mgmt->pending_list))
184 request = queue_pop_head(mgmt->request_queue);
190 /* allow multiple replies to jump the queue */
191 can_write = !queue_isempty(mgmt->reply_queue);
194 if (!send_request(mgmt, request))
200 static void wakeup_writer(struct mgmt *mgmt)
202 if (!queue_isempty(mgmt->pending_list)) {
203 /* only queued reply commands trigger wakeup */
204 if (queue_isempty(mgmt->reply_queue))
208 if (mgmt->writer_active)
211 mgmt->writer_active = true;
213 io_set_write_handler(mgmt->io, can_write_data, mgmt,
214 write_watch_destroy);
217 struct opcode_index {
222 static bool match_request_opcode_index(const void *a, const void *b)
224 const struct mgmt_request *request = a;
225 const struct opcode_index *match = b;
227 return request->opcode == match->opcode &&
228 request->index == match->index;
231 static void request_complete(struct mgmt *mgmt, uint8_t status,
232 uint16_t opcode, uint16_t index,
233 uint16_t length, const void *param)
235 struct opcode_index match = { .opcode = opcode, .index = index };
236 struct mgmt_request *request;
238 request = queue_remove_if(mgmt->pending_list,
239 match_request_opcode_index, &match);
241 if (request->callback)
242 request->callback(status, length, param,
245 destroy_request(request);
258 static void notify_handler(void *data, void *user_data)
260 struct mgmt_notify *notify = data;
261 struct event_index *match = user_data;
263 if (notify->event != match->event)
266 if (notify->index != match->index && notify->index != MGMT_INDEX_NONE)
269 if (notify->callback)
270 notify->callback(match->index, match->length, match->param,
274 static void process_notify(struct mgmt *mgmt, uint16_t event, uint16_t index,
275 uint16_t length, const void *param)
277 struct event_index match = { .event = event, .index = index,
278 .length = length, .param = param };
280 queue_foreach(mgmt->notify_list, notify_handler, &match);
283 static bool can_read_data(struct io *io, void *user_data)
285 struct mgmt *mgmt = user_data;
286 struct mgmt_hdr *hdr;
287 struct mgmt_ev_cmd_complete *cc;
288 struct mgmt_ev_cmd_status *cs;
290 uint16_t opcode, event, index, length;
292 bytes_read = read(mgmt->fd, mgmt->buf, mgmt->len);
296 util_hexdump('>', mgmt->buf, bytes_read,
297 mgmt->debug_callback, mgmt->debug_data);
299 if (bytes_read < MGMT_HDR_SIZE)
303 event = btohs(hdr->opcode);
304 index = btohs(hdr->index);
305 length = btohs(hdr->len);
307 if (bytes_read < length + MGMT_HDR_SIZE)
313 case MGMT_EV_CMD_COMPLETE:
314 cc = mgmt->buf + MGMT_HDR_SIZE;
315 opcode = btohs(cc->opcode);
317 util_debug(mgmt->debug_callback, mgmt->debug_data,
318 "[0x%04x] command 0x%04x complete: 0x%02x",
319 index, opcode, cc->status);
321 request_complete(mgmt, cc->status, opcode, index, length - 3,
322 mgmt->buf + MGMT_HDR_SIZE + 3);
324 case MGMT_EV_CMD_STATUS:
325 cs = mgmt->buf + MGMT_HDR_SIZE;
326 opcode = btohs(cs->opcode);
328 util_debug(mgmt->debug_callback, mgmt->debug_data,
329 "[0x%04x] command 0x%02x status: 0x%02x",
330 index, opcode, cs->status);
332 request_complete(mgmt, cs->status, opcode, index, 0, NULL);
335 util_debug(mgmt->debug_callback, mgmt->debug_data,
336 "[0x%04x] event 0x%04x", index, event);
338 process_notify(mgmt, event, index, length,
339 mgmt->buf + MGMT_HDR_SIZE);
348 struct mgmt *mgmt_new(int fd)
355 mgmt = new0(struct mgmt, 1);
360 mgmt->close_on_unref = false;
363 mgmt->buf = malloc(mgmt->len);
369 mgmt->io = io_new(fd);
376 mgmt->request_queue = queue_new();
377 if (!mgmt->request_queue) {
378 io_destroy(mgmt->io);
384 mgmt->reply_queue = queue_new();
385 if (!mgmt->reply_queue) {
386 queue_destroy(mgmt->request_queue, NULL);
387 io_destroy(mgmt->io);
393 mgmt->pending_list = queue_new();
394 if (!mgmt->pending_list) {
395 queue_destroy(mgmt->reply_queue, NULL);
396 queue_destroy(mgmt->request_queue, NULL);
397 io_destroy(mgmt->io);
403 mgmt->notify_list = queue_new();
404 if (!mgmt->notify_list) {
405 queue_destroy(mgmt->pending_list, NULL);
406 queue_destroy(mgmt->reply_queue, NULL);
407 queue_destroy(mgmt->request_queue, NULL);
408 io_destroy(mgmt->io);
414 if (!io_set_read_handler(mgmt->io, can_read_data, mgmt, NULL)) {
415 queue_destroy(mgmt->notify_list, NULL);
416 queue_destroy(mgmt->pending_list, NULL);
417 queue_destroy(mgmt->reply_queue, NULL);
418 queue_destroy(mgmt->request_queue, NULL);
419 io_destroy(mgmt->io);
425 mgmt->writer_active = false;
427 return mgmt_ref(mgmt);
430 struct mgmt *mgmt_new_default(void)
434 struct sockaddr common;
435 struct sockaddr_hci hci;
439 fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
444 memset(&addr, 0, sizeof(addr));
445 addr.hci.hci_family = AF_BLUETOOTH;
446 addr.hci.hci_dev = HCI_DEV_NONE;
447 addr.hci.hci_channel = HCI_CHANNEL_CONTROL;
449 if (bind(fd, &addr.common, sizeof(addr.hci)) < 0) {
460 mgmt->close_on_unref = true;
465 struct mgmt *mgmt_ref(struct mgmt *mgmt)
470 __sync_fetch_and_add(&mgmt->ref_count, 1);
475 void mgmt_unref(struct mgmt *mgmt)
480 if (__sync_sub_and_fetch(&mgmt->ref_count, 1))
483 mgmt_unregister_all(mgmt);
484 mgmt_cancel_all(mgmt);
486 queue_destroy(mgmt->reply_queue, NULL);
487 queue_destroy(mgmt->request_queue, NULL);
489 io_set_write_handler(mgmt->io, NULL, NULL, NULL);
490 io_set_read_handler(mgmt->io, NULL, NULL, NULL);
492 io_destroy(mgmt->io);
495 if (mgmt->close_on_unref)
498 if (mgmt->debug_destroy)
499 mgmt->debug_destroy(mgmt->debug_data);
504 queue_destroy(mgmt->notify_list, NULL);
505 queue_destroy(mgmt->pending_list, NULL);
511 bool mgmt_set_debug(struct mgmt *mgmt, mgmt_debug_func_t callback,
512 void *user_data, mgmt_destroy_func_t destroy)
517 if (mgmt->debug_destroy)
518 mgmt->debug_destroy(mgmt->debug_data);
520 mgmt->debug_callback = callback;
521 mgmt->debug_destroy = destroy;
522 mgmt->debug_data = user_data;
527 bool mgmt_set_close_on_unref(struct mgmt *mgmt, bool do_close)
532 mgmt->close_on_unref = do_close;
537 static struct mgmt_request *create_request(uint16_t opcode, uint16_t index,
538 uint16_t length, const void *param,
539 mgmt_request_func_t callback,
540 void *user_data, mgmt_destroy_func_t destroy)
542 struct mgmt_request *request;
543 struct mgmt_hdr *hdr;
548 if (length > 0 && !param)
551 request = new0(struct mgmt_request, 1);
555 request->len = length + MGMT_HDR_SIZE;
556 request->buf = malloc(request->len);
563 memcpy(request->buf + MGMT_HDR_SIZE, param, length);
566 hdr->opcode = htobs(opcode);
567 hdr->index = htobs(index);
568 hdr->len = htobs(length);
570 request->opcode = opcode;
571 request->index = index;
573 request->callback = callback;
574 request->destroy = destroy;
575 request->user_data = user_data;
580 unsigned int mgmt_send(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
581 uint16_t length, const void *param,
582 mgmt_request_func_t callback,
583 void *user_data, mgmt_destroy_func_t destroy)
585 struct mgmt_request *request;
590 request = create_request(opcode, index, length, param,
591 callback, user_data, destroy);
595 if (mgmt->next_request_id < 1)
596 mgmt->next_request_id = 1;
598 request->id = mgmt->next_request_id++;
600 if (!queue_push_tail(mgmt->request_queue, request)) {
611 unsigned int mgmt_send_nowait(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
612 uint16_t length, const void *param,
613 mgmt_request_func_t callback,
614 void *user_data, mgmt_destroy_func_t destroy)
616 struct mgmt_request *request;
621 request = create_request(opcode, index, length, param,
622 callback, user_data, destroy);
626 if (mgmt->next_request_id < 1)
627 mgmt->next_request_id = 1;
629 request->id = mgmt->next_request_id++;
631 if (!send_request(mgmt, request))
637 unsigned int mgmt_reply(struct mgmt *mgmt, uint16_t opcode, uint16_t index,
638 uint16_t length, const void *param,
639 mgmt_request_func_t callback,
640 void *user_data, mgmt_destroy_func_t destroy)
642 struct mgmt_request *request;
647 request = create_request(opcode, index, length, param,
648 callback, user_data, destroy);
652 if (mgmt->next_request_id < 1)
653 mgmt->next_request_id = 1;
655 request->id = mgmt->next_request_id++;
657 if (!queue_push_tail(mgmt->reply_queue, request)) {
668 bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
670 struct mgmt_request *request;
675 request = queue_remove_if(mgmt->request_queue, match_request_id,
680 request = queue_remove_if(mgmt->reply_queue, match_request_id,
685 request = queue_remove_if(mgmt->pending_list, match_request_id,
691 destroy_request(request);
698 bool mgmt_cancel_index(struct mgmt *mgmt, uint16_t index)
703 queue_remove_all(mgmt->request_queue, match_request_index,
704 UINT_TO_PTR(index), destroy_request);
705 queue_remove_all(mgmt->reply_queue, match_request_index,
706 UINT_TO_PTR(index), destroy_request);
707 queue_remove_all(mgmt->pending_list, match_request_index,
708 UINT_TO_PTR(index), destroy_request);
713 bool mgmt_cancel_all(struct mgmt *mgmt)
718 queue_remove_all(mgmt->pending_list, NULL, NULL, destroy_request);
719 queue_remove_all(mgmt->reply_queue, NULL, NULL, destroy_request);
720 queue_remove_all(mgmt->request_queue, NULL, NULL, destroy_request);
725 unsigned int mgmt_register(struct mgmt *mgmt, uint16_t event, uint16_t index,
726 mgmt_notify_func_t callback,
727 void *user_data, mgmt_destroy_func_t destroy)
729 struct mgmt_notify *notify;
734 notify = new0(struct mgmt_notify, 1);
738 notify->event = event;
739 notify->index = index;
741 notify->callback = callback;
742 notify->destroy = destroy;
743 notify->user_data = user_data;
745 if (mgmt->next_notify_id < 1)
746 mgmt->next_notify_id = 1;
748 notify->id = mgmt->next_notify_id++;
750 if (!queue_push_tail(mgmt->notify_list, notify)) {
758 bool mgmt_unregister(struct mgmt *mgmt, unsigned int id)
760 struct mgmt_notify *notify;
765 notify = queue_remove_if(mgmt->notify_list, match_notify_id,
770 destroy_notify(notify);
774 bool mgmt_unregister_index(struct mgmt *mgmt, uint16_t index)
779 queue_remove_all(mgmt->notify_list, match_notify_index,
780 UINT_TO_PTR(index), destroy_notify);
785 bool mgmt_unregister_all(struct mgmt *mgmt)
790 queue_remove_all(mgmt->notify_list, NULL, NULL, destroy_notify);