monitor/control.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2014  Intel Corporation
   6  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <stdbool.h>
  31 #include <errno.h>
  32 #include <unistd.h>
  33 #include <stdlib.h>
  34 #include <string.h>
  35 #include <sys/time.h>
  36 #include <sys/socket.h>
  37 #include <sys/un.h>
  38
  39 #include "lib/bluetooth.h"
  40 #include "lib/hci.h"
  41 #include "lib/mgmt.h"
  42
  43 #include "src/shared/util.h"
  44 #include "src/shared/btsnoop.h"
  45 #include "src/shared/mainloop.h"
  46
  47 #include "display.h"
  48 #include "packet.h"
  49 #include "hcidump.h"
  50 #include "ellisys.h"
  51 #include "control.h"
  52
  53 static struct btsnoop *btsnoop_file = NULL;
  54 static bool hcidump_fallback = false;
  55
  56 struct control_data {
  57         uint16_t channel;
  58         int fd;
  59         unsigned char buf[BTSNOOP_MAX_PACKET_SIZE];
  60         uint16_t offset;
  61 };
  62
  63 static void free_data(void *user_data)
  64 {
  65         struct control_data *data = user_data;
  66
  67         close(data->fd);
  68
  69         free(data);
  70 }
  71
  72 static void mgmt_index_added(uint16_t len, const void *buf)
  73 {
  74         printf("@ Index Added\n");
  75
  76         packet_hexdump(buf, len);
  77 }
  78
  79 static void mgmt_index_removed(uint16_t len, const void *buf)
  80 {
  81         printf("@ Index Removed\n");
  82
  83         packet_hexdump(buf, len);
  84 }
  85
  86 static void mgmt_unconf_index_added(uint16_t len, const void *buf)
  87 {
  88         printf("@ Unconfigured Index Added\n");
  89
  90         packet_hexdump(buf, len);
  91 }
  92
  93 static void mgmt_unconf_index_removed(uint16_t len, const void *buf)
  94 {
  95         printf("@ Unconfigured Index Removed\n");
  96
  97         packet_hexdump(buf, len);
  98 }
  99
 100 static void mgmt_ext_index_added(uint16_t len, const void *buf)
 101 {
 102         const struct mgmt_ev_ext_index_added *ev = buf;
 103
 104         if (len < sizeof(*ev)) {
 105                 printf("* Malformed Extended Index Added control\n");
 106                 return;
 107         }
 108
 109         printf("@ Extended Index Added: %u (%u)\n", ev->type, ev->bus);
 110
 111         buf += sizeof(*ev);
 112         len -= sizeof(*ev);
 113
 114         packet_hexdump(buf, len);
 115 }
 116
 117 static void mgmt_ext_index_removed(uint16_t len, const void *buf)
 118 {
 119         const struct mgmt_ev_ext_index_removed *ev = buf;
 120
 121         if (len < sizeof(*ev)) {
 122                 printf("* Malformed Extended Index Removed control\n");
 123                 return;
 124         }
 125
 126         printf("@ Extended Index Removed: %u (%u)\n", ev->type, ev->bus);
 127
 128         buf += sizeof(*ev);
 129         len -= sizeof(*ev);
 130
 131         packet_hexdump(buf, len);
 132 }
 133
 134 static void mgmt_controller_error(uint16_t len, const void *buf)
 135 {
 136         const struct mgmt_ev_controller_error *ev = buf;
 137
 138         if (len < sizeof(*ev)) {
 139                 printf("* Malformed Controller Error control\n");
 140                 return;
 141         }
 142
 143         printf("@ Controller Error: 0x%2.2x\n", ev->error_code);
 144
 145         buf += sizeof(*ev);
 146         len -= sizeof(*ev);
 147
 148         packet_hexdump(buf, len);
 149 }
 150
 151 #ifndef NELEM
 152 #define NELEM(x) (sizeof(x) / sizeof((x)[0]))
 153 #endif
 154
 155 static const char *config_options_str[] = {
 156         "external", "public-address",
 157 };
 158
 159 static void mgmt_new_config_options(uint16_t len, const void *buf)
 160 {
 161         uint32_t options;
 162         unsigned int i;
 163
 164         if (len < 4) {
 165                 printf("* Malformed New Configuration Options control\n");
 166                 return;
 167         }
 168
 169         options = get_le32(buf);
 170
 171         printf("@ New Configuration Options: 0x%4.4x\n", options);
 172
 173         if (options) {
 174                 printf("%-12c", ' ');
 175                 for (i = 0; i < NELEM(config_options_str); i++) {
 176                         if (options & (1 << i))
 177                                 printf("%s ", config_options_str[i]);
 178                 }
 179                 printf("\n");
 180         }
 181
 182         buf += 4;
 183         len -= 4;
 184
 185         packet_hexdump(buf, len);
 186 }
 187
 188 static const char *settings_str[] = {
 189         "powered", "connectable", "fast-connectable", "discoverable",
 190         "bondable", "link-security", "ssp", "br/edr", "hs", "le",
 191         "advertising", "secure-conn", "debug-keys", "privacy",
 192         "configuration", "static-addr",
 193 };
 194
 195 static void mgmt_new_settings(uint16_t len, const void *buf)
 196 {
 197         uint32_t settings;
 198         unsigned int i;
 199
 200         if (len < 4) {
 201                 printf("* Malformed New Settings control\n");
 202                 return;
 203         }
 204
 205         settings = get_le32(buf);
 206
 207         printf("@ New Settings: 0x%4.4x\n", settings);
 208
 209         if (settings) {
 210                 printf("%-12c", ' ');
 211                 for (i = 0; i < NELEM(settings_str); i++) {
 212                         if (settings & (1 << i))
 213                                 printf("%s ", settings_str[i]);
 214                 }
 215                 printf("\n");
 216         }
 217
 218         buf += 4;
 219         len -= 4;
 220
 221         packet_hexdump(buf, len);
 222 }
 223
 224 static void mgmt_class_of_dev_changed(uint16_t len, const void *buf)
 225 {
 226         const struct mgmt_ev_class_of_dev_changed *ev = buf;
 227
 228         if (len < sizeof(*ev)) {
 229                 printf("* Malformed Class of Device Changed control\n");
 230                 return;
 231         }
 232
 233         printf("@ Class of Device Changed: 0x%2.2x%2.2x%2.2x\n",
 234                                                 ev->dev_class[2],
 235                                                 ev->dev_class[1],
 236                                                 ev->dev_class[0]);
 237
 238         buf += sizeof(*ev);
 239         len -= sizeof(*ev);
 240
 241         packet_hexdump(buf, len);
 242 }
 243
 244 static void mgmt_local_name_changed(uint16_t len, const void *buf)
 245 {
 246         const struct mgmt_ev_local_name_changed *ev = buf;
 247
 248         if (len < sizeof(*ev)) {
 249                 printf("* Malformed Local Name Changed control\n");
 250                 return;
 251         }
 252
 253         printf("@ Local Name Changed: %s (%s)\n", ev->name, ev->short_name);
 254
 255         buf += sizeof(*ev);
 256         len -= sizeof(*ev);
 257
 258         packet_hexdump(buf, len);
 259 }
 260
 261 static void mgmt_new_link_key(uint16_t len, const void *buf)
 262 {
 263         const struct mgmt_ev_new_link_key *ev = buf;
 264         const char *type;
 265         char str[18];
 266         static const char *types[] = {
 267                 "Combination key",
 268                 "Local Unit key",
 269                 "Remote Unit key",
 270                 "Debug Combination key",
 271                 "Unauthenticated Combination key from P-192",
 272                 "Authenticated Combination key from P-192",
 273                 "Changed Combination key",
 274                 "Unauthenticated Combination key from P-256",
 275                 "Authenticated Combination key from P-256",
 276         };
 277
 278         if (len < sizeof(*ev)) {
 279                 printf("* Malformed New Link Key control\n");
 280                 return;
 281         }
 282
 283         if (ev->key.type < NELEM(types))
 284                 type = types[ev->key.type];
 285         else
 286                 type = "Reserved";
 287
 288         ba2str(&ev->key.addr.bdaddr, str);
 289
 290         printf("@ New Link Key: %s (%d) %s (%u)\n", str,
 291                                 ev->key.addr.type, type, ev->key.type);
 292
 293         buf += sizeof(*ev);
 294         len -= sizeof(*ev);
 295
 296         packet_hexdump(buf, len);
 297 }
 298
 299 static void mgmt_new_long_term_key(uint16_t len, const void *buf)
 300 {
 301         const struct mgmt_ev_new_long_term_key *ev = buf;
 302         const char *type;
 303         char str[18];
 304
 305         if (len < sizeof(*ev)) {
 306                 printf("* Malformed New Long Term Key control\n");
 307                 return;
 308         }
 309
 310         /* LE SC keys are both for master and slave */
 311         switch (ev->key.type) {
 312         case 0x00:
 313                 if (ev->key.master)
 314                         type = "Master (Unauthenticated)";
 315                 else
 316                         type = "Slave (Unauthenticated)";
 317                 break;
 318         case 0x01:
 319                 if (ev->key.master)
 320                         type = "Master (Authenticated)";
 321                 else
 322                         type = "Slave (Authenticated)";
 323                 break;
 324         case 0x02:
 325                 type = "SC (Unauthenticated)";
 326                 break;
 327         case 0x03:
 328                 type = "SC (Authenticated)";
 329                 break;
 330         case 0x04:
 331                 type = "SC (Debug)";
 332                 break;
 333         default:
 334                 type = "<unknown>";
 335                 break;
 336         }
 337
 338         ba2str(&ev->key.addr.bdaddr, str);
 339
 340         printf("@ New Long Term Key: %s (%d) %s 0x%02x\n", str,
 341                         ev->key.addr.type, type, ev->key.type);
 342
 343         buf += sizeof(*ev);
 344         len -= sizeof(*ev);
 345
 346         packet_hexdump(buf, len);
 347 }
 348
 349 static void mgmt_device_connected(uint16_t len, const void *buf)
 350 {
 351         const struct mgmt_ev_device_connected *ev = buf;
 352         uint32_t flags;
 353         char str[18];
 354
 355         if (len < sizeof(*ev)) {
 356                 printf("* Malformed Device Connected control\n");
 357                 return;
 358         }
 359
 360         flags = le32_to_cpu(ev->flags);
 361         ba2str(&ev->addr.bdaddr, str);
 362
 363         printf("@ Device Connected: %s (%d) flags 0x%4.4x\n",
 364                                                 str, ev->addr.type, flags);
 365
 366         buf += sizeof(*ev);
 367         len -= sizeof(*ev);
 368
 369         packet_hexdump(buf, len);
 370 }
 371
 372 static void mgmt_device_disconnected(uint16_t len, const void *buf)
 373 {
 374         const struct mgmt_ev_device_disconnected *ev = buf;
 375         char str[18];
 376         uint8_t reason;
 377         uint16_t consumed_len;
 378
 379         if (len < sizeof(struct mgmt_addr_info)) {
 380                 printf("* Malformed Device Disconnected control\n");
 381                 return;
 382         }
 383
 384         if (len < sizeof(*ev)) {
 385                 reason = MGMT_DEV_DISCONN_UNKNOWN;
 386                 consumed_len = len;
 387         } else {
 388                 reason = ev->reason;
 389                 consumed_len = sizeof(*ev);
 390         }
 391
 392         ba2str(&ev->addr.bdaddr, str);
 393
 394         printf("@ Device Disconnected: %s (%d) reason %u\n", str, ev->addr.type,
 395                                                                         reason);
 396
 397         buf += consumed_len;
 398         len -= consumed_len;
 399
 400         packet_hexdump(buf, len);
 401 }
 402
 403 static void mgmt_connect_failed(uint16_t len, const void *buf)
 404 {
 405         const struct mgmt_ev_connect_failed *ev = buf;
 406         char str[18];
 407
 408         if (len < sizeof(*ev)) {
 409                 printf("* Malformed Connect Failed control\n");
 410                 return;
 411         }
 412
 413         ba2str(&ev->addr.bdaddr, str);
 414
 415         printf("@ Connect Failed: %s (%d) status 0x%2.2x\n",
 416                                         str, ev->addr.type, ev->status);
 417
 418         buf += sizeof(*ev);
 419         len -= sizeof(*ev);
 420
 421         packet_hexdump(buf, len);
 422 }
 423
 424 static void mgmt_pin_code_request(uint16_t len, const void *buf)
 425 {
 426         const struct mgmt_ev_pin_code_request *ev = buf;
 427         char str[18];
 428
 429         if (len < sizeof(*ev)) {
 430                 printf("* Malformed PIN Code Request control\n");
 431                 return;
 432         }
 433
 434         ba2str(&ev->addr.bdaddr, str);
 435
 436         printf("@ PIN Code Request: %s (%d) secure 0x%2.2x\n",
 437                                         str, ev->addr.type, ev->secure);
 438
 439         buf += sizeof(*ev);
 440         len -= sizeof(*ev);
 441
 442         packet_hexdump(buf, len);
 443 }
 444
 445 static void mgmt_user_confirm_request(uint16_t len, const void *buf)
 446 {
 447         const struct mgmt_ev_user_confirm_request *ev = buf;
 448         char str[18];
 449
 450         if (len < sizeof(*ev)) {
 451                 printf("* Malformed User Confirmation Request control\n");
 452                 return;
 453         }
 454
 455         ba2str(&ev->addr.bdaddr, str);
 456
 457         printf("@ User Confirmation Request: %s (%d) hint %d value %d\n",
 458                         str, ev->addr.type, ev->confirm_hint, ev->value);
 459
 460         buf += sizeof(*ev);
 461         len -= sizeof(*ev);
 462
 463         packet_hexdump(buf, len);
 464 }
 465
 466 static void mgmt_user_passkey_request(uint16_t len, const void *buf)
 467 {
 468         const struct mgmt_ev_user_passkey_request *ev = buf;
 469         char str[18];
 470
 471         if (len < sizeof(*ev)) {
 472                 printf("* Malformed User Passkey Request control\n");
 473                 return;
 474         }
 475
 476         ba2str(&ev->addr.bdaddr, str);
 477
 478         printf("@ User Passkey Request: %s (%d)\n", str, ev->addr.type);
 479
 480         buf += sizeof(*ev);
 481         len -= sizeof(*ev);
 482
 483         packet_hexdump(buf, len);
 484 }
 485
 486 static void mgmt_auth_failed(uint16_t len, const void *buf)
 487 {
 488         const struct mgmt_ev_auth_failed *ev = buf;
 489         char str[18];
 490
 491         if (len < sizeof(*ev)) {
 492                 printf("* Malformed Authentication Failed control\n");
 493                 return;
 494         }
 495
 496         ba2str(&ev->addr.bdaddr, str);
 497
 498         printf("@ Authentication Failed: %s (%d) status 0x%2.2x\n",
 499                                         str, ev->addr.type, ev->status);
 500
 501         buf += sizeof(*ev);
 502         len -= sizeof(*ev);
 503
 504         packet_hexdump(buf, len);
 505 }
 506
 507 static void mgmt_device_found(uint16_t len, const void *buf)
 508 {
 509         const struct mgmt_ev_device_found *ev = buf;
 510         uint32_t flags;
 511         char str[18];
 512
 513         if (len < sizeof(*ev)) {
 514                 printf("* Malformed Device Found control\n");
 515                 return;
 516         }
 517
 518         flags = le32_to_cpu(ev->flags);
 519         ba2str(&ev->addr.bdaddr, str);
 520
 521         printf("@ Device Found: %s (%d) rssi %d flags 0x%4.4x\n",
 522                                         str, ev->addr.type, ev->rssi, flags);
 523
 524         buf += sizeof(*ev);
 525         len -= sizeof(*ev);
 526
 527         packet_hexdump(buf, len);
 528 }
 529
 530 static void mgmt_discovering(uint16_t len, const void *buf)
 531 {
 532         const struct mgmt_ev_discovering *ev = buf;
 533
 534         if (len < sizeof(*ev)) {
 535                 printf("* Malformed Discovering control\n");
 536                 return;
 537         }
 538
 539         printf("@ Discovering: 0x%2.2x (%d)\n", ev->discovering, ev->type);
 540
 541         buf += sizeof(*ev);
 542         len -= sizeof(*ev);
 543
 544         packet_hexdump(buf, len);
 545 }
 546
 547 static void mgmt_device_blocked(uint16_t len, const void *buf)
 548 {
 549         const struct mgmt_ev_device_blocked *ev = buf;
 550         char str[18];
 551
 552         if (len < sizeof(*ev)) {
 553                 printf("* Malformed Device Blocked control\n");
 554                 return;
 555         }
 556
 557         ba2str(&ev->addr.bdaddr, str);
 558
 559         printf("@ Device Blocked: %s (%d)\n", str, ev->addr.type);
 560
 561         buf += sizeof(*ev);
 562         len -= sizeof(*ev);
 563
 564         packet_hexdump(buf, len);
 565 }
 566
 567 static void mgmt_device_unblocked(uint16_t len, const void *buf)
 568 {
 569         const struct mgmt_ev_device_unblocked *ev = buf;
 570         char str[18];
 571
 572         if (len < sizeof(*ev)) {
 573                 printf("* Malformed Device Unblocked control\n");
 574                 return;
 575         }
 576
 577         ba2str(&ev->addr.bdaddr, str);
 578
 579         printf("@ Device Unblocked: %s (%d)\n", str, ev->addr.type);
 580
 581         buf += sizeof(*ev);
 582         len -= sizeof(*ev);
 583
 584         packet_hexdump(buf, len);
 585 }
 586
 587 static void mgmt_device_unpaired(uint16_t len, const void *buf)
 588 {
 589         const struct mgmt_ev_device_unpaired *ev = buf;
 590         char str[18];
 591
 592         if (len < sizeof(*ev)) {
 593                 printf("* Malformed Device Unpaired control\n");
 594                 return;
 595         }
 596
 597         ba2str(&ev->addr.bdaddr, str);
 598
 599         printf("@ Device Unpaired: %s (%d)\n", str, ev->addr.type);
 600
 601         buf += sizeof(*ev);
 602         len -= sizeof(*ev);
 603
 604         packet_hexdump(buf, len);
 605 }
 606
 607 static void mgmt_passkey_notify(uint16_t len, const void *buf)
 608 {
 609         const struct mgmt_ev_passkey_notify *ev = buf;
 610         uint32_t passkey;
 611         char str[18];
 612
 613         if (len < sizeof(*ev)) {
 614                 printf("* Malformed Passkey Notify control\n");
 615                 return;
 616         }
 617
 618         ba2str(&ev->addr.bdaddr, str);
 619
 620         passkey = le32_to_cpu(ev->passkey);
 621
 622         printf("@ Passkey Notify: %s (%d) passkey %06u entered %u\n",
 623                                 str, ev->addr.type, passkey, ev->entered);
 624
 625         buf += sizeof(*ev);
 626         len -= sizeof(*ev);
 627
 628         packet_hexdump(buf, len);
 629 }
 630
 631 static void mgmt_new_irk(uint16_t len, const void *buf)
 632 {
 633         const struct mgmt_ev_new_irk *ev = buf;
 634         char addr[18], rpa[18];
 635
 636         if (len < sizeof(*ev)) {
 637                 printf("* Malformed New IRK control\n");
 638                 return;
 639         }
 640
 641         ba2str(&ev->rpa, rpa);
 642         ba2str(&ev->key.addr.bdaddr, addr);
 643
 644         printf("@ New IRK: %s (%d) %s\n", addr, ev->key.addr.type, rpa);
 645
 646         buf += sizeof(*ev);
 647         len -= sizeof(*ev);
 648
 649         packet_hexdump(buf, len);
 650 }
 651
 652 static void mgmt_new_csrk(uint16_t len, const void *buf)
 653 {
 654         const struct mgmt_ev_new_csrk *ev = buf;
 655         const char *type;
 656         char addr[18];
 657
 658         if (len < sizeof(*ev)) {
 659                 printf("* Malformed New CSRK control\n");
 660                 return;
 661         }
 662
 663         ba2str(&ev->key.addr.bdaddr, addr);
 664
 665         switch (ev->key.type) {
 666         case 0x00:
 667                 type = "Local Unauthenticated";
 668                 break;
 669         case 0x01:
 670                 type = "Remote Unauthenticated";
 671                 break;
 672         case 0x02:
 673                 type = "Local Authenticated";
 674                 break;
 675         case 0x03:
 676                 type = "Remote Authenticated";
 677                 break;
 678         default:
 679                 type = "<unknown>";
 680                 break;
 681         }
 682
 683         printf("@ New CSRK: %s (%d) %s (%u)\n", addr, ev->key.addr.type,
 684                                                         type, ev->key.type);
 685
 686         buf += sizeof(*ev);
 687         len -= sizeof(*ev);
 688
 689         packet_hexdump(buf, len);
 690 }
 691
 692 static void mgmt_device_added(uint16_t len, const void *buf)
 693 {
 694         const struct mgmt_ev_device_added *ev = buf;
 695         char str[18];
 696
 697         if (len < sizeof(*ev)) {
 698                 printf("* Malformed Device Added control\n");
 699                 return;
 700         }
 701
 702         ba2str(&ev->addr.bdaddr, str);
 703
 704         printf("@ Device Added: %s (%d) %d\n", str, ev->addr.type, ev->action);
 705
 706         buf += sizeof(*ev);
 707         len -= sizeof(*ev);
 708
 709         packet_hexdump(buf, len);
 710 }
 711
 712 static void mgmt_device_removed(uint16_t len, const void *buf)
 713 {
 714         const struct mgmt_ev_device_removed *ev = buf;
 715         char str[18];
 716
 717         if (len < sizeof(*ev)) {
 718                 printf("* Malformed Device Removed control\n");
 719                 return;
 720         }
 721
 722         ba2str(&ev->addr.bdaddr, str);
 723
 724         printf("@ Device Removed: %s (%d)\n", str, ev->addr.type);
 725
 726         buf += sizeof(*ev);
 727         len -= sizeof(*ev);
 728
 729         packet_hexdump(buf, len);
 730 }
 731
 732 static void mgmt_new_conn_param(uint16_t len, const void *buf)
 733 {
 734         const struct mgmt_ev_new_conn_param *ev = buf;
 735         char addr[18];
 736         uint16_t min, max, latency, timeout;
 737
 738         if (len < sizeof(*ev)) {
 739                 printf("* Malformed New Connection Parameter control\n");
 740                 return;
 741         }
 742
 743         ba2str(&ev->addr.bdaddr, addr);
 744         min = le16_to_cpu(ev->min_interval);
 745         max = le16_to_cpu(ev->max_interval);
 746         latency = le16_to_cpu(ev->latency);
 747         timeout = le16_to_cpu(ev->timeout);
 748
 749         printf("@ New Conn Param: %s (%d) hint %d min 0x%4.4x max 0x%4.4x "
 750                 "latency 0x%4.4x timeout 0x%4.4x\n", addr, ev->addr.type,
 751                 ev->store_hint, min, max, latency, timeout);
 752
 753         buf += sizeof(*ev);
 754         len -= sizeof(*ev);
 755
 756         packet_hexdump(buf, len);
 757 }
 758
 759 static void mgmt_advertising_added(uint16_t len, const void *buf)
 760 {
 761         const struct mgmt_ev_advertising_added *ev = buf;
 762
 763         if (len < sizeof(*ev)) {
 764                 printf("* Malformed Advertising Added control\n");
 765                 return;
 766         }
 767
 768         printf("@ Advertising Added: %u\n", ev->instance);
 769
 770         buf += sizeof(*ev);
 771         len -= sizeof(*ev);
 772
 773         packet_hexdump(buf, len);
 774 }
 775
 776 static void mgmt_advertising_removed(uint16_t len, const void *buf)
 777 {
 778         const struct mgmt_ev_advertising_removed *ev = buf;
 779
 780         if (len < sizeof(*ev)) {
 781                 printf("* Malformed Advertising Removed control\n");
 782                 return;
 783         }
 784
 785         printf("@ Advertising Removed: %u\n", ev->instance);
 786
 787         buf += sizeof(*ev);
 788         len -= sizeof(*ev);
 789
 790         packet_hexdump(buf, len);
 791 }
 792
 793 void control_message(uint16_t opcode, const void *data, uint16_t size)
 794 {
 795         switch (opcode) {
 796         case MGMT_EV_INDEX_ADDED:
 797                 mgmt_index_added(size, data);
 798                 break;
 799         case MGMT_EV_INDEX_REMOVED:
 800                 mgmt_index_removed(size, data);
 801                 break;
 802         case MGMT_EV_CONTROLLER_ERROR:
 803                 mgmt_controller_error(size, data);
 804                 break;
 805         case MGMT_EV_NEW_SETTINGS:
 806                 mgmt_new_settings(size, data);
 807                 break;
 808         case MGMT_EV_CLASS_OF_DEV_CHANGED:
 809                 mgmt_class_of_dev_changed(size, data);
 810                 break;
 811         case MGMT_EV_LOCAL_NAME_CHANGED:
 812                 mgmt_local_name_changed(size, data);
 813                 break;
 814         case MGMT_EV_NEW_LINK_KEY:
 815                 mgmt_new_link_key(size, data);
 816                 break;
 817         case MGMT_EV_NEW_LONG_TERM_KEY:
 818                 mgmt_new_long_term_key(size, data);
 819                 break;
 820         case MGMT_EV_DEVICE_CONNECTED:
 821                 mgmt_device_connected(size, data);
 822                 break;
 823         case MGMT_EV_DEVICE_DISCONNECTED:
 824                 mgmt_device_disconnected(size, data);
 825                 break;
 826         case MGMT_EV_CONNECT_FAILED:
 827                 mgmt_connect_failed(size, data);
 828                 break;
 829         case MGMT_EV_PIN_CODE_REQUEST:
 830                 mgmt_pin_code_request(size, data);
 831                 break;
 832         case MGMT_EV_USER_CONFIRM_REQUEST:
 833                 mgmt_user_confirm_request(size, data);
 834                 break;
 835         case MGMT_EV_USER_PASSKEY_REQUEST:
 836                 mgmt_user_passkey_request(size, data);
 837                 break;
 838         case MGMT_EV_AUTH_FAILED:
 839                 mgmt_auth_failed(size, data);
 840                 break;
 841         case MGMT_EV_DEVICE_FOUND:
 842                 mgmt_device_found(size, data);
 843                 break;
 844         case MGMT_EV_DISCOVERING:
 845                 mgmt_discovering(size, data);
 846                 break;
 847         case MGMT_EV_DEVICE_BLOCKED:
 848                 mgmt_device_blocked(size, data);
 849                 break;
 850         case MGMT_EV_DEVICE_UNBLOCKED:
 851                 mgmt_device_unblocked(size, data);
 852                 break;
 853         case MGMT_EV_DEVICE_UNPAIRED:
 854                 mgmt_device_unpaired(size, data);
 855                 break;
 856         case MGMT_EV_PASSKEY_NOTIFY:
 857                 mgmt_passkey_notify(size, data);
 858                 break;
 859         case MGMT_EV_NEW_IRK:
 860                 mgmt_new_irk(size, data);
 861                 break;
 862         case MGMT_EV_NEW_CSRK:
 863                 mgmt_new_csrk(size, data);
 864                 break;
 865         case MGMT_EV_DEVICE_ADDED:
 866                 mgmt_device_added(size, data);
 867                 break;
 868         case MGMT_EV_DEVICE_REMOVED:
 869                 mgmt_device_removed(size, data);
 870                 break;
 871         case MGMT_EV_NEW_CONN_PARAM:
 872                 mgmt_new_conn_param(size, data);
 873                 break;
 874         case MGMT_EV_UNCONF_INDEX_ADDED:
 875                 mgmt_unconf_index_added(size, data);
 876                 break;
 877         case MGMT_EV_UNCONF_INDEX_REMOVED:
 878                 mgmt_unconf_index_removed(size, data);
 879                 break;
 880         case MGMT_EV_NEW_CONFIG_OPTIONS:
 881                 mgmt_new_config_options(size, data);
 882                 break;
 883         case MGMT_EV_EXT_INDEX_ADDED:
 884                 mgmt_ext_index_added(size, data);
 885                 break;
 886         case MGMT_EV_EXT_INDEX_REMOVED:
 887                 mgmt_ext_index_removed(size, data);
 888                 break;
 889         case MGMT_EV_ADVERTISING_ADDED:
 890                 mgmt_advertising_added(size, data);
 891                 break;
 892         case MGMT_EV_ADVERTISING_REMOVED:
 893                 mgmt_advertising_removed(size, data);
 894                 break;
 895         default:
 896                 printf("* Unknown control (code %d len %d)\n", opcode, size);
 897                 packet_hexdump(data, size);
 898                 break;
 899         }
 900 }
 901
 902 static void data_callback(int fd, uint32_t events, void *user_data)
 903 {
 904         struct control_data *data = user_data;
 905         unsigned char control[64];
 906         struct mgmt_hdr hdr;
 907         struct msghdr msg;
 908         struct iovec iov[2];
 909
 910         if (events & (EPOLLERR | EPOLLHUP)) {
 911                 mainloop_remove_fd(data->fd);
 912                 return;
 913         }
 914
 915         iov[0].iov_base = &hdr;
 916         iov[0].iov_len = MGMT_HDR_SIZE;
 917         iov[1].iov_base = data->buf;
 918         iov[1].iov_len = sizeof(data->buf);
 919
 920         memset(&msg, 0, sizeof(msg));
 921         msg.msg_iov = iov;
 922         msg.msg_iovlen = 2;
 923         msg.msg_control = control;
 924         msg.msg_controllen = sizeof(control);
 925
 926         while (1) {
 927                 struct cmsghdr *cmsg;
 928                 struct timeval *tv = NULL;
 929                 struct timeval ctv;
 930                 struct ucred *cred = NULL;
 931                 struct ucred ccred;
 932                 uint16_t opcode, index, pktlen;
 933                 ssize_t len;
 934
 935                 len = recvmsg(data->fd, &msg, MSG_DONTWAIT);
 936                 if (len < 0)
 937                         break;
 938
 939                 if (len < MGMT_HDR_SIZE)
 940                         break;
 941
 942                 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 943                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 944                         if (cmsg->cmsg_level != SOL_SOCKET)
 945                                 continue;
 946
 947                         if (cmsg->cmsg_type == SCM_TIMESTAMP) {
 948                                 memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
 949                                 tv = &ctv;
 950                         }
 951
 952                         if (cmsg->cmsg_type == SCM_CREDENTIALS) {
 953                                 memcpy(&ccred, CMSG_DATA(cmsg), sizeof(ccred));
 954                                 cred = &ccred;
 955                         }
 956                 }
 957
 958                 opcode = le16_to_cpu(hdr.opcode);
 959                 index  = le16_to_cpu(hdr.index);
 960                 pktlen = le16_to_cpu(hdr.len);
 961
 962                 switch (data->channel) {
 963                 case HCI_CHANNEL_CONTROL:
 964                         packet_control(tv, cred, index, opcode,
 965                                                         data->buf, pktlen);
 966                         break;
 967                 case HCI_CHANNEL_MONITOR:
 968                         btsnoop_write_hci(btsnoop_file, tv, index, opcode,
 969                                                         data->buf, pktlen);
 970                         ellisys_inject_hci(tv, index, opcode,
 971                                                         data->buf, pktlen);
 972                         packet_monitor(tv, cred, index, opcode,
 973                                                         data->buf, pktlen);
 974                         break;
 975                 }
 976         }
 977 }
 978
 979 static int open_socket(uint16_t channel)
 980 {
 981         struct sockaddr_hci addr;
 982         int fd, opt = 1;
 983
 984         fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI);
 985         if (fd < 0) {
 986                 perror("Failed to open channel");
 987                 return -1;
 988         }
 989
 990         memset(&addr, 0, sizeof(addr));
 991         addr.hci_family = AF_BLUETOOTH;
 992         addr.hci_dev = HCI_DEV_NONE;
 993         addr.hci_channel = channel;
 994
 995         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 996                 if (errno == EINVAL) {
 997                         /* Fallback to hcidump support */
 998                         hcidump_fallback = true;
 999                         close(fd);
1000                         return -1;
1001                 }
1002                 perror("Failed to bind channel");
1003                 close(fd);
1004                 return -1;
1005         }
1006
1007         if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) {
1008                 perror("Failed to enable timestamps");
1009                 close(fd);
1010                 return -1;
1011         }
1012
1013         if (setsockopt(fd, SOL_SOCKET, SO_PASSCRED, &opt, sizeof(opt)) < 0) {
1014                 perror("Failed to enable credentials");
1015                 close(fd);
1016                 return -1;
1017         }
1018
1019         return fd;
1020 }
1021
1022 static int open_channel(uint16_t channel)
1023 {
1024         struct control_data *data;
1025
1026         data = malloc(sizeof(*data));
1027         if (!data)
1028                 return -1;
1029
1030         memset(data, 0, sizeof(*data));
1031         data->channel = channel;
1032
1033         data->fd = open_socket(channel);
1034         if (data->fd < 0) {
1035                 free(data);
1036                 return -1;
1037         }
1038
1039         mainloop_add_fd(data->fd, EPOLLIN, data_callback, data, free_data);
1040
1041         return 0;
1042 }
1043
1044 static void client_callback(int fd, uint32_t events, void *user_data)
1045 {
1046         struct control_data *data = user_data;
1047         ssize_t len;
1048
1049         if (events & (EPOLLERR | EPOLLHUP)) {
1050                 mainloop_remove_fd(data->fd);
1051                 return;
1052         }
1053
1054         len = recv(data->fd, data->buf + data->offset,
1055                         sizeof(data->buf) - data->offset, MSG_DONTWAIT);
1056         if (len < 0)
1057                 return;
1058
1059         data->offset += len;
1060
1061         if (data->offset > MGMT_HDR_SIZE) {
1062                 struct mgmt_hdr *hdr = (struct mgmt_hdr *) data->buf;
1063                 uint16_t pktlen = le16_to_cpu(hdr->len);
1064
1065                 if (data->offset > pktlen + MGMT_HDR_SIZE) {
1066                         uint16_t opcode = le16_to_cpu(hdr->opcode);
1067                         uint16_t index = le16_to_cpu(hdr->index);
1068
1069                         packet_monitor(NULL, NULL, index, opcode,
1070                                         data->buf + MGMT_HDR_SIZE, pktlen);
1071
1072                         data->offset -= pktlen + MGMT_HDR_SIZE;
1073
1074                         if (data->offset > 0)
1075                                 memmove(data->buf, data->buf +
1076                                          MGMT_HDR_SIZE + pktlen, data->offset);
1077                 }
1078         }
1079 }
1080
1081 static void server_accept_callback(int fd, uint32_t events, void *user_data)
1082 {
1083         struct control_data *data;
1084         struct sockaddr_un addr;
1085         socklen_t len;
1086         int nfd;
1087
1088         if (events & (EPOLLERR | EPOLLHUP)) {
1089                 mainloop_remove_fd(fd);
1090                 return;
1091         }
1092
1093         memset(&addr, 0, sizeof(addr));
1094         len = sizeof(addr);
1095
1096         nfd = accept(fd, (struct sockaddr *) &addr, &len);
1097         if (nfd < 0) {
1098                 perror("Failed to accept client socket");
1099                 return;
1100         }
1101
1102         printf("--- New monitor connection ---\n");
1103
1104         data = malloc(sizeof(*data));
1105         if (!data) {
1106                 close(nfd);
1107                 return;
1108         }
1109
1110         memset(data, 0, sizeof(*data));
1111         data->channel = HCI_CHANNEL_MONITOR;
1112         data->fd = nfd;
1113
1114         mainloop_add_fd(data->fd, EPOLLIN, client_callback, data, free_data);
1115 }
1116
1117 static int server_fd = -1;
1118
1119 void control_server(const char *path)
1120 {
1121         struct sockaddr_un addr;
1122         int fd;
1123
1124         if (server_fd >= 0)
1125                 return;
1126
1127         unlink(path);
1128
1129         fd = socket(PF_UNIX, SOCK_STREAM | SOCK_CLOEXEC, 0);
1130         if (fd < 0) {
1131                 perror("Failed to open server socket");
1132                 return;
1133         }
1134
1135         memset(&addr, 0, sizeof(addr));
1136         addr.sun_family = AF_UNIX;
1137         strcpy(addr.sun_path, path);
1138
1139         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
1140                 perror("Failed to bind server socket");
1141                 close(fd);
1142                 return;
1143         }
1144
1145         if (listen(fd, 5) < 0) {
1146                 perror("Failed to listen server socket");
1147                 close(fd);
1148                 return;
1149         }
1150
1151         if (mainloop_add_fd(fd, EPOLLIN, server_accept_callback,
1152                                                 NULL, NULL) < 0) {
1153                 close(fd);
1154                 return;
1155         }
1156
1157         server_fd = fd;
1158 }
1159
1160 #ifdef __TIZEN_PATCH__
1161 bool control_writer(const char *path, int16_t rotate_count, ssize_t file_size)
1162 #else
1163 bool control_writer(const char *path)
1164 #endif
1165 {
1166 #ifdef __TIZEN_PATCH__
1167         btsnoop_file = btsnoop_create(path, BTSNOOP_FORMAT_MONITOR,
1168                         rotate_count, file_size);
1169 #else
1170         btsnoop_file = btsnoop_create(path, BTSNOOP_FORMAT_MONITOR);
1171 #endif
1172
1173         return !!btsnoop_file;
1174 }
1175
1176 void control_reader(const char *path)
1177 {
1178         unsigned char buf[BTSNOOP_MAX_PACKET_SIZE];
1179         uint16_t pktlen;
1180         uint32_t format;
1181         struct timeval tv;
1182
1183         btsnoop_file = btsnoop_open(path, BTSNOOP_FLAG_PKLG_SUPPORT);
1184         if (!btsnoop_file)
1185                 return;
1186
1187         format = btsnoop_get_format(btsnoop_file);
1188
1189         switch (format) {
1190         case BTSNOOP_FORMAT_HCI:
1191         case BTSNOOP_FORMAT_UART:
1192         case BTSNOOP_FORMAT_SIMULATOR:
1193                 packet_del_filter(PACKET_FILTER_SHOW_INDEX);
1194                 break;
1195
1196         case BTSNOOP_FORMAT_MONITOR:
1197                 packet_add_filter(PACKET_FILTER_SHOW_INDEX);
1198                 break;
1199         }
1200
1201 #ifdef __TIZEN_PATCH__
1202         setenv("PAGER", "cat", 0);
1203 #endif
1204         open_pager();
1205
1206         switch (format) {
1207         case BTSNOOP_FORMAT_HCI:
1208         case BTSNOOP_FORMAT_UART:
1209         case BTSNOOP_FORMAT_MONITOR:
1210                 while (1) {
1211                         uint16_t index, opcode;
1212
1213                         if (!btsnoop_read_hci(btsnoop_file, &tv, &index,
1214                                                         &opcode, buf, &pktlen))
1215                                 break;
1216
1217                         if (opcode == 0xffff)
1218                                 continue;
1219
1220                         packet_monitor(&tv, NULL, index, opcode, buf, pktlen);
1221                         ellisys_inject_hci(&tv, index, opcode, buf, pktlen);
1222                 }
1223                 break;
1224
1225         case BTSNOOP_FORMAT_SIMULATOR:
1226                 while (1) {
1227                         uint16_t frequency;
1228
1229                         if (!btsnoop_read_phy(btsnoop_file, &tv, &frequency,
1230                                                                 buf, &pktlen))
1231                                 break;
1232
1233                         packet_simulator(&tv, frequency, buf, pktlen);
1234                 }
1235                 break;
1236         }
1237
1238         close_pager();
1239
1240         btsnoop_unref(btsnoop_file);
1241 }
1242
1243 int control_tracing(void)
1244 {
1245         packet_add_filter(PACKET_FILTER_SHOW_INDEX);
1246
1247         if (server_fd >= 0)
1248                 return 0;
1249
1250         if (open_channel(HCI_CHANNEL_MONITOR) < 0) {
1251                 if (!hcidump_fallback)
1252                         return -1;
1253                 if (hcidump_tracing() < 0)
1254                         return -1;
1255                 return 0;
1256         }
1257
1258         open_channel(HCI_CHANNEL_CONTROL);
1259
1260         return 0;
1261 }