monitor/hcidump.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2014  Intel Corporation
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <stdio.h>
  17 #include <errno.h>
  18 #include <unistd.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21 #include <sys/ioctl.h>
  22 #include <sys/socket.h>
  23
  24 #include "lib/bluetooth.h"
  25 #include "lib/hci.h"
  26 #include "lib/hci_lib.h"
  27
  28 #include "src/shared/mainloop.h"
  29
  30 #include "packet.h"
  31 #include "hcidump.h"
  32
  33 struct hcidump_data {
  34         uint16_t index;
  35         int fd;
  36 };
  37
  38 static void free_data(void *user_data)
  39 {
  40         struct hcidump_data *data = user_data;
  41
  42         close(data->fd);
  43
  44         free(data);
  45 }
  46
  47 static int open_hci_dev(uint16_t index)
  48 {
  49         struct sockaddr_hci addr;
  50         struct hci_filter flt;
  51         int fd, opt = 1;
  52
  53         fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI);
  54         if (fd < 0) {
  55                 perror("Failed to open channel");
  56                 return -1;
  57         }
  58
  59         /* Setup filter */
  60         hci_filter_clear(&flt);
  61         hci_filter_all_ptypes(&flt);
  62         hci_filter_all_events(&flt);
  63
  64         if (setsockopt(fd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
  65                 perror("Failed to set HCI filter");
  66                 close(fd);
  67                 return -1;
  68         }
  69
  70         if (setsockopt(fd, SOL_HCI, HCI_DATA_DIR, &opt, sizeof(opt)) < 0) {
  71                 perror("Failed to enable HCI data direction info");
  72                 close(fd);
  73                 return -1;
  74         }
  75
  76         if (setsockopt(fd, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
  77                 perror("Failed to enable HCI time stamps");
  78                 close(fd);
  79                 return -1;
  80         }
  81
  82         memset(&addr, 0, sizeof(addr));
  83         addr.hci_family = AF_BLUETOOTH;
  84         addr.hci_dev = index;
  85         addr.hci_channel = HCI_CHANNEL_RAW;
  86
  87         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
  88                 perror("Failed to bind channel");
  89                 close(fd);
  90                 return -1;
  91         }
  92
  93         return fd;
  94 }
  95
  96 static void device_callback(int fd, uint32_t events, void *user_data)
  97 {
  98         struct hcidump_data *data = user_data;
  99         unsigned char buf[HCI_MAX_FRAME_SIZE * 2];
 100         unsigned char control[64];
 101         struct msghdr msg;
 102         struct iovec iov;
 103
 104         if (events & (EPOLLERR | EPOLLHUP)) {
 105                 mainloop_remove_fd(fd);
 106                 return;
 107         }
 108
 109         iov.iov_base = buf;
 110         iov.iov_len = sizeof(buf);
 111
 112         memset(&msg, 0, sizeof(msg));
 113         msg.msg_iov = &iov;
 114         msg.msg_iovlen = 1;
 115         msg.msg_control = control;
 116         msg.msg_controllen = sizeof(control);
 117
 118         while (1) {
 119                 struct cmsghdr *cmsg;
 120                 struct timeval *tv = NULL;
 121                 struct timeval ctv;
 122                 int dir = -1;
 123                 ssize_t len;
 124
 125                 len = recvmsg(fd, &msg, MSG_DONTWAIT);
 126                 if (len < 0)
 127                         break;
 128
 129                 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 130                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 131                         if (cmsg->cmsg_level != SOL_HCI)
 132                                 continue;
 133
 134                         switch (cmsg->cmsg_type) {
 135                         case HCI_DATA_DIR:
 136                                 memcpy(&dir, CMSG_DATA(cmsg), sizeof(dir));
 137                                 break;
 138                         case HCI_CMSG_TSTAMP:
 139                                 memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
 140                                 tv = &ctv;
 141                                 break;
 142                         }
 143                 }
 144
 145                 if (dir < 0 || len < 1)
 146                         continue;
 147
 148                 switch (buf[0]) {
 149                 case HCI_COMMAND_PKT:
 150                         packet_hci_command(tv, NULL, data->index,
 151                                                         buf + 1, len - 1);
 152                         break;
 153                 case HCI_EVENT_PKT:
 154                         packet_hci_event(tv, NULL, data->index,
 155                                                         buf + 1, len - 1);
 156                         break;
 157                 case HCI_ACLDATA_PKT:
 158                         packet_hci_acldata(tv, NULL, data->index, !!dir,
 159                                                         buf + 1, len - 1);
 160                         break;
 161                 case HCI_SCODATA_PKT:
 162                         packet_hci_scodata(tv, NULL, data->index, !!dir,
 163                                                         buf + 1, len - 1);
 164                         break;
 165                 }
 166         }
 167 }
 168
 169 static void open_device(uint16_t index)
 170 {
 171         struct hcidump_data *data;
 172
 173         data = malloc(sizeof(*data));
 174         if (!data)
 175                 return;
 176
 177         memset(data, 0, sizeof(*data));
 178         data->index = index;
 179
 180         data->fd = open_hci_dev(index);
 181         if (data->fd < 0) {
 182                 free(data);
 183                 return;
 184         }
 185
 186         if (mainloop_add_fd(data->fd, EPOLLIN, device_callback,
 187                                                 data, free_data) < 0) {
 188                 close(data->fd);
 189                 free(data);
 190         }
 191 }
 192
 193 static void device_info(int fd, uint16_t index, uint8_t *type, uint8_t *bus,
 194                                                 bdaddr_t *bdaddr, char *name)
 195 {
 196         struct hci_dev_info di;
 197
 198         memset(&di, 0, sizeof(di));
 199         di.dev_id = index;
 200
 201         if (ioctl(fd, HCIGETDEVINFO, (void *) &di) < 0) {
 202                 perror("Failed to get device information");
 203                 return;
 204         }
 205
 206         *type = di.type >> 4;
 207         *bus = di.type & 0x0f;
 208
 209         bacpy(bdaddr, &di.bdaddr);
 210         memcpy(name, di.name, 8);
 211 }
 212
 213 static void device_list(int fd, int max_dev)
 214 {
 215         struct hci_dev_list_req *dl;
 216         struct hci_dev_req *dr;
 217         int i;
 218
 219         dl = malloc(max_dev * sizeof(*dr) + sizeof(*dl));
 220         if (!dl) {
 221                 perror("Failed to allocate device list memory");
 222                 return;
 223         }
 224
 225         memset(dl, 0, max_dev * sizeof(*dr) + sizeof(*dl));
 226         dl->dev_num = max_dev;
 227
 228         dr = dl->dev_req;
 229
 230         if (ioctl(fd, HCIGETDEVLIST, (void *) dl) < 0) {
 231                 perror("Failed to get device list");
 232                 goto done;
 233         }
 234
 235         for (i = 0; i < dl->dev_num; i++, dr++) {
 236                 struct timeval tmp_tv, *tv = NULL;
 237                 uint8_t type = 0xff, bus = 0xff;
 238                 char str[18], name[8] = "";
 239                 bdaddr_t bdaddr;
 240
 241                 bacpy(&bdaddr, BDADDR_ANY);
 242
 243                 if (!gettimeofday(&tmp_tv, NULL))
 244                         tv = &tmp_tv;
 245
 246                 device_info(fd, dr->dev_id, &type, &bus, &bdaddr, name);
 247                 ba2str(&bdaddr, str);
 248                 packet_new_index(tv, dr->dev_id, str, type, bus, name);
 249                 open_device(dr->dev_id);
 250         }
 251
 252 done:
 253         free(dl);
 254 }
 255
 256 static int open_stack_internal(void)
 257 {
 258         struct sockaddr_hci addr;
 259         struct hci_filter flt;
 260         int fd, opt = 1;
 261
 262         fd = socket(AF_BLUETOOTH, SOCK_RAW | SOCK_CLOEXEC, BTPROTO_HCI);
 263         if (fd < 0) {
 264                 perror("Failed to open channel");
 265                 return -1;
 266         }
 267
 268         /* Setup filter */
 269         hci_filter_clear(&flt);
 270         hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
 271         hci_filter_set_event(EVT_STACK_INTERNAL, &flt);
 272
 273         if (setsockopt(fd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
 274                 perror("Failed to set HCI filter");
 275                 close(fd);
 276                 return -1;
 277         }
 278
 279         if (setsockopt(fd, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
 280                 perror("Failed to enable HCI time stamps");
 281                 close(fd);
 282                 return -1;
 283         }
 284
 285         memset(&addr, 0, sizeof(addr));
 286         addr.hci_family = AF_BLUETOOTH;
 287         addr.hci_dev = HCI_DEV_NONE;
 288         addr.hci_channel = HCI_CHANNEL_RAW;
 289
 290         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 291                 perror("Failed to bind channel");
 292                 close(fd);
 293                 return -1;
 294         }
 295
 296         device_list(fd, HCI_MAX_DEV);
 297
 298         return fd;
 299 }
 300
 301 static void stack_internal_callback(int fd, uint32_t events, void *user_data)
 302 {
 303         unsigned char buf[HCI_MAX_FRAME_SIZE];
 304         unsigned char control[32];
 305         struct msghdr msg;
 306         struct iovec iov;
 307         struct cmsghdr *cmsg;
 308         ssize_t len;
 309         hci_event_hdr *eh;
 310         evt_stack_internal *si;
 311         evt_si_device *sd;
 312         struct timeval *tv = NULL;
 313         struct timeval ctv;
 314         uint8_t type = 0xff, bus = 0xff;
 315         char str[18], name[8] = "";
 316         bdaddr_t bdaddr;
 317
 318         bacpy(&bdaddr, BDADDR_ANY);
 319
 320         if (events & (EPOLLERR | EPOLLHUP)) {
 321                 mainloop_remove_fd(fd);
 322                 return;
 323         }
 324
 325         iov.iov_base = buf;
 326         iov.iov_len = sizeof(buf);
 327
 328         memset(&msg, 0, sizeof(msg));
 329         msg.msg_iov = &iov;
 330         msg.msg_iovlen = 1;
 331         msg.msg_control = control;
 332         msg.msg_controllen = sizeof(control);
 333
 334         len = recvmsg(fd, &msg, MSG_DONTWAIT);
 335         if (len < 0)
 336                 return;
 337
 338         for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
 339                                         cmsg = CMSG_NXTHDR(&msg, cmsg)) {
 340                 if (cmsg->cmsg_level != SOL_HCI)
 341                         continue;
 342
 343                 switch (cmsg->cmsg_type) {
 344                 case HCI_CMSG_TSTAMP:
 345                         memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
 346                         tv = &ctv;
 347                         break;
 348                 }
 349         }
 350
 351         if (len < 1 + HCI_EVENT_HDR_SIZE + EVT_STACK_INTERNAL_SIZE +
 352                                                         EVT_SI_DEVICE_SIZE)
 353                 return;
 354
 355         if (buf[0] != HCI_EVENT_PKT)
 356                 return;
 357
 358         eh = (hci_event_hdr *) (buf + 1);
 359         if (eh->evt != EVT_STACK_INTERNAL)
 360                 return;
 361
 362         si = (evt_stack_internal *) (buf + 1 + HCI_EVENT_HDR_SIZE);
 363         if (si->type != EVT_SI_DEVICE)
 364                 return;
 365
 366         sd = (evt_si_device *) &si->data;
 367
 368         switch (sd->event) {
 369         case HCI_DEV_REG:
 370                 device_info(fd, sd->dev_id, &type, &bus, &bdaddr, name);
 371                 ba2str(&bdaddr, str);
 372                 packet_new_index(tv, sd->dev_id, str, type, bus, name);
 373                 open_device(sd->dev_id);
 374                 break;
 375         case HCI_DEV_UNREG:
 376                 ba2str(&bdaddr, str);
 377                 packet_del_index(tv, sd->dev_id, str);
 378                 break;
 379         }
 380 }
 381
 382 int hcidump_tracing(void)
 383 {
 384         struct hcidump_data *data;
 385
 386         data = malloc(sizeof(*data));
 387         if (!data)
 388                 return -1;
 389
 390         memset(data, 0, sizeof(*data));
 391         data->index = HCI_DEV_NONE;
 392
 393         data->fd = open_stack_internal();
 394         if (data->fd < 0) {
 395                 free(data);
 396                 return -1;
 397         }
 398
 399         if (mainloop_add_fd(data->fd, EPOLLIN, stack_internal_callback,
 400                                                         data, free_data) < 0) {
 401                 close(data->fd);
 402                 free(data);
 403                 return -1;
 404         }
 405
 406         return 0;
 407 }