src/shared/btsnoop.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2012-2014  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #ifdef __TIZEN_PATCH__
  29 #include <stdio.h>
  30 #endif
  31 #include <endian.h>
  32 #include <fcntl.h>
  33 #include <unistd.h>
  34 #include <stdlib.h>
  35 #include <string.h>
  36 #include <arpa/inet.h>
  37 #include <sys/stat.h>
  38
  39 #include "src/shared/btsnoop.h"
  40
  41 struct btsnoop_hdr {
  42         uint8_t         id[8];          /* Identification Pattern */
  43         uint32_t        version;        /* Version Number = 1 */
  44         uint32_t        type;           /* Datalink Type */
  45 } __attribute__ ((packed));
  46 #define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr))
  47
  48 struct btsnoop_pkt {
  49         uint32_t        size;           /* Original Length */
  50         uint32_t        len;            /* Included Length */
  51         uint32_t        flags;          /* Packet Flags */
  52         uint32_t        drops;          /* Cumulative Drops */
  53         uint64_t        ts;             /* Timestamp microseconds */
  54         uint8_t         data[0];        /* Packet Data */
  55 } __attribute__ ((packed));
  56 #define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt))
  57
  58 static const uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e,
  59                                       0x6f, 0x6f, 0x70, 0x00 };
  60
  61 static const uint32_t btsnoop_version = 1;
  62
  63 struct pklg_pkt {
  64         uint32_t        len;
  65         uint64_t        ts;
  66         uint8_t         type;
  67 } __attribute__ ((packed));
  68 #define PKLG_PKT_SIZE (sizeof(struct pklg_pkt))
  69
  70 struct btsnoop {
  71         int ref_count;
  72         int fd;
  73         unsigned long flags;
  74         uint32_t format;
  75         uint16_t index;
  76         bool aborted;
  77         bool pklg_format;
  78         bool pklg_v2;
  79 #ifdef __TIZEN_PATCH__
  80         char *path;
  81         int16_t rotate_count;
  82         ssize_t file_size;
  83 #endif
  84 };
  85
  86 struct btsnoop *btsnoop_open(const char *path, unsigned long flags)
  87 {
  88         struct btsnoop *btsnoop;
  89         struct btsnoop_hdr hdr;
  90         ssize_t len;
  91
  92         btsnoop = calloc(1, sizeof(*btsnoop));
  93         if (!btsnoop)
  94                 return NULL;
  95
  96         btsnoop->fd = open(path, O_RDONLY | O_CLOEXEC);
  97         if (btsnoop->fd < 0) {
  98                 free(btsnoop);
  99                 return NULL;
 100         }
 101
 102         btsnoop->flags = flags;
 103
 104         len = read(btsnoop->fd, &hdr, BTSNOOP_HDR_SIZE);
 105         if (len < 0 || len != BTSNOOP_HDR_SIZE)
 106                 goto failed;
 107
 108         if (!memcmp(hdr.id, btsnoop_id, sizeof(btsnoop_id))) {
 109                 /* Check for BTSnoop version 1 format */
 110                 if (be32toh(hdr.version) != btsnoop_version)
 111                         goto failed;
 112
 113                 btsnoop->format = be32toh(hdr.type);
 114                 btsnoop->index = 0xffff;
 115         } else {
 116                 if (!(btsnoop->flags & BTSNOOP_FLAG_PKLG_SUPPORT))
 117                         goto failed;
 118
 119                 /* Check for Apple Packet Logger format */
 120                 if (hdr.id[0] != 0x00 ||
 121                                 (hdr.id[1] != 0x00 && hdr.id[1] != 0x01))
 122                         goto failed;
 123
 124                 btsnoop->format = BTSNOOP_FORMAT_MONITOR;
 125                 btsnoop->index = 0xffff;
 126                 btsnoop->pklg_format = true;
 127                 btsnoop->pklg_v2 = (hdr.id[1] == 0x01);
 128
 129                 /* Apple Packet Logger format has no header */
 130                 lseek(btsnoop->fd, 0, SEEK_SET);
 131         }
 132
 133         return btsnoop_ref(btsnoop);
 134
 135 failed:
 136         close(btsnoop->fd);
 137         free(btsnoop);
 138
 139         return NULL;
 140 }
 141
 142 #ifdef __TIZEN_PATCH__
 143 struct btsnoop *btsnoop_create(const char *path, uint32_t format,
 144                 int16_t rotate_count, ssize_t file_size)
 145 #else
 146 struct btsnoop *btsnoop_create(const char *path, uint32_t format)
 147 #endif
 148 {
 149         struct btsnoop *btsnoop;
 150         struct btsnoop_hdr hdr;
 151         ssize_t written;
 152
 153         btsnoop = calloc(1, sizeof(*btsnoop));
 154         if (!btsnoop)
 155                 return NULL;
 156
 157         btsnoop->fd = open(path, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
 158                                         S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 159         if (btsnoop->fd < 0) {
 160                 free(btsnoop);
 161                 return NULL;
 162         }
 163
 164         btsnoop->format = format;
 165         btsnoop->index = 0xffff;
 166
 167         memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
 168         hdr.version = htobe32(btsnoop_version);
 169         hdr.type = htobe32(btsnoop->format);
 170
 171         written = write(btsnoop->fd, &hdr, BTSNOOP_HDR_SIZE);
 172         if (written < 0) {
 173                 close(btsnoop->fd);
 174                 free(btsnoop);
 175                 return NULL;
 176         }
 177
 178 #ifdef __TIZEN_PATCH__
 179         if (rotate_count > 0 && file_size > 0) {
 180                 btsnoop->path = strdup(path);
 181                 btsnoop->rotate_count = rotate_count;
 182                 btsnoop->file_size = file_size;
 183         }
 184 #endif
 185
 186         return btsnoop_ref(btsnoop);
 187 }
 188
 189 #ifdef __TIZEN_PATCH__
 190 static int btsnoop_create_2(struct btsnoop *btsnoop)
 191 {
 192         struct btsnoop_hdr hdr;
 193         ssize_t written;
 194
 195         if (btsnoop->fd >= 0)
 196                 close(btsnoop->fd);
 197
 198         btsnoop->fd = open(btsnoop->path,
 199                         O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC,
 200                         S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH);
 201         if (btsnoop->fd < 0) {
 202                 btsnoop_unref(btsnoop);
 203                 return -1;
 204         }
 205
 206         memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id));
 207         hdr.version = htobe32(btsnoop_version);
 208         hdr.type = htobe32(btsnoop->format);
 209
 210         written = write(btsnoop->fd, &hdr, BTSNOOP_HDR_SIZE);
 211         if (written < 0) {
 212                 btsnoop_unref(btsnoop);
 213                 return -1;
 214         }
 215
 216         return btsnoop->fd;
 217 }
 218
 219 static void btsnoop_rotate_files(struct btsnoop *btsnoop)
 220 {
 221         char *filename = NULL;
 222         char *new_filename = NULL;
 223         int i;
 224         int postfix_width = 0;
 225         int err;
 226
 227         if (btsnoop->rotate_count <= 1)
 228                 return;
 229
 230         for (i = btsnoop->rotate_count / 10; i; i /= 10)
 231                 postfix_width++;
 232
 233         for (i = btsnoop->rotate_count - 2; i >= 0; i--) {
 234                 if (i == 0) {
 235                         filename = strdup(btsnoop->path);
 236                         err = (filename == NULL) ? -1 : 0;
 237                 } else {
 238                         err = asprintf(&filename, "%s.%0*d",
 239                                         btsnoop->path, postfix_width, i);
 240                 }
 241
 242                 if (err < 0 || access(filename, F_OK) < 0)
 243                         goto done;
 244
 245                 err = asprintf(&new_filename, "%s.%0*d",
 246                                 btsnoop->path, postfix_width, i + 1);
 247                 if (err < 0)
 248                         goto done;
 249
 250                 err = rename(filename, new_filename);
 251
 252 done:
 253                 if (new_filename) {
 254                         free(new_filename);
 255                         new_filename = NULL;
 256                 }
 257
 258                 if (filename) {
 259                         free(filename);
 260                         filename = NULL;
 261                 }
 262
 263                 if (err < 0)
 264                         break;
 265         }
 266
 267         return;
 268 }
 269 #endif
 270
 271 struct btsnoop *btsnoop_ref(struct btsnoop *btsnoop)
 272 {
 273         if (!btsnoop)
 274                 return NULL;
 275
 276         __sync_fetch_and_add(&btsnoop->ref_count, 1);
 277
 278         return btsnoop;
 279 }
 280
 281 void btsnoop_unref(struct btsnoop *btsnoop)
 282 {
 283         if (!btsnoop)
 284                 return;
 285
 286         if (__sync_sub_and_fetch(&btsnoop->ref_count, 1))
 287                 return;
 288
 289 #ifdef __TIZEN_PATCH__
 290         if (btsnoop->path)
 291                 free(btsnoop->path);
 292 #endif
 293
 294         if (btsnoop->fd >= 0)
 295                 close(btsnoop->fd);
 296
 297         free(btsnoop);
 298 }
 299
 300 uint32_t btsnoop_get_format(struct btsnoop *btsnoop)
 301 {
 302         if (!btsnoop)
 303                 return BTSNOOP_FORMAT_INVALID;
 304
 305         return btsnoop->format;
 306 }
 307
 308 bool btsnoop_write(struct btsnoop *btsnoop, struct timeval *tv,
 309                         uint32_t flags, const void *data, uint16_t size)
 310 {
 311         struct btsnoop_pkt pkt;
 312         uint64_t ts;
 313         ssize_t written;
 314
 315         if (!btsnoop || !tv)
 316                 return false;
 317
 318         ts = (tv->tv_sec - 946684800ll) * 1000000ll + tv->tv_usec;
 319
 320         pkt.size  = htobe32(size);
 321         pkt.len   = htobe32(size);
 322         pkt.flags = htobe32(flags);
 323         pkt.drops = htobe32(0);
 324         pkt.ts    = htobe64(ts + 0x00E03AB44A676000ll);
 325
 326 #ifdef __TIZEN_PATCH__
 327         if ((btsnoop->rotate_count > 0 && btsnoop->file_size > 0) &&
 328                         lseek(btsnoop->fd, 0x00, SEEK_CUR) +
 329                         BTSNOOP_PKT_SIZE + size > btsnoop->file_size) {
 330                 btsnoop_rotate_files(btsnoop);
 331                 if (btsnoop_create_2(btsnoop) < 0)
 332                         return false;
 333         }
 334 #endif
 335
 336         written = write(btsnoop->fd, &pkt, BTSNOOP_PKT_SIZE);
 337         if (written < 0)
 338                 return false;
 339
 340         if (data && size > 0) {
 341                 written = write(btsnoop->fd, data, size);
 342                 if (written < 0)
 343                         return false;
 344         }
 345
 346         return true;
 347 }
 348
 349 static uint32_t get_flags_from_opcode(uint16_t opcode)
 350 {
 351         switch (opcode) {
 352         case BTSNOOP_OPCODE_NEW_INDEX:
 353         case BTSNOOP_OPCODE_DEL_INDEX:
 354                 break;
 355         case BTSNOOP_OPCODE_COMMAND_PKT:
 356                 return 0x02;
 357         case BTSNOOP_OPCODE_EVENT_PKT:
 358                 return 0x03;
 359         case BTSNOOP_OPCODE_ACL_TX_PKT:
 360                 return 0x00;
 361         case BTSNOOP_OPCODE_ACL_RX_PKT:
 362                 return 0x01;
 363         case BTSNOOP_OPCODE_SCO_TX_PKT:
 364         case BTSNOOP_OPCODE_SCO_RX_PKT:
 365                 break;
 366         case BTSNOOP_OPCODE_OPEN_INDEX:
 367         case BTSNOOP_OPCODE_CLOSE_INDEX:
 368                 break;
 369         }
 370
 371         return 0xff;
 372 }
 373
 374 bool btsnoop_write_hci(struct btsnoop *btsnoop, struct timeval *tv,
 375                                         uint16_t index, uint16_t opcode,
 376                                         const void *data, uint16_t size)
 377 {
 378         uint32_t flags;
 379
 380         if (!btsnoop)
 381                 return false;
 382
 383         switch (btsnoop->format) {
 384         case BTSNOOP_FORMAT_HCI:
 385                 if (btsnoop->index == 0xffff)
 386                         btsnoop->index = index;
 387
 388                 if (index != btsnoop->index)
 389                         return false;
 390
 391                 flags = get_flags_from_opcode(opcode);
 392                 if (flags == 0xff)
 393                         return false;
 394                 break;
 395
 396         case BTSNOOP_FORMAT_MONITOR:
 397                 flags = (index << 16) | opcode;
 398                 break;
 399
 400         default:
 401                 return false;
 402         }
 403
 404         return btsnoop_write(btsnoop, tv, flags, data, size);
 405 }
 406
 407 bool btsnoop_write_phy(struct btsnoop *btsnoop, struct timeval *tv,
 408                         uint16_t frequency, const void *data, uint16_t size)
 409 {
 410         uint32_t flags;
 411
 412         if (!btsnoop)
 413                 return false;
 414
 415         switch (btsnoop->format) {
 416         case BTSNOOP_FORMAT_SIMULATOR:
 417                 flags = (1 << 16) | frequency;
 418                 break;
 419
 420         default:
 421                 return false;
 422         }
 423
 424         return btsnoop_write(btsnoop, tv, flags, data, size);
 425 }
 426
 427 static bool pklg_read_hci(struct btsnoop *btsnoop, struct timeval *tv,
 428                                         uint16_t *index, uint16_t *opcode,
 429                                         void *data, uint16_t *size)
 430 {
 431         struct pklg_pkt pkt;
 432         uint32_t toread;
 433         uint64_t ts;
 434         ssize_t len;
 435
 436         len = read(btsnoop->fd, &pkt, PKLG_PKT_SIZE);
 437         if (len == 0)
 438                 return false;
 439
 440         if (len < 0 || len != PKLG_PKT_SIZE) {
 441                 btsnoop->aborted = true;
 442                 return false;
 443         }
 444
 445         if (btsnoop->pklg_v2) {
 446                 toread = le32toh(pkt.len) - (PKLG_PKT_SIZE - 4);
 447
 448                 ts = le64toh(pkt.ts);
 449                 tv->tv_sec = ts & 0xffffffff;
 450                 tv->tv_usec = ts >> 32;
 451         } else {
 452                 toread = be32toh(pkt.len) - (PKLG_PKT_SIZE - 4);
 453
 454                 ts = be64toh(pkt.ts);
 455                 tv->tv_sec = ts >> 32;
 456                 tv->tv_usec = ts & 0xffffffff;
 457         }
 458
 459         switch (pkt.type) {
 460         case 0x00:
 461                 *index = 0x0000;
 462                 *opcode = BTSNOOP_OPCODE_COMMAND_PKT;
 463                 break;
 464         case 0x01:
 465                 *index = 0x0000;
 466                 *opcode = BTSNOOP_OPCODE_EVENT_PKT;
 467                 break;
 468         case 0x02:
 469                 *index = 0x0000;
 470                 *opcode = BTSNOOP_OPCODE_ACL_TX_PKT;
 471                 break;
 472         case 0x03:
 473                 *index = 0x0000;
 474                 *opcode = BTSNOOP_OPCODE_ACL_RX_PKT;
 475                 break;
 476         case 0x0b:
 477                 *index = 0x0000;
 478                 *opcode = BTSNOOP_OPCODE_VENDOR_DIAG;
 479                 break;
 480         case 0xfc:
 481                 *index = 0xffff;
 482                 *opcode = BTSNOOP_OPCODE_SYSTEM_NOTE;
 483                 break;
 484         default:
 485                 *index = 0xffff;
 486                 *opcode = 0xffff;
 487                 break;
 488         }
 489
 490         len = read(btsnoop->fd, data, toread);
 491         if (len < 0) {
 492                 btsnoop->aborted = true;
 493                 return false;
 494         }
 495
 496         *size = toread;
 497
 498         return true;
 499 }
 500
 501 static uint16_t get_opcode_from_flags(uint8_t type, uint32_t flags)
 502 {
 503         switch (type) {
 504         case 0x01:
 505                 return BTSNOOP_OPCODE_COMMAND_PKT;
 506         case 0x02:
 507                 if (flags & 0x01)
 508                         return BTSNOOP_OPCODE_ACL_RX_PKT;
 509                 else
 510                         return BTSNOOP_OPCODE_ACL_TX_PKT;
 511         case 0x03:
 512                 if (flags & 0x01)
 513                         return BTSNOOP_OPCODE_SCO_RX_PKT;
 514                 else
 515                         return BTSNOOP_OPCODE_SCO_TX_PKT;
 516         case 0x04:
 517                 return BTSNOOP_OPCODE_EVENT_PKT;
 518         case 0xff:
 519                 if (flags & 0x02) {
 520                         if (flags & 0x01)
 521                                 return BTSNOOP_OPCODE_EVENT_PKT;
 522                         else
 523                                 return BTSNOOP_OPCODE_COMMAND_PKT;
 524                 } else {
 525                         if (flags & 0x01)
 526                                 return BTSNOOP_OPCODE_ACL_RX_PKT;
 527                         else
 528                                 return BTSNOOP_OPCODE_ACL_TX_PKT;
 529                 }
 530                 break;
 531         }
 532
 533         return 0xffff;
 534 }
 535
 536 bool btsnoop_read_hci(struct btsnoop *btsnoop, struct timeval *tv,
 537                                         uint16_t *index, uint16_t *opcode,
 538                                         void *data, uint16_t *size)
 539 {
 540         struct btsnoop_pkt pkt;
 541         uint32_t toread, flags;
 542         uint64_t ts;
 543         uint8_t pkt_type;
 544         ssize_t len;
 545
 546         if (!btsnoop || btsnoop->aborted)
 547                 return false;
 548
 549         if (btsnoop->pklg_format)
 550                 return pklg_read_hci(btsnoop, tv, index, opcode, data, size);
 551
 552         len = read(btsnoop->fd, &pkt, BTSNOOP_PKT_SIZE);
 553         if (len == 0)
 554                 return false;
 555
 556         if (len < 0 || len != BTSNOOP_PKT_SIZE) {
 557                 btsnoop->aborted = true;
 558                 return false;
 559         }
 560
 561         toread = be32toh(pkt.size);
 562         if (toread > BTSNOOP_MAX_PACKET_SIZE) {
 563                 btsnoop->aborted = true;
 564                 return false;
 565         }
 566
 567         flags = be32toh(pkt.flags);
 568
 569         ts = be64toh(pkt.ts) - 0x00E03AB44A676000ll;
 570         tv->tv_sec = (ts / 1000000ll) + 946684800ll;
 571         tv->tv_usec = ts % 1000000ll;
 572
 573         switch (btsnoop->format) {
 574         case BTSNOOP_FORMAT_HCI:
 575                 *index = 0;
 576                 *opcode = get_opcode_from_flags(0xff, flags);
 577                 break;
 578
 579         case BTSNOOP_FORMAT_UART:
 580                 len = read(btsnoop->fd, &pkt_type, 1);
 581                 if (len < 0) {
 582                         btsnoop->aborted = true;
 583                         return false;
 584                 }
 585                 toread--;
 586
 587                 *index = 0;
 588                 *opcode = get_opcode_from_flags(pkt_type, flags);
 589                 break;
 590
 591         case BTSNOOP_FORMAT_MONITOR:
 592                 *index = flags >> 16;
 593                 *opcode = flags & 0xffff;
 594                 break;
 595
 596         default:
 597                 btsnoop->aborted = true;
 598                 return false;
 599         }
 600
 601         len = read(btsnoop->fd, data, toread);
 602         if (len < 0) {
 603                 btsnoop->aborted = true;
 604                 return false;
 605         }
 606
 607         *size = toread;
 608
 609         return true;
 610 }
 611
 612 bool btsnoop_read_phy(struct btsnoop *btsnoop, struct timeval *tv,
 613                         uint16_t *frequency, void *data, uint16_t *size)
 614 {
 615         return false;
 616 }