mobile/monitor/control.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program 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
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; 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 <errno.h>
  31 #include <unistd.h>
  32 #include <stdlib.h>
  33 #include <string.h>
  34 #include <sys/time.h>
  35
  36 #include <bluetooth/bluetooth.h>
  37 #include <bluetooth/hci.h>
  38 #include <bluetooth/mgmt.h>
  39
  40 #include "mainloop.h"
  41 #include "packet.h"
  42 #include "control.h"
  43
  44 struct control_data {
  45         uint16_t channel;
  46         int fd;
  47 };
  48
  49 static void free_data(void *user_data)
  50 {
  51         struct control_data *data = user_data;
  52
  53         close(data->fd);
  54
  55         free(data);
  56 }
  57
  58 static void mgmt_index_added(uint16_t len, const void *buf)
  59 {
  60         printf("@ Index Added\n");
  61
  62         packet_hexdump(buf, len);
  63 }
  64
  65 static void mgmt_index_removed(uint16_t len, const void *buf)
  66 {
  67         printf("@ Index Removed\n");
  68
  69         packet_hexdump(buf, len);
  70 }
  71
  72 static void mgmt_controller_error(uint16_t len, const void *buf)
  73 {
  74         const struct mgmt_ev_controller_error *ev = buf;
  75
  76         if (len < sizeof(*ev)) {
  77                 printf("* Malformed Controller Error control\n");
  78                 return;
  79         }
  80
  81         printf("@ Controller Error: 0x%2.2x\n", ev->error_code);
  82
  83         buf += sizeof(*ev);
  84         len -= sizeof(*ev);
  85
  86         packet_hexdump(buf, len);
  87 }
  88
  89 #ifndef NELEM
  90 #define NELEM(x) (sizeof(x) / sizeof((x)[0]))
  91 #endif
  92
  93 static const char *settings_str[] = {
  94         "powered", "connectable", "fast-connectable", "discoverable",
  95         "pairable", "link-security", "ssp", "br/edr", "hs", "le"
  96 };
  97
  98 static void mgmt_new_settings(uint16_t len, const void *buf)
  99 {
 100         uint32_t settings;
 101         unsigned int i;
 102
 103         if (len < 4) {
 104                 printf("* Malformed New Settings control\n");
 105                 return;
 106         }
 107
 108         settings = bt_get_le32(buf);
 109
 110         printf("@ New Settings: 0x%4.4x\n", settings);
 111
 112         printf("%-12c", ' ');
 113         for (i = 0; i < NELEM(settings_str); i++) {
 114                 if (settings & (1 << i))
 115                         printf("%s ", settings_str[i]);
 116         }
 117         printf("\n");
 118
 119         buf += 4;
 120         len -= 4;
 121
 122         packet_hexdump(buf, len);
 123 }
 124
 125 static void mgmt_class_of_dev_changed(uint16_t len, const void *buf)
 126 {
 127         const struct mgmt_ev_class_of_dev_changed *ev = buf;
 128
 129         if (len < sizeof(*ev)) {
 130                 printf("* Malformed Class of Device Changed control\n");
 131                 return;
 132         }
 133
 134         printf("@ Class of Device Changed: 0x%2.2x%2.2x%2.2x\n",
 135                                                 ev->class_of_dev[2],
 136                                                 ev->class_of_dev[1],
 137                                                 ev->class_of_dev[0]);
 138
 139         buf += sizeof(*ev);
 140         len -= sizeof(*ev);
 141
 142         packet_hexdump(buf, len);
 143 }
 144
 145 static void mgmt_local_name_changed(uint16_t len, const void *buf)
 146 {
 147         const struct mgmt_ev_local_name_changed *ev = buf;
 148
 149         if (len < sizeof(*ev)) {
 150                 printf("* Malformed Local Name Changed control\n");
 151                 return;
 152         }
 153
 154         printf("@ Local Name Changed: %s (%s)\n", ev->name, ev->short_name);
 155
 156         buf += sizeof(*ev);
 157         len -= sizeof(*ev);
 158
 159         packet_hexdump(buf, len);
 160 }
 161
 162 static void mgmt_new_link_key(uint16_t len, const void *buf)
 163 {
 164         const struct mgmt_ev_new_link_key *ev = buf;
 165         char str[18];
 166
 167         if (len < sizeof(*ev)) {
 168                 printf("* Malformed New Link Key control\n");
 169                 return;
 170         }
 171
 172         ba2str(&ev->key.addr.bdaddr, str);
 173
 174         printf("@ New Link Key: %s (%d)\n", str, ev->key.addr.type);
 175
 176         buf += sizeof(*ev);
 177         len -= sizeof(*ev);
 178
 179         packet_hexdump(buf, len);
 180 }
 181
 182 static void mgmt_new_long_term_key(uint16_t len, const void *buf)
 183 {
 184         const struct mgmt_ev_new_long_term_key *ev = buf;
 185         char str[18];
 186
 187         if (len < sizeof(*ev)) {
 188                 printf("* Malformed New Long Term Key control\n");
 189                 return;
 190         }
 191
 192         ba2str(&ev->key.addr.bdaddr, str);
 193
 194         printf("@ New Long Term Key: %s (%d)\n", str, ev->key.addr.type);
 195
 196         buf += sizeof(*ev);
 197         len -= sizeof(*ev);
 198
 199         packet_hexdump(buf, len);
 200 }
 201
 202 static void mgmt_device_connected(uint16_t len, const void *buf)
 203 {
 204         const struct mgmt_ev_device_connected *ev = buf;
 205         uint32_t flags;
 206         char str[18];
 207
 208         if (len < sizeof(*ev)) {
 209                 printf("* Malformed Device Connected control\n");
 210                 return;
 211         }
 212
 213         flags = btohs(ev->flags);
 214         ba2str(&ev->addr.bdaddr, str);
 215
 216         printf("@ Device Connected: %s (%d) flags 0x%4.4x\n",
 217                                                 str, ev->addr.type, flags);
 218
 219         buf += sizeof(*ev);
 220         len -= sizeof(*ev);
 221
 222         packet_hexdump(buf, len);
 223 }
 224
 225 static void mgmt_device_disconnected(uint16_t len, const void *buf)
 226 {
 227         const struct mgmt_ev_device_disconnected *ev = buf;
 228         char str[18];
 229
 230         if (len < sizeof(*ev)) {
 231                 printf("* Malformed Device Disconnected control\n");
 232                 return;
 233         }
 234
 235         ba2str(&ev->addr.bdaddr, str);
 236
 237         printf("@ Device Disconnected: %s (%d)\n", str, ev->addr.type);
 238
 239         buf += sizeof(*ev);
 240         len -= sizeof(*ev);
 241
 242         packet_hexdump(buf, len);
 243 }
 244
 245 static void mgmt_connect_failed(uint16_t len, const void *buf)
 246 {
 247         const struct mgmt_ev_connect_failed *ev = buf;
 248         char str[18];
 249
 250         if (len < sizeof(*ev)) {
 251                 printf("* Malformed Connect Failed control\n");
 252                 return;
 253         }
 254
 255         ba2str(&ev->addr.bdaddr, str);
 256
 257         printf("@ Connect Failed: %s (%d) status 0x%2.2x\n",
 258                                         str, ev->addr.type, ev->status);
 259
 260         buf += sizeof(*ev);
 261         len -= sizeof(*ev);
 262
 263         packet_hexdump(buf, len);
 264 }
 265
 266 static void mgmt_pin_code_request(uint16_t len, const void *buf)
 267 {
 268         const struct mgmt_ev_pin_code_request *ev = buf;
 269         char str[18];
 270
 271         if (len < sizeof(*ev)) {
 272                 printf("* Malformed PIN Code Request control\n");
 273                 return;
 274         }
 275
 276         ba2str(&ev->addr.bdaddr, str);
 277
 278         printf("@ PIN Code Request: %s (%d) secure 0x%2.2x\n",
 279                                         str, ev->addr.type, ev->secure);
 280
 281         buf += sizeof(*ev);
 282         len -= sizeof(*ev);
 283
 284         packet_hexdump(buf, len);
 285 }
 286
 287 static void mgmt_user_confirm_request(uint16_t len, const void *buf)
 288 {
 289         const struct mgmt_ev_user_confirm_request *ev = buf;
 290         char str[18];
 291
 292         if (len < sizeof(*ev)) {
 293                 printf("* Malformed User Confirmation Request control\n");
 294                 return;
 295         }
 296
 297         ba2str(&ev->addr.bdaddr, str);
 298
 299         printf("@ User Confirmation Request: %s (%d) hint %d value %d\n",
 300                         str, ev->addr.type, ev->confirm_hint, ev->value);
 301
 302         buf += sizeof(*ev);
 303         len -= sizeof(*ev);
 304
 305         packet_hexdump(buf, len);
 306 }
 307
 308 static void mgmt_user_passkey_request(uint16_t len, const void *buf)
 309 {
 310         const struct mgmt_ev_user_passkey_request *ev = buf;
 311         char str[18];
 312
 313         if (len < sizeof(*ev)) {
 314                 printf("* Malformed User Passkey Request control\n");
 315                 return;
 316         }
 317
 318         ba2str(&ev->addr.bdaddr, str);
 319
 320         printf("@ PIN User Passkey Request: %s (%d)\n", str, ev->addr.type);
 321
 322         buf += sizeof(*ev);
 323         len -= sizeof(*ev);
 324
 325         packet_hexdump(buf, len);
 326 }
 327
 328 static void mgmt_auth_failed(uint16_t len, const void *buf)
 329 {
 330         const struct mgmt_ev_auth_failed *ev = buf;
 331         char str[18];
 332
 333         if (len < sizeof(*ev)) {
 334                 printf("* Malformed Authentication Failed control\n");
 335                 return;
 336         }
 337
 338         ba2str(&ev->addr.bdaddr, str);
 339
 340         printf("@ Authentication Failed: %s (%d) status 0x%2.2x\n",
 341                                         str, ev->addr.type, ev->status);
 342
 343         buf += sizeof(*ev);
 344         len -= sizeof(*ev);
 345
 346         packet_hexdump(buf, len);
 347 }
 348
 349 static void mgmt_device_found(uint16_t len, const void *buf)
 350 {
 351         const struct mgmt_ev_device_found *ev = buf;
 352         uint32_t flags;
 353         char str[18];
 354
 355         if (len < sizeof(*ev)) {
 356                 printf("* Malformed Device Found control\n");
 357                 return;
 358         }
 359
 360         flags = btohs(ev->flags);
 361         ba2str(&ev->addr.bdaddr, str);
 362
 363         printf("@ Device Found: %s (%d) rssi %d flags 0x%4.4x\n",
 364                                         str, ev->addr.type, ev->rssi, flags);
 365
 366         buf += sizeof(*ev);
 367         len -= sizeof(*ev);
 368
 369         packet_hexdump(buf, len);
 370 }
 371
 372 static void mgmt_discovering(uint16_t len, const void *buf)
 373 {
 374         const struct mgmt_ev_discovering *ev = buf;
 375
 376         if (len < sizeof(*ev)) {
 377                 printf("* Malformed Discovering control\n");
 378                 return;
 379         }
 380
 381         printf("@ Discovering: 0x%2.2x (%d)\n", ev->discovering, ev->type);
 382
 383         buf += sizeof(*ev);
 384         len -= sizeof(*ev);
 385
 386         packet_hexdump(buf, len);
 387 }
 388
 389 static void mgmt_device_blocked(uint16_t len, const void *buf)
 390 {
 391         const struct mgmt_ev_device_blocked *ev = buf;
 392         char str[18];
 393
 394         if (len < sizeof(*ev)) {
 395                 printf("* Malformed Device Blocked control\n");
 396                 return;
 397         }
 398
 399         ba2str(&ev->addr.bdaddr, str);
 400
 401         printf("@ Device Blocked: %s (%d)\n", str, ev->addr.type);
 402
 403         buf += sizeof(*ev);
 404         len -= sizeof(*ev);
 405
 406         packet_hexdump(buf, len);
 407 }
 408
 409 static void mgmt_device_unblocked(uint16_t len, const void *buf)
 410 {
 411         const struct mgmt_ev_device_unblocked *ev = buf;
 412         char str[18];
 413
 414         if (len < sizeof(*ev)) {
 415                 printf("* Malformed Device Unblocked control\n");
 416                 return;
 417         }
 418
 419         ba2str(&ev->addr.bdaddr, str);
 420
 421         printf("@ Device Unblocked: %s (%d)\n", str, ev->addr.type);
 422
 423         buf += sizeof(*ev);
 424         len -= sizeof(*ev);
 425
 426         packet_hexdump(buf, len);
 427 }
 428
 429 static void mgmt_device_unpaired(uint16_t len, const void *buf)
 430 {
 431         const struct mgmt_ev_device_unpaired *ev = buf;
 432         char str[18];
 433
 434         if (len < sizeof(*ev)) {
 435                 printf("* Malformed Device Unpaired control\n");
 436                 return;
 437         }
 438
 439         ba2str(&ev->addr.bdaddr, str);
 440
 441         printf("@ Device Unpaired: %s (%d)\n", str, ev->addr.type);
 442
 443         buf += sizeof(*ev);
 444         len -= sizeof(*ev);
 445
 446         packet_hexdump(buf, len);
 447 }
 448
 449 void control_message(uint16_t opcode, const void *data, uint16_t size)
 450 {
 451         switch (opcode) {
 452         case MGMT_EV_INDEX_ADDED:
 453                 mgmt_index_added(size, data);
 454                 break;
 455         case MGMT_EV_INDEX_REMOVED:
 456                 mgmt_index_removed(size, data);
 457                 break;
 458         case MGMT_EV_CONTROLLER_ERROR:
 459                 mgmt_controller_error(size, data);
 460                 break;
 461         case MGMT_EV_NEW_SETTINGS:
 462                 mgmt_new_settings(size, data);
 463                 break;
 464         case MGMT_EV_CLASS_OF_DEV_CHANGED:
 465                 mgmt_class_of_dev_changed(size, data);
 466                 break;
 467         case MGMT_EV_LOCAL_NAME_CHANGED:
 468                 mgmt_local_name_changed(size, data);
 469                 break;
 470         case MGMT_EV_NEW_LINK_KEY:
 471                 mgmt_new_link_key(size, data);
 472                 break;
 473         case MGMT_EV_NEW_LONG_TERM_KEY:
 474                 mgmt_new_long_term_key(size, data);
 475                 break;
 476         case MGMT_EV_DEVICE_CONNECTED:
 477                 mgmt_device_connected(size, data);
 478                 break;
 479         case MGMT_EV_DEVICE_DISCONNECTED:
 480                 mgmt_device_disconnected(size, data);
 481                 break;
 482         case MGMT_EV_CONNECT_FAILED:
 483                 mgmt_connect_failed(size, data);
 484                 break;
 485         case MGMT_EV_PIN_CODE_REQUEST:
 486                 mgmt_pin_code_request(size, data);
 487                 break;
 488         case MGMT_EV_USER_CONFIRM_REQUEST:
 489                 mgmt_user_confirm_request(size, data);
 490                 break;
 491         case MGMT_EV_USER_PASSKEY_REQUEST:
 492                 mgmt_user_passkey_request(size, data);
 493                 break;
 494         case MGMT_EV_AUTH_FAILED:
 495                 mgmt_auth_failed(size, data);
 496                 break;
 497         case MGMT_EV_DEVICE_FOUND:
 498                 mgmt_device_found(size, data);
 499                 break;
 500         case MGMT_EV_DISCOVERING:
 501                 mgmt_discovering(size, data);
 502                 break;
 503         case MGMT_EV_DEVICE_BLOCKED:
 504                 mgmt_device_blocked(size, data);
 505                 break;
 506         case MGMT_EV_DEVICE_UNBLOCKED:
 507                 mgmt_device_unblocked(size, data);
 508                 break;
 509         case MGMT_EV_DEVICE_UNPAIRED:
 510                 mgmt_device_unpaired(size, data);
 511                 break;
 512         default:
 513                 printf("* Unknown control (code %d len %d)\n", opcode, size);
 514                 packet_hexdump(data, size);
 515                 break;
 516         }
 517 }
 518
 519 static void data_callback(int fd, uint32_t events, void *user_data)
 520 {
 521         struct control_data *data = user_data;
 522         unsigned char buf[HCI_MAX_FRAME_SIZE];
 523         unsigned char control[32];
 524         struct mgmt_hdr hdr;
 525         struct msghdr msg;
 526         struct iovec iov[2];
 527
 528         if (events & (EPOLLERR | EPOLLHUP)) {
 529                 mainloop_remove_fd(fd);
 530                 return;
 531         }
 532
 533         iov[0].iov_base = &hdr;
 534         iov[0].iov_len = MGMT_HDR_SIZE;
 535         iov[1].iov_base = buf;
 536         iov[1].iov_len = sizeof(buf);
 537
 538         memset(&msg, 0, sizeof(msg));
 539         msg.msg_iov = iov;
 540         msg.msg_iovlen = 2;
 541         msg.msg_control = control;
 542         msg.msg_controllen = sizeof(control);
 543
 544         while (1) {
 545                 struct cmsghdr *cmsg;
 546                 struct timeval *tv = NULL;
 547                 uint16_t opcode, index, pktlen;
 548                 ssize_t len;
 549
 550                 len = recvmsg(fd, &msg, MSG_DONTWAIT);
 551                 if (len < 0)
 552                         break;
 553
 554                 if (len < MGMT_HDR_SIZE)
 555                         break;
 556
 557                 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 558                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 559                         if (cmsg->cmsg_level != SOL_SOCKET)
 560                                 continue;
 561
 562                         if (cmsg->cmsg_type == SCM_TIMESTAMP)
 563                                 tv = (struct timeval *) CMSG_DATA(cmsg);
 564                 }
 565
 566                 opcode = btohs(hdr.opcode);
 567                 index  = btohs(hdr.index);
 568                 pktlen = btohs(hdr.len);
 569
 570                 switch (data->channel) {
 571                 case HCI_CHANNEL_CONTROL:
 572                         packet_control(tv, index, opcode, buf, pktlen);
 573                         break;
 574                 case HCI_CHANNEL_MONITOR:
 575                         packet_monitor(tv, index, opcode, buf, pktlen);
 576                         break;
 577                 }
 578         }
 579 }
 580
 581 static int open_socket(uint16_t channel)
 582 {
 583         struct sockaddr_hci addr;
 584         int fd, opt = 1;
 585
 586         fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
 587         if (fd < 0) {
 588                 perror("Failed to open channel");
 589                 return -1;
 590         }
 591
 592         memset(&addr, 0, sizeof(addr));
 593         addr.hci_family = AF_BLUETOOTH;
 594         addr.hci_dev = HCI_DEV_NONE;
 595         addr.hci_channel = channel;
 596
 597         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 598                 if (errno == EINVAL) {
 599                         /* Fallback to hcidump support */
 600                         close(fd);
 601                         return -1;
 602                 }
 603                 perror("Failed to bind channel");
 604                 close(fd);
 605                 return -1;
 606         }
 607
 608         if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) {
 609                 perror("Failed to enable timestamps");
 610                 close(fd);
 611                 return -1;
 612         }
 613
 614         return fd;
 615 }
 616
 617 static int open_channel(uint16_t channel)
 618 {
 619         struct control_data *data;
 620
 621         data = malloc(sizeof(*data));
 622         if (!data)
 623                 return -1;
 624
 625         memset(data, 0, sizeof(*data));
 626         data->channel = channel;
 627
 628         data->fd = open_socket(channel);
 629         if (data->fd < 0) {
 630                 free(data);
 631                 return -1;
 632         }
 633
 634         mainloop_add_fd(data->fd, EPOLLIN, data_callback, data, free_data);
 635
 636         return 0;
 637 }
 638
 639 int control_tracing(void)
 640 {
 641         if (open_channel(HCI_CHANNEL_MONITOR) < 0)
 642                 return -1;
 643
 644         open_channel(HCI_CHANNEL_CONTROL);
 645
 646         return 0;
 647 }