1/****************************************************************************** 2 * 3 * Copyright (C) 2009-2012 Broadcom Corporation 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 19/**************************************************************************** 20 * 21 * Name: btsnoopdisp.c 22 * 23 * Function: this file contains functions to generate a BTSNOOP file 24 * 25 * 26 ****************************************************************************/ 27#include <stdio.h> 28#include <dlfcn.h> 29#include <stdlib.h> 30#include <errno.h> 31#include <string.h> 32#include <pthread.h> 33#include <sys/prctl.h> 34#include <unistd.h> 35#include <ctype.h> 36#include <fcntl.h> 37 38#include <arpa/inet.h> 39#include <netinet/in.h> 40#include <netdb.h> 41 42/* for gettimeofday */ 43#include <sys/time.h> 44/* for the S_* open parameters */ 45#include <sys/stat.h> 46/* for write */ 47#include <unistd.h> 48/* for O_* open parameters */ 49#include <fcntl.h> 50/* defines the O_* open parameters */ 51#include <fcntl.h> 52 53#define LOG_TAG "BTSNOOP-DISP" 54#include <cutils/log.h> 55 56#include "bt_hci_bdroid.h" 57#include "utils.h" 58 59#ifndef BTSNOOP_DBG 60#define BTSNOOP_DBG FALSE 61#endif 62 63#if (BTSNOOP_DBG == TRUE) 64#define SNOOPDBG(param, ...) {ALOGD(param, ## __VA_ARGS__);} 65#else 66#define SNOOPDBG(param, ...) {} 67#endif 68 69/* file descriptor of the BT snoop file (by default, -1 means disabled) */ 70int hci_btsnoop_fd = -1; 71 72/* Macro to perform a multiplication of 2 unsigned 32bit values and store the result 73 * in an unsigned 64 bit value (as two 32 bit variables): 74 * u64 = u32In1 * u32In2 75 * u32OutLow = u64[31:0] 76 * u32OutHi = u64[63:32] 77 * basically the algorithm: 78 * (hi1*2^16 + lo1)*(hi2*2^16 + lo2) = lo1*lo2 + (hi1*hi2)*2^32 + (hi1*lo2 + hi2*lo1)*2^16 79 * and the carry is propagated 16 bit by 16 bit: 80 * result[15:0] = lo1*lo2 & 0xFFFF 81 * result[31:16] = ((lo1*lo2) >> 16) + (hi1*lo2 + hi2*lo1) 82 * and so on 83 */ 84#define HCIDISP_MULT_64(u32In1, u32In2, u32OutLo, u32OutHi) \ 85do { \ 86 uint32_t u32In1Tmp = u32In1; \ 87 uint32_t u32In2Tmp = u32In2; \ 88 uint32_t u32Tmp, u32Carry; \ 89 u32OutLo = (u32In1Tmp & 0xFFFF) * (u32In2Tmp & 0xFFFF); /*lo1*lo2*/ \ 90 u32OutHi = ((u32In1Tmp >> 16) & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF); /*hi1*hi2*/ \ 91 u32Tmp = (u32In1Tmp & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF); /*lo1*hi2*/ \ 92 u32Carry = (uint32_t)((u32OutLo>>16)&0xFFFF); \ 93 u32Carry += (u32Tmp&0xFFFF); \ 94 u32OutLo += (u32Tmp << 16) ; \ 95 u32OutHi += (u32Tmp >> 16); \ 96 u32Tmp = ((u32In1Tmp >> 16) & 0xFFFF) * (u32In2Tmp & 0xFFFF); \ 97 u32Carry += (u32Tmp)&0xFFFF; \ 98 u32Carry>>=16; \ 99 u32OutLo += (u32Tmp << 16); \ 100 u32OutHi += (u32Tmp >> 16); \ 101 u32OutHi += u32Carry; \ 102} while (0) 103 104/* Macro to make an addition of 2 64 bit values: 105 * result = (u32OutHi & u32OutLo) + (u32InHi & u32InLo) 106 * u32OutHi = result[63:32] 107 * u32OutLo = result[31:0] 108 */ 109#define HCIDISP_ADD_64(u32InLo, u32InHi, u32OutLo, u32OutHi) \ 110do { \ 111 (u32OutLo) += (u32InLo); \ 112 if ((u32OutLo) < (u32InLo)) (u32OutHi)++; \ 113 (u32OutHi) += (u32InHi); \ 114} while (0) 115 116/* EPOCH in microseconds since 01/01/0000 : 0x00dcddb3.0f2f8000 */ 117#define BTSNOOP_EPOCH_HI 0x00dcddb3U 118#define BTSNOOP_EPOCH_LO 0x0f2f8000U 119 120/******************************************************************************* 121 ** 122 ** Function tv_to_btsnoop_ts 123 ** 124 ** Description This function generate a BT Snoop timestamp. 125 ** 126 ** Returns void 127 ** 128 ** NOTE 129 ** The return value is 64 bit as 2 32 bit variables out_lo and * out_hi. 130 ** A BT Snoop timestamp is the number of microseconds since 01/01/0000. 131 ** The timeval structure contains the number of microseconds since EPOCH 132 ** (01/01/1970) encoded as: tv.tv_sec, number of seconds since EPOCH and 133 ** tv_usec, number of microseconds in current second 134 ** 135 ** Therefore the algorithm is: 136 ** result = tv.tv_sec * 1000000 137 ** result += tv.tv_usec 138 ** result += EPOCH_OFFSET 139 *******************************************************************************/ 140static void tv_to_btsnoop_ts(uint32_t *out_lo, uint32_t *out_hi, struct timeval *tv) 141{ 142 /* multiply the seconds by 1000000 */ 143 HCIDISP_MULT_64(tv->tv_sec, 0xf4240, *out_lo, *out_hi); 144 145 /* add the microseconds */ 146 HCIDISP_ADD_64((uint32_t)tv->tv_usec, 0, *out_lo, *out_hi); 147 148 /* add the epoch */ 149 HCIDISP_ADD_64(BTSNOOP_EPOCH_LO, BTSNOOP_EPOCH_HI, *out_lo, *out_hi); 150} 151 152/******************************************************************************* 153 ** 154 ** Function l_to_be 155 ** 156 ** Description Function to convert a 32 bit value into big endian format 157 ** 158 ** Returns 32 bit value in big endian format 159*******************************************************************************/ 160static uint32_t l_to_be(uint32_t x) 161{ 162 #if __BIG_ENDIAN != TRUE 163 x = (x >> 24) | 164 ((x >> 8) & 0xFF00) | 165 ((x << 8) & 0xFF0000) | 166 (x << 24); 167 #endif 168 return x; 169} 170 171/******************************************************************************* 172 ** 173 ** Function btsnoop_is_open 174 ** 175 ** Description Function to check if BTSNOOP is open 176 ** 177 ** Returns 1 if open otherwise 0 178*******************************************************************************/ 179int btsnoop_is_open(void) 180{ 181#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 182 SNOOPDBG("btsnoop_is_open: snoop fd = %d\n", hci_btsnoop_fd); 183 184 if (hci_btsnoop_fd != -1) 185 { 186 return 1; 187 } 188 return 0; 189#else 190 return 2; /* Snoop not available */ 191#endif 192} 193 194/******************************************************************************* 195 ** 196 ** Function btsnoop_log_open 197 ** 198 ** Description Function to open the BTSNOOP file 199 ** 200 ** Returns None 201*******************************************************************************/ 202static int btsnoop_log_open(char *btsnoop_logfile) 203{ 204#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 205 hci_btsnoop_fd = -1; 206 207 SNOOPDBG("btsnoop_log_open: snoop log file = %s\n", btsnoop_logfile); 208 209 /* write the BT snoop header */ 210 if ((btsnoop_logfile != NULL) && (strlen(btsnoop_logfile) != 0)) 211 { 212 hci_btsnoop_fd = open(btsnoop_logfile, \ 213 O_WRONLY|O_CREAT|O_TRUNC, \ 214 S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH); 215 if (hci_btsnoop_fd == -1) 216 { 217 perror("open"); 218 SNOOPDBG("btsnoop_log_open: Unable to open snoop log file\n"); 219 hci_btsnoop_fd = -1; 220 return 0; 221 } 222 write(hci_btsnoop_fd, "btsnoop\0\0\0\0\1\0\0\x3\xea", 16); 223 return 1; 224 } 225#endif 226 return 2; /* Snoop not available */ 227} 228 229/******************************************************************************* 230 ** 231 ** Function btsnoop_log_close 232 ** 233 ** Description Function to close the BTSNOOP file 234 ** 235 ** Returns None 236*******************************************************************************/ 237static int btsnoop_log_close(void) 238{ 239#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 240 /* write the BT snoop header */ 241 if (hci_btsnoop_fd != -1) 242 { 243 SNOOPDBG("btsnoop_log_close: Closing snoop log file\n"); 244 close(hci_btsnoop_fd); 245 hci_btsnoop_fd = -1; 246 return 1; 247 } 248 return 0; 249#else 250 return 2; /* Snoop not available */ 251#endif 252} 253 254/******************************************************************************* 255 ** 256 ** Function btsnoop_hci_cmd 257 ** 258 ** Description Function to add a command in the BTSNOOP file 259 ** 260 ** Returns None 261*******************************************************************************/ 262void btsnoop_hci_cmd(uint8_t *p) 263{ 264 SNOOPDBG("btsnoop_hci_cmd: fd = %d", hci_btsnoop_fd); 265 266 if (hci_btsnoop_fd != -1) 267 { 268 uint32_t value, value_hi; 269 struct timeval tv; 270 271 /* since these display functions are called from different contexts */ 272 utils_lock(); 273 274 /* store the length in both original and included fields */ 275 value = l_to_be(p[2] + 4); 276 write(hci_btsnoop_fd, &value, 4); 277 write(hci_btsnoop_fd, &value, 4); 278 /* flags: command sent from the host */ 279 value = l_to_be(2); 280 write(hci_btsnoop_fd, &value, 4); 281 /* drops: none */ 282 value = 0; 283 write(hci_btsnoop_fd, &value, 4); 284 /* time */ 285 gettimeofday(&tv, NULL); 286 tv_to_btsnoop_ts(&value, &value_hi, &tv); 287 value_hi = l_to_be(value_hi); 288 value = l_to_be(value); 289 write(hci_btsnoop_fd, &value_hi, 4); 290 write(hci_btsnoop_fd, &value, 4); 291 /* data */ 292 write(hci_btsnoop_fd, "\x1", 1); 293 write(hci_btsnoop_fd, p, p[2] + 3); 294 295 /* since these display functions are called from different contexts */ 296 utils_unlock(); 297 } 298} 299 300/******************************************************************************* 301 ** 302 ** Function btsnoop_hci_evt 303 ** 304 ** Description Function to add a event in the BTSNOOP file 305 ** 306 ** Returns None 307*******************************************************************************/ 308void btsnoop_hci_evt(uint8_t *p) 309{ 310 SNOOPDBG("btsnoop_hci_evt: fd = %d", hci_btsnoop_fd); 311 312 if (hci_btsnoop_fd != -1) 313 { 314 uint32_t value, value_hi; 315 struct timeval tv; 316 317 /* since these display functions are called from different contexts */ 318 utils_lock(); 319 320 /* store the length in both original and included fields */ 321 value = l_to_be(p[1] + 3); 322 write(hci_btsnoop_fd, &value, 4); 323 write(hci_btsnoop_fd, &value, 4); 324 /* flags: event received in the host */ 325 value = l_to_be(3); 326 write(hci_btsnoop_fd, &value, 4); 327 /* drops: none */ 328 value = 0; 329 write(hci_btsnoop_fd, &value, 4); 330 /* time */ 331 gettimeofday(&tv, NULL); 332 tv_to_btsnoop_ts(&value, &value_hi, &tv); 333 value_hi = l_to_be(value_hi); 334 value = l_to_be(value); 335 write(hci_btsnoop_fd, &value_hi, 4); 336 write(hci_btsnoop_fd, &value, 4); 337 /* data */ 338 write(hci_btsnoop_fd, "\x4", 1); 339 write(hci_btsnoop_fd, p, p[1] + 2); 340 341 /* since these display functions are called from different contexts */ 342 utils_unlock(); 343 } 344} 345 346/******************************************************************************* 347 ** 348 ** Function btsnoop_sco_data 349 ** 350 ** Description Function to add a SCO data packet in the BTSNOOP file 351 ** 352 ** Returns None 353*******************************************************************************/ 354void btsnoop_sco_data(uint8_t *p, uint8_t is_rcvd) 355{ 356 SNOOPDBG("btsnoop_sco_data: fd = %d", hci_btsnoop_fd); 357 358 if (hci_btsnoop_fd != -1) 359 { 360 uint32_t value, value_hi; 361 struct timeval tv; 362 363 /* since these display functions are called from different contexts */ 364 utils_lock(); 365 366 /* store the length in both original and included fields */ 367 value = l_to_be(p[2] + 4); 368 write(hci_btsnoop_fd, &value, 4); 369 write(hci_btsnoop_fd, &value, 4); 370 /* flags: data can be sent or received */ 371 value = l_to_be(is_rcvd?1:0); 372 write(hci_btsnoop_fd, &value, 4); 373 /* drops: none */ 374 value = 0; 375 write(hci_btsnoop_fd, &value, 4); 376 /* time */ 377 gettimeofday(&tv, NULL); 378 tv_to_btsnoop_ts(&value, &value_hi, &tv); 379 value_hi = l_to_be(value_hi); 380 value = l_to_be(value); 381 write(hci_btsnoop_fd, &value_hi, 4); 382 write(hci_btsnoop_fd, &value, 4); 383 /* data */ 384 write(hci_btsnoop_fd, "\x3", 1); 385 write(hci_btsnoop_fd, p, p[2] + 3); 386 387 /* since these display functions are called from different contexts */ 388 utils_unlock(); 389 } 390} 391 392/******************************************************************************* 393 ** 394 ** Function btsnoop_acl_data 395 ** 396 ** Description Function to add an ACL data packet in the BTSNOOP file 397 ** 398 ** Returns None 399*******************************************************************************/ 400void btsnoop_acl_data(uint8_t *p, uint8_t is_rcvd) 401{ 402 SNOOPDBG("btsnoop_acl_data: fd = %d", hci_btsnoop_fd); 403 if (hci_btsnoop_fd != -1) 404 { 405 uint32_t value, value_hi; 406 struct timeval tv; 407 408 /* since these display functions are called from different contexts */ 409 utils_lock(); 410 411 /* store the length in both original and included fields */ 412 value = l_to_be((p[3]<<8) + p[2] + 5); 413 write(hci_btsnoop_fd, &value, 4); 414 write(hci_btsnoop_fd, &value, 4); 415 /* flags: data can be sent or received */ 416 value = l_to_be(is_rcvd?1:0); 417 write(hci_btsnoop_fd, &value, 4); 418 /* drops: none */ 419 value = 0; 420 write(hci_btsnoop_fd, &value, 4); 421 /* time */ 422 gettimeofday(&tv, NULL); 423 tv_to_btsnoop_ts(&value, &value_hi, &tv); 424 value_hi = l_to_be(value_hi); 425 value = l_to_be(value); 426 write(hci_btsnoop_fd, &value_hi, 4); 427 write(hci_btsnoop_fd, &value, 4); 428 /* data */ 429 write(hci_btsnoop_fd, "\x2", 1); 430 write(hci_btsnoop_fd, p, (p[3]<<8) + p[2] + 4); 431 432 /* since these display functions are called from different contexts */ 433 utils_unlock(); 434 } 435} 436 437 438/******************************************************************************** 439 ** API allow external realtime parsing of output using e.g hcidump 440 *********************************************************************************/ 441 442#define EXT_PARSER_PORT 4330 443 444static pthread_t thread_id; 445static int s_listen = -1; 446static int ext_parser_fd = -1; 447 448static void ext_parser_detached(void); 449 450static int ext_parser_accept(int port) 451{ 452 socklen_t clilen; 453 struct sockaddr_in cliaddr, servaddr; 454 int s, srvlen; 455 int n = 1; 456 int size_n; 457 int result = 0; 458 459 ALOGD("waiting for connection on port %d", port); 460 461 s_listen = socket(AF_INET, SOCK_STREAM, 0); 462 463 if (s_listen < 0) 464 { 465 ALOGE("listener not created: listen fd %d", s_listen); 466 return -1; 467 } 468 469 bzero(&servaddr, sizeof(servaddr)); 470 servaddr.sin_family = AF_INET; 471 servaddr.sin_addr.s_addr = htonl(INADDR_ANY); 472 servaddr.sin_port = htons(port); 473 474 srvlen = sizeof(servaddr); 475 476 /* allow reuse of sock addr upon bind */ 477 result = setsockopt(s_listen, SOL_SOCKET, SO_REUSEADDR, &n, sizeof(n)); 478 479 if (result<0) 480 { 481 perror("setsockopt"); 482 } 483 484 result = bind(s_listen, (struct sockaddr *) &servaddr, srvlen); 485 486 if (result < 0) 487 perror("bind"); 488 489 result = listen(s_listen, 1); 490 491 if (result < 0) 492 perror("listen"); 493 494 clilen = sizeof(struct sockaddr_in); 495 496 s = accept(s_listen, (struct sockaddr *) &cliaddr, &clilen); 497 498 if (s < 0) 499{ 500 perror("accept"); 501 return -1; 502 } 503 504 ALOGD("connected (%d)", s); 505 506 return s; 507} 508 509static int send_ext_parser(char *p, int len) 510{ 511 int n; 512 513 /* check if io socket is connected */ 514 if (ext_parser_fd == -1) 515 return 0; 516 517 SNOOPDBG("write %d to snoop socket\n", len); 518 519 n = write(ext_parser_fd, p, len); 520 521 if (n<=0) 522 { 523 ext_parser_detached(); 524 } 525 526 return n; 527} 528 529static void ext_parser_detached(void) 530{ 531 ALOGD("ext parser detached"); 532 533 if (ext_parser_fd>0) 534 close(ext_parser_fd); 535 536 if (s_listen > 0) 537 close(s_listen); 538 539 ext_parser_fd = -1; 540 s_listen = -1; 541} 542 543static void interruptFn (int sig) 544{ 545 ALOGD("interruptFn"); 546 pthread_exit(0); 547} 548 549static void ext_parser_thread(void* param) 550{ 551 int fd; 552 int sig = SIGUSR2; 553 sigset_t sigSet; 554 sigemptyset (&sigSet); 555 sigaddset (&sigSet, sig); 556 557 ALOGD("ext_parser_thread"); 558 559 prctl(PR_SET_NAME, (unsigned long)"BtsnoopExtParser", 0, 0, 0); 560 561 pthread_sigmask (SIG_UNBLOCK, &sigSet, NULL); 562 563 struct sigaction act; 564 act.sa_handler = interruptFn; 565 sigaction (sig, &act, NULL ); 566 567 do 568 { 569 fd = ext_parser_accept(EXT_PARSER_PORT); 570 571 ext_parser_fd = fd; 572 573 ALOGD("ext parser attached on fd %d\n", ext_parser_fd); 574 } while (1); 575} 576 577void btsnoop_stop_listener(void) 578{ 579 ALOGD("btsnoop_init"); 580 ext_parser_detached(); 581} 582 583void btsnoop_init(void) 584{ 585#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE) 586 ALOGD("btsnoop_init"); 587 588 /* always setup ext listener port */ 589 if (pthread_create(&thread_id, NULL, 590 (void*)ext_parser_thread,NULL)!=0) 591 perror("pthread_create"); 592#endif 593} 594 595void btsnoop_open(char *p_path) 596{ 597#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 598 ALOGD("btsnoop_open"); 599 btsnoop_log_open(p_path); 600#endif // BTSNOOPDISP_INCLUDED 601} 602 603void btsnoop_close(void) 604{ 605#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 606 ALOGD("btsnoop_close"); 607 btsnoop_log_close(); 608#endif 609} 610 611void btsnoop_cleanup (void) 612{ 613#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE) 614 ALOGD("btsnoop_cleanup"); 615 pthread_kill(thread_id, SIGUSR2); 616 pthread_join(thread_id, NULL); 617 ext_parser_detached(); 618#endif 619} 620 621 622#define HCIT_TYPE_COMMAND 1 623#define HCIT_TYPE_ACL_DATA 2 624#define HCIT_TYPE_SCO_DATA 3 625#define HCIT_TYPE_EVENT 4 626 627void btsnoop_capture(HC_BT_HDR *p_buf, uint8_t is_rcvd) 628{ 629 uint8_t *p = (uint8_t *)(p_buf + 1) + p_buf->offset; 630 631 SNOOPDBG("btsnoop_capture: fd = %d, type %x, rcvd %d, ext %d", \ 632 hci_btsnoop_fd, p_buf->event, is_rcvd, ext_parser_fd); 633 634#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE) 635 if (ext_parser_fd > 0) 636 { 637 uint8_t tmp = *p; 638 639 /* borrow one byte for H4 packet type indicator */ 640 p--; 641 642 switch (p_buf->event & MSG_EVT_MASK) 643 { 644 case MSG_HC_TO_STACK_HCI_EVT: 645 *p = HCIT_TYPE_EVENT; 646 break; 647 case MSG_HC_TO_STACK_HCI_ACL: 648 case MSG_STACK_TO_HC_HCI_ACL: 649 *p = HCIT_TYPE_ACL_DATA; 650 break; 651 case MSG_HC_TO_STACK_HCI_SCO: 652 case MSG_STACK_TO_HC_HCI_SCO: 653 *p = HCIT_TYPE_SCO_DATA; 654 break; 655 case MSG_STACK_TO_HC_HCI_CMD: 656 *p = HCIT_TYPE_COMMAND; 657 break; 658 } 659 660 send_ext_parser((char*)p, p_buf->len+1); 661 *(++p) = tmp; 662 return; 663 } 664#endif 665 666#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE) 667 if (hci_btsnoop_fd == -1) 668 return; 669 670 switch (p_buf->event & MSG_EVT_MASK) 671 { 672 case MSG_HC_TO_STACK_HCI_EVT: 673 SNOOPDBG("TYPE : EVT"); 674 btsnoop_hci_evt(p); 675 break; 676 case MSG_HC_TO_STACK_HCI_ACL: 677 case MSG_STACK_TO_HC_HCI_ACL: 678 SNOOPDBG("TYPE : ACL"); 679 btsnoop_acl_data(p, is_rcvd); 680 break; 681 case MSG_HC_TO_STACK_HCI_SCO: 682 case MSG_STACK_TO_HC_HCI_SCO: 683 SNOOPDBG("TYPE : SCO"); 684 btsnoop_sco_data(p, is_rcvd); 685 break; 686 case MSG_STACK_TO_HC_HCI_CMD: 687 SNOOPDBG("TYPE : CMD"); 688 btsnoop_hci_cmd(p); 689 break; 690 } 691#endif // BTSNOOPDISP_INCLUDED 692} 693 694 695