tizen 2.3.1 release
[framework/connectivity/bluez.git] / src / shared / mgmt.c
1 /*
2  *
3  *  BlueZ - Bluetooth protocol stack for Linux
4  *
5  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
6  *
7  *
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.
12  *
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.
17  *
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
21  *
22  */
23
24 #ifdef HAVE_CONFIG_H
25 #include <config.h>
26 #endif
27
28 #include <stdio.h>
29 #include <unistd.h>
30 #include <string.h>
31 #include <errno.h>
32
33 #include "lib/bluetooth.h"
34 #include "lib/mgmt.h"
35 #include "lib/hci.h"
36
37 #include "src/shared/io.h"
38 #include "src/shared/queue.h"
39 #include "src/shared/util.h"
40 #include "src/shared/mgmt.h"
41
42 struct mgmt {
43         int ref_count;
44         int fd;
45         bool close_on_unref;
46         struct io *io;
47         bool writer_active;
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;
54         void *buf;
55         uint16_t len;
56         mgmt_debug_func_t debug_callback;
57         mgmt_destroy_func_t debug_destroy;
58         void *debug_data;
59 };
60
61 struct mgmt_request {
62         unsigned int id;
63         uint16_t opcode;
64         uint16_t index;
65         void *buf;
66         uint16_t len;
67         mgmt_request_func_t callback;
68         mgmt_destroy_func_t destroy;
69         void *user_data;
70 };
71
72 struct mgmt_notify {
73         unsigned int id;
74         uint16_t event;
75         uint16_t index;
76         mgmt_notify_func_t callback;
77         mgmt_destroy_func_t destroy;
78         void *user_data;
79 };
80
81 static void destroy_request(void *data)
82 {
83         struct mgmt_request *request = data;
84
85         if (request->destroy)
86                 request->destroy(request->user_data);
87
88         free(request->buf);
89         free(request);
90 }
91
92 static bool match_request_id(const void *a, const void *b)
93 {
94         const struct mgmt_request *request = a;
95         unsigned int id = PTR_TO_UINT(b);
96
97         return request->id == id;
98 }
99
100 static bool match_request_index(const void *a, const void *b)
101 {
102         const struct mgmt_request *request = a;
103         uint16_t index = PTR_TO_UINT(b);
104
105         return request->index == index;
106 }
107
108 static void destroy_notify(void *data)
109 {
110         struct mgmt_notify *notify = data;
111
112         if (notify->destroy)
113                 notify->destroy(notify->user_data);
114
115         free(notify);
116 }
117
118 static bool match_notify_id(const void *a, const void *b)
119 {
120         const struct mgmt_notify *notify = a;
121         unsigned int id = PTR_TO_UINT(b);
122
123         return notify->id == id;
124 }
125
126 static bool match_notify_index(const void *a, const void *b)
127 {
128         const struct mgmt_notify *notify = a;
129         uint16_t index = PTR_TO_UINT(b);
130
131         return notify->index == index;
132 }
133
134 static void write_watch_destroy(void *user_data)
135 {
136         struct mgmt *mgmt = user_data;
137
138         mgmt->writer_active = false;
139 }
140
141 static bool send_request(struct mgmt *mgmt, struct mgmt_request *request)
142 {
143         struct iovec iov;
144         ssize_t ret;
145
146         iov.iov_base = request->buf;
147         iov.iov_len = request->len;
148
149         ret = io_send(mgmt->io, &iov, 1);
150         if (ret < 0) {
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,
155                                                         request->user_data);
156                 destroy_request(request);
157                 return false;
158         }
159
160         util_debug(mgmt->debug_callback, mgmt->debug_data,
161                                 "[0x%04x] command 0x%04x",
162                                 request->index, request->opcode);
163
164         util_hexdump('<', request->buf, ret, mgmt->debug_callback,
165                                                         mgmt->debug_data);
166
167         queue_push_tail(mgmt->pending_list, request);
168
169         return true;
170 }
171
172 static bool can_write_data(struct io *io, void *user_data)
173 {
174         struct mgmt *mgmt = user_data;
175         struct mgmt_request *request;
176         bool can_write;
177
178         request = queue_pop_head(mgmt->reply_queue);
179         if (!request) {
180                 /* only reply commands can jump the queue */
181                 if (!queue_isempty(mgmt->pending_list))
182                         return false;
183
184                 request = queue_pop_head(mgmt->request_queue);
185                 if (!request)
186                         return false;
187
188                 can_write = false;
189         } else {
190                 /* allow multiple replies to jump the queue */
191                 can_write = !queue_isempty(mgmt->reply_queue);
192         }
193
194         if (!send_request(mgmt, request))
195                 return true;
196
197         return can_write;
198 }
199
200 static void wakeup_writer(struct mgmt *mgmt)
201 {
202         if (!queue_isempty(mgmt->pending_list)) {
203                 /* only queued reply commands trigger wakeup */
204                 if (queue_isempty(mgmt->reply_queue))
205                         return;
206         }
207
208         if (mgmt->writer_active)
209                 return;
210
211         mgmt->writer_active = true;
212
213         io_set_write_handler(mgmt->io, can_write_data, mgmt,
214                                                 write_watch_destroy);
215 }
216
217 struct opcode_index {
218         uint16_t opcode;
219         uint16_t index;
220 };
221
222 static bool match_request_opcode_index(const void *a, const void *b)
223 {
224         const struct mgmt_request *request = a;
225         const struct opcode_index *match = b;
226
227         return request->opcode == match->opcode &&
228                                         request->index == match->index;
229 }
230
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)
234 {
235         struct opcode_index match = { .opcode = opcode, .index = index };
236         struct mgmt_request *request;
237
238         request = queue_remove_if(mgmt->pending_list,
239                                         match_request_opcode_index, &match);
240         if (request) {
241                 if (request->callback)
242                         request->callback(status, length, param,
243                                                         request->user_data);
244
245                 destroy_request(request);
246         }
247
248         wakeup_writer(mgmt);
249 }
250
251 struct event_index {
252         uint16_t event;
253         uint16_t index;
254         uint16_t length;
255         const void *param;
256 };
257
258 static void notify_handler(void *data, void *user_data)
259 {
260         struct mgmt_notify *notify = data;
261         struct event_index *match = user_data;
262
263         if (notify->event != match->event)
264                 return;
265
266         if (notify->index != match->index && notify->index != MGMT_INDEX_NONE)
267                 return;
268
269         if (notify->callback)
270                 notify->callback(match->index, match->length, match->param,
271                                                         notify->user_data);
272 }
273
274 static void process_notify(struct mgmt *mgmt, uint16_t event, uint16_t index,
275                                         uint16_t length, const void *param)
276 {
277         struct event_index match = { .event = event, .index = index,
278                                         .length = length, .param = param };
279
280         queue_foreach(mgmt->notify_list, notify_handler, &match);
281 }
282
283 static bool can_read_data(struct io *io, void *user_data)
284 {
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;
289         ssize_t bytes_read;
290         uint16_t opcode, event, index, length;
291
292         bytes_read = read(mgmt->fd, mgmt->buf, mgmt->len);
293         if (bytes_read < 0)
294                 return false;
295
296         util_hexdump('>', mgmt->buf, bytes_read,
297                                 mgmt->debug_callback, mgmt->debug_data);
298
299         if (bytes_read < MGMT_HDR_SIZE)
300                 return true;
301
302         hdr = mgmt->buf;
303         event = btohs(hdr->opcode);
304         index = btohs(hdr->index);
305         length = btohs(hdr->len);
306
307         if (bytes_read < length + MGMT_HDR_SIZE)
308                 return true;
309
310         mgmt_ref(mgmt);
311
312         switch (event) {
313         case MGMT_EV_CMD_COMPLETE:
314                 cc = mgmt->buf + MGMT_HDR_SIZE;
315                 opcode = btohs(cc->opcode);
316
317                 util_debug(mgmt->debug_callback, mgmt->debug_data,
318                                 "[0x%04x] command 0x%04x complete: 0x%02x",
319                                                 index, opcode, cc->status);
320
321                 request_complete(mgmt, cc->status, opcode, index, length - 3,
322                                                 mgmt->buf + MGMT_HDR_SIZE + 3);
323                 break;
324         case MGMT_EV_CMD_STATUS:
325                 cs = mgmt->buf + MGMT_HDR_SIZE;
326                 opcode = btohs(cs->opcode);
327
328                 util_debug(mgmt->debug_callback, mgmt->debug_data,
329                                 "[0x%04x] command 0x%02x status: 0x%02x",
330                                                 index, opcode, cs->status);
331
332                 request_complete(mgmt, cs->status, opcode, index, 0, NULL);
333                 break;
334         default:
335                 util_debug(mgmt->debug_callback, mgmt->debug_data,
336                                 "[0x%04x] event 0x%04x", index, event);
337
338                 process_notify(mgmt, event, index, length,
339                                                 mgmt->buf + MGMT_HDR_SIZE);
340                 break;
341         }
342
343         mgmt_unref(mgmt);
344
345         return true;
346 }
347
348 struct mgmt *mgmt_new(int fd)
349 {
350         struct mgmt *mgmt;
351
352         if (fd < 0)
353                 return NULL;
354
355         mgmt = new0(struct mgmt, 1);
356         if (!mgmt)
357                 return NULL;
358
359         mgmt->fd = fd;
360         mgmt->close_on_unref = false;
361
362         mgmt->len = 512;
363         mgmt->buf = malloc(mgmt->len);
364         if (!mgmt->buf) {
365                 free(mgmt);
366                 return NULL;
367         }
368
369         mgmt->io = io_new(fd);
370         if (!mgmt->io) {
371                 free(mgmt->buf);
372                 free(mgmt);
373                 return NULL;
374         }
375
376         mgmt->request_queue = queue_new();
377         if (!mgmt->request_queue) {
378                 io_destroy(mgmt->io);
379                 free(mgmt->buf);
380                 free(mgmt);
381                 return NULL;
382         }
383
384         mgmt->reply_queue = queue_new();
385         if (!mgmt->reply_queue) {
386                 queue_destroy(mgmt->request_queue, NULL);
387                 io_destroy(mgmt->io);
388                 free(mgmt->buf);
389                 free(mgmt);
390                 return NULL;
391         }
392
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);
398                 free(mgmt->buf);
399                 free(mgmt);
400                 return NULL;
401         }
402
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);
409                 free(mgmt->buf);
410                 free(mgmt);
411                 return NULL;
412         }
413
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);
420                 free(mgmt->buf);
421                 free(mgmt);
422                 return NULL;
423         }
424
425         mgmt->writer_active = false;
426
427         return mgmt_ref(mgmt);
428 }
429
430 struct mgmt *mgmt_new_default(void)
431 {
432         struct mgmt *mgmt;
433         union {
434                 struct sockaddr common;
435                 struct sockaddr_hci hci;
436         } addr;
437         int fd;
438
439         fd = socket(PF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK,
440                                                                 BTPROTO_HCI);
441         if (fd < 0)
442                 return NULL;
443
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;
448
449         if (bind(fd, &addr.common, sizeof(addr.hci)) < 0) {
450                 close(fd);
451                 return NULL;
452         }
453
454         mgmt = mgmt_new(fd);
455         if (!mgmt) {
456                 close(fd);
457                 return NULL;
458         }
459
460         mgmt->close_on_unref = true;
461
462         return mgmt;
463 }
464
465 struct mgmt *mgmt_ref(struct mgmt *mgmt)
466 {
467         if (!mgmt)
468                 return NULL;
469
470         __sync_fetch_and_add(&mgmt->ref_count, 1);
471
472         return mgmt;
473 }
474
475 void mgmt_unref(struct mgmt *mgmt)
476 {
477         if (!mgmt)
478                 return;
479
480         if (__sync_sub_and_fetch(&mgmt->ref_count, 1))
481                 return;
482
483         mgmt_unregister_all(mgmt);
484         mgmt_cancel_all(mgmt);
485
486         queue_destroy(mgmt->reply_queue, NULL);
487         queue_destroy(mgmt->request_queue, NULL);
488
489         io_set_write_handler(mgmt->io, NULL, NULL, NULL);
490         io_set_read_handler(mgmt->io, NULL, NULL, NULL);
491
492         io_destroy(mgmt->io);
493         mgmt->io = NULL;
494
495         if (mgmt->close_on_unref)
496                 close(mgmt->fd);
497
498         if (mgmt->debug_destroy)
499                 mgmt->debug_destroy(mgmt->debug_data);
500
501         free(mgmt->buf);
502         mgmt->buf = NULL;
503
504         queue_destroy(mgmt->notify_list, NULL);
505         queue_destroy(mgmt->pending_list, NULL);
506         free(mgmt);
507
508         return;
509 }
510
511 bool mgmt_set_debug(struct mgmt *mgmt, mgmt_debug_func_t callback,
512                                 void *user_data, mgmt_destroy_func_t destroy)
513 {
514         if (!mgmt)
515                 return false;
516
517         if (mgmt->debug_destroy)
518                 mgmt->debug_destroy(mgmt->debug_data);
519
520         mgmt->debug_callback = callback;
521         mgmt->debug_destroy = destroy;
522         mgmt->debug_data = user_data;
523
524         return true;
525 }
526
527 bool mgmt_set_close_on_unref(struct mgmt *mgmt, bool do_close)
528 {
529         if (!mgmt)
530                 return false;
531
532         mgmt->close_on_unref = do_close;
533
534         return true;
535 }
536
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)
541 {
542         struct mgmt_request *request;
543         struct mgmt_hdr *hdr;
544
545         if (!opcode)
546                 return NULL;
547
548         if (length > 0 && !param)
549                 return NULL;
550
551         request = new0(struct mgmt_request, 1);
552         if (!request)
553                 return NULL;
554
555         request->len = length + MGMT_HDR_SIZE;
556         request->buf = malloc(request->len);
557         if (!request->buf) {
558                 free(request);
559                 return NULL;
560         }
561
562         if (length > 0)
563                 memcpy(request->buf + MGMT_HDR_SIZE, param, length);
564
565         hdr = request->buf;
566         hdr->opcode = htobs(opcode);
567         hdr->index = htobs(index);
568         hdr->len = htobs(length);
569
570         request->opcode = opcode;
571         request->index = index;
572
573         request->callback = callback;
574         request->destroy = destroy;
575         request->user_data = user_data;
576
577         return request;
578 }
579
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)
584 {
585         struct mgmt_request *request;
586
587         if (!mgmt)
588                 return 0;
589
590         request = create_request(opcode, index, length, param,
591                                         callback, user_data, destroy);
592         if (!request)
593                 return 0;
594
595         if (mgmt->next_request_id < 1)
596                 mgmt->next_request_id = 1;
597
598         request->id = mgmt->next_request_id++;
599
600         if (!queue_push_tail(mgmt->request_queue, request)) {
601                 free(request->buf);
602                 free(request);
603                 return 0;
604         }
605
606         wakeup_writer(mgmt);
607
608         return request->id;
609 }
610
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)
615 {
616         struct mgmt_request *request;
617
618         if (!mgmt)
619                 return 0;
620
621         request = create_request(opcode, index, length, param,
622                                         callback, user_data, destroy);
623         if (!request)
624                 return 0;
625
626         if (mgmt->next_request_id < 1)
627                 mgmt->next_request_id = 1;
628
629         request->id = mgmt->next_request_id++;
630
631         if (!send_request(mgmt, request))
632                 return 0;
633
634         return request->id;
635 }
636
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)
641 {
642         struct mgmt_request *request;
643
644         if (!mgmt)
645                 return 0;
646
647         request = create_request(opcode, index, length, param,
648                                         callback, user_data, destroy);
649         if (!request)
650                 return 0;
651
652         if (mgmt->next_request_id < 1)
653                 mgmt->next_request_id = 1;
654
655         request->id = mgmt->next_request_id++;
656
657         if (!queue_push_tail(mgmt->reply_queue, request)) {
658                 free(request->buf);
659                 free(request);
660                 return 0;
661         }
662
663         wakeup_writer(mgmt);
664
665         return request->id;
666 }
667
668 bool mgmt_cancel(struct mgmt *mgmt, unsigned int id)
669 {
670         struct mgmt_request *request;
671
672         if (!mgmt || !id)
673                 return false;
674
675         request = queue_remove_if(mgmt->request_queue, match_request_id,
676                                                         UINT_TO_PTR(id));
677         if (request)
678                 goto done;
679
680         request = queue_remove_if(mgmt->reply_queue, match_request_id,
681                                                         UINT_TO_PTR(id));
682         if (request)
683                 goto done;
684
685         request = queue_remove_if(mgmt->pending_list, match_request_id,
686                                                         UINT_TO_PTR(id));
687         if (!request)
688                 return false;
689
690 done:
691         destroy_request(request);
692
693         wakeup_writer(mgmt);
694
695         return true;
696 }
697
698 bool mgmt_cancel_index(struct mgmt *mgmt, uint16_t index)
699 {
700         if (!mgmt)
701                 return false;
702
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);
709
710         return true;
711 }
712
713 bool mgmt_cancel_all(struct mgmt *mgmt)
714 {
715         if (!mgmt)
716                 return false;
717
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);
721
722         return true;
723 }
724
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)
728 {
729         struct mgmt_notify *notify;
730
731         if (!mgmt || !event)
732                 return 0;
733
734         notify = new0(struct mgmt_notify, 1);
735         if (!notify)
736                 return 0;
737
738         notify->event = event;
739         notify->index = index;
740
741         notify->callback = callback;
742         notify->destroy = destroy;
743         notify->user_data = user_data;
744
745         if (mgmt->next_notify_id < 1)
746                 mgmt->next_notify_id = 1;
747
748         notify->id = mgmt->next_notify_id++;
749
750         if (!queue_push_tail(mgmt->notify_list, notify)) {
751                 free(notify);
752                 return 0;
753         }
754
755         return notify->id;
756 }
757
758 bool mgmt_unregister(struct mgmt *mgmt, unsigned int id)
759 {
760         struct mgmt_notify *notify;
761
762         if (!mgmt || !id)
763                 return false;
764
765         notify = queue_remove_if(mgmt->notify_list, match_notify_id,
766                                                         UINT_TO_PTR(id));
767         if (!notify)
768                 return false;
769
770         destroy_notify(notify);
771         return true;
772 }
773
774 bool mgmt_unregister_index(struct mgmt *mgmt, uint16_t index)
775 {
776         if (!mgmt)
777                 return false;
778
779         queue_remove_all(mgmt->notify_list, match_notify_index,
780                                         UINT_TO_PTR(index), destroy_notify);
781
782         return true;
783 }
784
785 bool mgmt_unregister_all(struct mgmt *mgmt)
786 {
787         if (!mgmt)
788                 return false;
789
790         queue_remove_all(mgmt->notify_list, NULL, NULL, destroy_notify);
791
792         return true;
793 }