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