monitor/btsnoop.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 <errno.h>
  31 #include <fcntl.h>
  32 #include <unistd.h>
  33 #include <stdint.h>
  34 #include <string.h>
  35 #include <sys/stat.h>
  36 #include <arpa/inet.h>
  37
  38 #include "lib/bluetooth.h"
  39 #include "lib/hci.h"
  40
  41 #include "btsnoop.h"
  42
  43 struct btsnoop_hdr {
  44         uint8_t         id[8];          /* Identification Pattern */
  45         uint32_t        version;        /* Version Number = 1 */
  46         uint32_t        type;           /* Datalink Type */
  47 } __attribute__ ((packed));
  48 #define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr))
  49
  50 struct btsnoop_pkt {
  51         uint32_t        size;           /* Original Length */
  52         uint32_t        len;            /* Included Length */
  53         uint32_t        flags;          /* Packet Flags */
  54         uint32_t        drops;          /* Cumulative Drops */
  55         uint64_t        ts;             /* Timestamp microseconds */
  56         uint8_t         data[0];        /* Packet Data */
  57 } __attribute__ ((packed));
  58 #define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt))
  59
  60 static const uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e,
  61                                       0x6f, 0x6f, 0x70, 0x00 };
  62
  63 static const uint32_t btsnoop_version = 1;
  64 static uint32_t btsnoop_type = 0;
  65
  66 static int btsnoop_fd = -1;
  67 static uint16_t btsnoop_index = 0xffff;
  68
  69 #ifdef __TIZEN_PATCH__
  70 static char *btsnoop_path = NULL;
  71 static int16_t btsnoop_rotate = -1;
  72 static ssize_t btsnoop_size = -1;
  73
  74 void btsnoop_create(const char *path, uint32_t type,
  75                 int16_t rotate_count, ssize_t file_size)
  76 #else
  77 void btsnoop_create(const char *path, uint32_t type)
  78 #endif
  79 {
  80         struct btsnoop_hdr hdr;
  81         ssize_t written;
  82
  83         if (btsnoop_fd >= 0)
  84                 return;
  85
  86         btsnoop_fd = open(path, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
  87                                 S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
  88         if (btsnoop_fd < 0)
  89                 return;
  90
  91         btsnoop_type = type;
  92
  93         memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
  94         hdr.version = htobe32(btsnoop_version);
  95         hdr.type = htobe32(btsnoop_type);
  96
  97         written = write(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
  98         if (written < 0) {
  99                 close(btsnoop_fd);
 100                 btsnoop_fd = -1;
 101                 return;
 102         }
 103
 104 #ifdef __TIZEN_PATCH__
 105         if (rotate_count > 0 && file_size > 0) {
 106                 btsnoop_path = strdup(path);
 107                 btsnoop_rotate = rotate_count;
 108                 btsnoop_size = file_size;
 109         }
 110 #endif
 111 }
 112
 113 #ifdef __TIZEN_PATCH__
 114 static void btsnoop_create_2(void)
 115 {
 116         struct btsnoop_hdr hdr;
 117         ssize_t written;
 118
 119         if (btsnoop_fd >= 0)
 120                 close(btsnoop_fd);
 121
 122         btsnoop_fd = open(btsnoop_path,
 123                         O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
 124                         S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 125         if (btsnoop_fd < 0)
 126                 goto fail;
 127
 128         memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
 129         hdr.version = htobe32(btsnoop_version);
 130         hdr.type = htobe32(btsnoop_type);
 131
 132         written = write(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
 133         if (written < 0) {
 134                 close(btsnoop_fd);
 135                 btsnoop_fd = -1;
 136                 goto fail;
 137         }
 138
 139         return;
 140
 141 fail:
 142         free(btsnoop_path);
 143         btsnoop_rotate = -1;
 144         btsnoop_size = -1;
 145         btsnoop_index = 0xffff;
 146
 147         return;
 148 }
 149
 150 static void btsnoop_rotate_files(void)
 151 {
 152         char *filename = NULL;
 153         char *new_filename = NULL;
 154         int i;
 155         int postfix_width = 0;
 156         int err;
 157
 158         if (btsnoop_rotate <= 1)
 159                 return;
 160
 161         for (i = btsnoop_rotate / 10; i; i /= 10)
 162                 postfix_width++;
 163
 164         for (i = btsnoop_rotate - 2; i >= 0; i--) {
 165                 if (i == 0) {
 166                         filename = strdup(btsnoop_path);
 167                         err = (filename == NULL) ? -1 : 0;
 168                 } else {
 169                         err = asprintf(&filename, "%s.%0*d",
 170                                         btsnoop_path, postfix_width, i);
 171                 }
 172
 173                 if (err < 0 || access(filename, F_OK) < 0)
 174                         goto done;
 175
 176                 err = asprintf(&new_filename, "%s.%0*d",
 177                                 btsnoop_path, postfix_width, i + 1);
 178                 if (err < 0)
 179                         goto done;
 180
 181                 err = rename(filename, new_filename)
 182
 183 done:
 184                 if (new_filename) {
 185                         free(new_filename);
 186                         new_filename = NULL;
 187                 }
 188
 189                 if (filename) {
 190                         free(filename);
 191                         filename = NULL;
 192                 }
 193
 194                 if (err < 0)
 195                         break;
 196         }
 197 }
 198 #endif
 199
 200 void btsnoop_write(struct timeval *tv, uint32_t flags,
 201                                         const void *data, uint16_t size)
 202 {
 203         struct btsnoop_pkt pkt;
 204         uint64_t ts;
 205         ssize_t written;
 206
 207         ts = (tv->tv_sec - 946684800ll) * 1000000ll + tv->tv_usec;
 208
 209         pkt.size  = htobe32(size);
 210         pkt.len   = htobe32(size);
 211         pkt.flags = htobe32(flags);
 212         pkt.drops = htobe32(0);
 213         pkt.ts    = htobe64(ts + 0x00E03AB44A676000ll);
 214
 215 #ifdef __TIZEN_PATCH__
 216         if ((btsnoop_rotate > 0 && btsnoop_size > 0) &&
 217                         lseek(btsnoop_fd, 0x00, SEEK_CUR) +
 218                         BTSNOOP_PKT_SIZE + size > btsnoop_size) {
 219                 btsnoop_rotate_files();
 220                 btsnoop_create_2();
 221                 if (btsnoop_fd < 0)
 222                         return;
 223         }
 224 #endif
 225
 226         written = write(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
 227         if (written < 0)
 228                 return;
 229
 230         if (data && size > 0) {
 231                 written = write(btsnoop_fd, data, size);
 232                 if (written < 0)
 233                         return;
 234         }
 235 }
 236
 237 static uint32_t get_flags_from_opcode(uint16_t opcode)
 238 {
 239         switch (opcode) {
 240         case BTSNOOP_OPCODE_NEW_INDEX:
 241         case BTSNOOP_OPCODE_DEL_INDEX:
 242                 break;
 243         case BTSNOOP_OPCODE_COMMAND_PKT:
 244                 return 0x02;
 245         case BTSNOOP_OPCODE_EVENT_PKT:
 246                 return 0x03;
 247         case BTSNOOP_OPCODE_ACL_TX_PKT:
 248                 return 0x00;
 249         case BTSNOOP_OPCODE_ACL_RX_PKT:
 250                 return 0x01;
 251         case BTSNOOP_OPCODE_SCO_TX_PKT:
 252         case BTSNOOP_OPCODE_SCO_RX_PKT:
 253                 break;
 254         }
 255
 256         return 0xff;
 257 }
 258
 259 void btsnoop_write_hci(struct timeval *tv, uint16_t index, uint16_t opcode,
 260                                         const void *data, uint16_t size)
 261 {
 262         uint32_t flags;
 263
 264         if (!tv)
 265                 return;
 266
 267         if (btsnoop_fd < 0)
 268                 return;
 269
 270         switch (btsnoop_type) {
 271         case BTSNOOP_TYPE_HCI:
 272                 if (btsnoop_index == 0xffff)
 273                         btsnoop_index = index;
 274
 275                 if (index != btsnoop_index)
 276                         return;
 277
 278                 flags = get_flags_from_opcode(opcode);
 279                 if (flags == 0xff)
 280                         return;
 281                 break;
 282
 283         case BTSNOOP_TYPE_MONITOR:
 284                 flags = (index << 16) | opcode;
 285                 break;
 286
 287         default:
 288                 return;
 289         }
 290
 291         btsnoop_write(tv, flags, data, size);
 292 }
 293
 294 void btsnoop_write_phy(struct timeval *tv, uint16_t frequency,
 295                                         const void *data, uint16_t size)
 296 {
 297         uint32_t flags;
 298
 299         if (!tv)
 300                 return;
 301
 302         if (btsnoop_fd < 0)
 303                 return;
 304
 305         switch (btsnoop_type) {
 306         case BTSNOOP_TYPE_SIMULATOR:
 307                 flags = (1 << 16) | frequency;
 308                 break;
 309
 310         default:
 311                 return;
 312         }
 313
 314         btsnoop_write(tv, flags, data, size);
 315 }
 316
 317 int btsnoop_open(const char *path, uint32_t *type)
 318 {
 319         struct btsnoop_hdr hdr;
 320         ssize_t len;
 321
 322         if (btsnoop_fd >= 0) {
 323                 fprintf(stderr, "Too many open files\n");
 324                 return -1;
 325         }
 326
 327         btsnoop_fd = open(path, O_RDONLY | O_CLOEXEC);
 328         if (btsnoop_fd < 0) {
 329                 perror("Failed to open file");
 330                 return -1;
 331         }
 332
 333         len = read(btsnoop_fd, &hdr, BTSNOOP_HDR_SIZE);
 334         if (len < 0 || len != BTSNOOP_HDR_SIZE) {
 335                 perror("Failed to read header");
 336                 close(btsnoop_fd);
 337                 btsnoop_fd = -1;
 338                 return -1;
 339         }
 340
 341         if (memcmp(hdr.id, btsnoop_id, sizeof(btsnoop_id))) {
 342                 fprintf(stderr, "Invalid btsnoop header\n");
 343                 close(btsnoop_fd);
 344                 btsnoop_fd = -1;
 345                 return -1;
 346         }
 347
 348         if (be32toh(hdr.version) != btsnoop_version) {
 349                 fprintf(stderr, "Invalid btsnoop version\n");
 350                 close(btsnoop_fd);
 351                 btsnoop_fd = -1;
 352                 return -1;
 353         }
 354
 355         btsnoop_type = be32toh(hdr.type);
 356
 357         if (type)
 358                 *type = btsnoop_type;
 359
 360         return 0;
 361 }
 362
 363 static uint16_t get_opcode_from_flags(uint8_t type, uint32_t flags)
 364 {
 365         switch (type) {
 366         case HCI_COMMAND_PKT:
 367                 return BTSNOOP_OPCODE_COMMAND_PKT;
 368         case HCI_ACLDATA_PKT:
 369                 if (flags & 0x01)
 370                         return BTSNOOP_OPCODE_ACL_RX_PKT;
 371                 else
 372                         return BTSNOOP_OPCODE_ACL_TX_PKT;
 373         case HCI_SCODATA_PKT:
 374                 if (flags & 0x01)
 375                         return BTSNOOP_OPCODE_SCO_RX_PKT;
 376                 else
 377                         return BTSNOOP_OPCODE_SCO_TX_PKT;
 378         case HCI_EVENT_PKT:
 379                 return BTSNOOP_OPCODE_EVENT_PKT;
 380         case 0xff:
 381                 if (flags & 0x02) {
 382                         if (flags & 0x01)
 383                                 return BTSNOOP_OPCODE_EVENT_PKT;
 384                         else
 385                                 return BTSNOOP_OPCODE_COMMAND_PKT;
 386                 } else {
 387                         if (flags & 0x01)
 388                                 return BTSNOOP_OPCODE_ACL_RX_PKT;
 389                         else
 390                                 return BTSNOOP_OPCODE_ACL_TX_PKT;
 391                 }
 392                 break;
 393         }
 394
 395         return 0xff;
 396 }
 397
 398 int btsnoop_read_hci(struct timeval *tv, uint16_t *index, uint16_t *opcode,
 399                                                 void *data, uint16_t *size)
 400 {
 401         struct btsnoop_pkt pkt;
 402         uint32_t toread, flags;
 403         uint64_t ts;
 404         uint8_t pkt_type;
 405         ssize_t len;
 406
 407         if (btsnoop_fd < 0)
 408                 return -1;
 409
 410         len = read(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
 411         if (len == 0)
 412                 return -1;
 413
 414         if (len < 0 || len != BTSNOOP_PKT_SIZE) {
 415                 perror("Failed to read packet");
 416                 close(btsnoop_fd);
 417                 btsnoop_fd = -1;
 418                 return -1;
 419         }
 420
 421         toread = be32toh(pkt.size);
 422         if (toread > BTSNOOP_MAX_PACKET_SIZE) {
 423                 perror("Packet len suspicially big: %u", toread);
 424                 close(btsnoop_fd);
 425                 btsnoop_fd = -1;
 426                 return -1;
 427         }
 428
 429         flags = be32toh(pkt.flags);
 430
 431         ts = be64toh(pkt.ts) - 0x00E03AB44A676000ll;
 432         tv->tv_sec = (ts / 1000000ll) + 946684800ll;
 433         tv->tv_usec = ts % 1000000ll;
 434
 435         switch (btsnoop_type) {
 436         case BTSNOOP_TYPE_HCI:
 437                 *index = 0;
 438                 *opcode = get_opcode_from_flags(0xff, flags);
 439                 break;
 440
 441         case BTSNOOP_TYPE_UART:
 442                 len = read(btsnoop_fd, &pkt_type, 1);
 443                 if (len < 0) {
 444                         perror("Failed to read packet type");
 445                         close(btsnoop_fd);
 446                         btsnoop_fd = -1;
 447                         return -1;
 448                 }
 449                 toread--;
 450
 451                 *index = 0;
 452                 *opcode = get_opcode_from_flags(pkt_type, flags);
 453                 break;
 454
 455         case BTSNOOP_TYPE_MONITOR:
 456                 *index = flags >> 16;
 457                 *opcode = flags & 0xffff;
 458                 break;
 459
 460         default:
 461                 fprintf(stderr, "Unknown packet type\n");
 462                 close(btsnoop_fd);
 463                 btsnoop_fd = -1;
 464                 return -1;
 465         }
 466
 467         len = read(btsnoop_fd, data, toread);
 468         if (len < 0) {
 469                 perror("Failed to read data");
 470                 close(btsnoop_fd);
 471                 btsnoop_fd = -1;
 472                 return -1;
 473         }
 474
 475         *size = toread;
 476
 477         return 0;
 478 }
 479
 480 int btsnoop_read_phy(struct timeval *tv, uint16_t *frequency,
 481                                                 void *data, uint16_t *size)
 482 {
 483         struct btsnoop_pkt pkt;
 484         uint32_t toread, flags;
 485         uint64_t ts;
 486         ssize_t len;
 487
 488         if (btsnoop_fd < 0)
 489                 return -1;
 490
 491         len = read(btsnoop_fd, &pkt, BTSNOOP_PKT_SIZE);
 492         if (len == 0)
 493                 return -1;
 494
 495         if (len < 0 || len != BTSNOOP_PKT_SIZE) {
 496                 perror("Failed to read packet");
 497                 close(btsnoop_fd);
 498                 btsnoop_fd = -1;
 499                 return -1;
 500         }
 501
 502         toread = be32toh(pkt.size);
 503         flags = be32toh(pkt.flags);
 504
 505         ts = be64toh(pkt.ts) - 0x00E03AB44A676000ll;
 506         tv->tv_sec = (ts / 1000000ll) + 946684800ll;
 507         tv->tv_usec = ts % 1000000ll;
 508
 509         switch (btsnoop_type) {
 510         case BTSNOOP_TYPE_SIMULATOR:
 511                 if ((flags >> 16) != 1)
 512                         break;
 513                 *frequency = flags & 0xffff;
 514                 break;
 515
 516         default:
 517                 fprintf(stderr, "Unknown packet type\n");
 518                 close(btsnoop_fd);
 519                 btsnoop_fd = -1;
 520                 return -1;
 521         }
 522
 523         len = read(btsnoop_fd, data, toread);
 524         if (len < 0) {
 525                 perror("Failed to read data");
 526                 close(btsnoop_fd);
 527                 btsnoop_fd = -1;
 528                 return -1;
 529         }
 530
 531         *size = toread;
 532
 533         return 0;
 534 }
 535
 536 void btsnoop_close(void)
 537 {
 538         if (btsnoop_fd < 0)
 539                 return;
 540
 541 #ifdef __TIZEN_PATCH__
 542         if (btsnoop_path) {
 543                 free(btsnoop_path);
 544                 btsnoop_path = NULL;
 545         }
 546         btsnoop_rotate = -1;
 547         btsnoop_size = -1;
 548 #endif
 549
 550         close(btsnoop_fd);
 551         btsnoop_fd = -1;
 552
 553         btsnoop_index = 0xffff;
 554 }