hci/src/btsnoop.c

/******************************************************************************
 *
 *  Copyright (C) 2009-2012 Broadcom Corporation
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at:
 *
 *  http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 *
 ******************************************************************************/

/****************************************************************************
 *
 *  Name:       btsnoopdisp.c
 *
 *  Function:   this file contains functions to generate a BTSNOOP file
 *
 *
 ****************************************************************************/
#include <stdio.h>
#include <dlfcn.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <sys/prctl.h>
#include <unistd.h>
#include <ctype.h>
#include <fcntl.h>

#include <arpa/inet.h>
#include <netinet/in.h>
#include <netdb.h>

/* for gettimeofday */
#include <sys/time.h>
/* for the S_* open parameters */
#include <sys/stat.h>
/* for write */
#include <unistd.h>
/* for O_* open parameters */
#include <fcntl.h>
/* defines the O_* open parameters */
#include <fcntl.h>

#define LOG_TAG "BTSNOOP-DISP"
#include <cutils/log.h>

#include "bt_hci_bdroid.h"
#include "utils.h"

#ifndef BTSNOOP_DBG
#define BTSNOOP_DBG FALSE
#endif

#if (BTSNOOP_DBG == TRUE)
#define SNOOPDBG(param, ...) {ALOGD(param, ## __VA_ARGS__);}
#else
#define SNOOPDBG(param, ...) {}
#endif

/* file descriptor of the BT snoop file (by default, -1 means disabled) */
int hci_btsnoop_fd = -1;

/* Macro to perform a multiplication of 2 unsigned 32bit values and store the result
 * in an unsigned 64 bit value (as two 32 bit variables):
 * u64 = u32In1 * u32In2
 * u32OutLow = u64[31:0]
 * u32OutHi = u64[63:32]
 * basically the algorithm:
 * (hi1*2^16 + lo1)*(hi2*2^16 + lo2) = lo1*lo2 + (hi1*hi2)*2^32 + (hi1*lo2 + hi2*lo1)*2^16
 * and the carry is propagated 16 bit by 16 bit:
 * result[15:0] = lo1*lo2 & 0xFFFF
 * result[31:16] = ((lo1*lo2) >> 16) + (hi1*lo2 + hi2*lo1)
 * and so on
 */
#define HCIDISP_MULT_64(u32In1, u32In2, u32OutLo, u32OutHi)                             \
do {                                                                                    \
    uint32_t u32In1Tmp = u32In1;                                                          \
    uint32_t u32In2Tmp = u32In2;                                                          \
    uint32_t u32Tmp, u32Carry;                                                            \
    u32OutLo = (u32In1Tmp & 0xFFFF) * (u32In2Tmp & 0xFFFF);              /*lo1*lo2*/    \
    u32OutHi = ((u32In1Tmp >> 16) & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF); /*hi1*hi2*/ \
    u32Tmp = (u32In1Tmp & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF);  /*lo1*hi2*/          \
    u32Carry = (uint32_t)((u32OutLo>>16)&0xFFFF);                                         \
    u32Carry += (u32Tmp&0xFFFF);                                                        \
    u32OutLo += (u32Tmp << 16) ;                                                        \
    u32OutHi += (u32Tmp >> 16);                                                         \
    u32Tmp = ((u32In1Tmp >> 16) & 0xFFFF) * (u32In2Tmp & 0xFFFF);                       \
    u32Carry += (u32Tmp)&0xFFFF;                                                        \
    u32Carry>>=16;                                                                      \
    u32OutLo += (u32Tmp << 16);                                                         \
    u32OutHi += (u32Tmp >> 16);                                                         \
    u32OutHi += u32Carry;                                                               \
} while (0)

/* Macro to make an addition of 2 64 bit values:
 * result = (u32OutHi & u32OutLo) + (u32InHi & u32InLo)
 * u32OutHi = result[63:32]
 * u32OutLo = result[31:0]
 */
#define HCIDISP_ADD_64(u32InLo, u32InHi, u32OutLo, u32OutHi)                            \
do {                                                                                    \
    (u32OutLo) += (u32InLo);                                                            \
    if ((u32OutLo) < (u32InLo)) (u32OutHi)++;                                           \
    (u32OutHi) += (u32InHi);                                                            \
} while (0)

/* EPOCH in microseconds since 01/01/0000 : 0x00dcddb3.0f2f8000 */
#define BTSNOOP_EPOCH_HI 0x00dcddb3U
#define BTSNOOP_EPOCH_LO 0x0f2f8000U

/*******************************************************************************
 **
 ** Function         tv_to_btsnoop_ts
 **
 ** Description      This function generate a BT Snoop timestamp.
 **
 ** Returns          void
 **
 ** NOTE
 ** The return value is 64 bit as 2 32 bit variables out_lo and * out_hi.
 ** A BT Snoop timestamp is the number of microseconds since 01/01/0000.
 ** The timeval structure contains the number of microseconds since EPOCH
 ** (01/01/1970) encoded as: tv.tv_sec, number of seconds since EPOCH and
 ** tv_usec, number of microseconds in current second
 **
 ** Therefore the algorithm is:
 **  result = tv.tv_sec * 1000000
 **  result += tv.tv_usec
 **  result += EPOCH_OFFSET
 *******************************************************************************/
static void tv_to_btsnoop_ts(uint32_t *out_lo, uint32_t *out_hi, struct timeval *tv)
{
    /* multiply the seconds by 1000000 */
    HCIDISP_MULT_64(tv->tv_sec, 0xf4240, *out_lo, *out_hi);

    /* add the microseconds */
    HCIDISP_ADD_64((uint32_t)tv->tv_usec, 0, *out_lo, *out_hi);

    /* add the epoch */
    HCIDISP_ADD_64(BTSNOOP_EPOCH_LO, BTSNOOP_EPOCH_HI, *out_lo, *out_hi);
}

/*******************************************************************************
 **
 ** Function         l_to_be
 **
 ** Description      Function to convert a 32 bit value into big endian format
 **
 ** Returns          32 bit value in big endian format
*******************************************************************************/
static uint32_t l_to_be(uint32_t x)
{
    #if __BIG_ENDIAN != TRUE
    x = (x >> 24) |
        ((x >> 8) & 0xFF00) |
        ((x << 8) & 0xFF0000) |
        (x << 24);
    #endif
    return x;
}

/*******************************************************************************
 **
 ** Function         btsnoop_is_open
 **
 ** Description      Function to check if BTSNOOP is open
 **
 ** Returns          1 if open otherwise 0
*******************************************************************************/
int btsnoop_is_open(void)
{
#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    SNOOPDBG("btsnoop_is_open: snoop fd = %d\n", hci_btsnoop_fd);

    if (hci_btsnoop_fd != -1)
    {
        return 1;
    }
    return 0;
#else
    return 2;  /* Snoop not available  */
#endif
}

/*******************************************************************************
 **
 ** Function         btsnoop_log_open
 **
 ** Description      Function to open the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
static int btsnoop_log_open(char *btsnoop_logfile)
{
#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    hci_btsnoop_fd = -1;

    SNOOPDBG("btsnoop_log_open: snoop log file = %s\n", btsnoop_logfile);

    /* write the BT snoop header */
    if ((btsnoop_logfile != NULL) && (strlen(btsnoop_logfile) != 0))
    {
        hci_btsnoop_fd = open(btsnoop_logfile, \
                              O_WRONLY|O_CREAT|O_TRUNC, \
                              S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH);
        if (hci_btsnoop_fd == -1)
        {
            perror("open");
            SNOOPDBG("btsnoop_log_open: Unable to open snoop log file\n");
            hci_btsnoop_fd = -1;
            return 0;
        }
        write(hci_btsnoop_fd, "btsnoop\0\0\0\0\1\0\0\x3\xea", 16);
        return 1;
    }
#endif
    return 2;  /* Snoop not available  */
}

/*******************************************************************************
 **
 ** Function         btsnoop_log_close
 **
 ** Description      Function to close the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
static int btsnoop_log_close(void)
{
#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    /* write the BT snoop header */
    if (hci_btsnoop_fd != -1)
    {
        SNOOPDBG("btsnoop_log_close: Closing snoop log file\n");
        close(hci_btsnoop_fd);
        hci_btsnoop_fd = -1;
        return 1;
    }
    return 0;
#else
    return 2;  /* Snoop not available  */
#endif
}

/*******************************************************************************
 **
 ** Function         btsnoop_hci_cmd
 **
 ** Description      Function to add a command in the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
void btsnoop_hci_cmd(uint8_t *p)
{
    SNOOPDBG("btsnoop_hci_cmd: fd = %d", hci_btsnoop_fd);

    if (hci_btsnoop_fd != -1)
    {
        uint32_t value, value_hi;
        struct timeval tv;

        /* since these display functions are called from different contexts */
        utils_lock();

        /* store the length in both original and included fields */
        value = l_to_be(p[2] + 4);
        write(hci_btsnoop_fd, &value, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* flags: command sent from the host */
        value = l_to_be(2);
        write(hci_btsnoop_fd, &value, 4);
        /* drops: none */
        value = 0;
        write(hci_btsnoop_fd, &value, 4);
        /* time */
        gettimeofday(&tv, NULL);
        tv_to_btsnoop_ts(&value, &value_hi, &tv);
        value_hi = l_to_be(value_hi);
        value = l_to_be(value);
        write(hci_btsnoop_fd, &value_hi, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* data */
        write(hci_btsnoop_fd, "\x1", 1);
        write(hci_btsnoop_fd, p, p[2] + 3);

        /* since these display functions are called from different contexts */
        utils_unlock();
    }
}

/*******************************************************************************
 **
 ** Function         btsnoop_hci_evt
 **
 ** Description      Function to add a event in the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
void btsnoop_hci_evt(uint8_t *p)
{
    SNOOPDBG("btsnoop_hci_evt: fd = %d", hci_btsnoop_fd);

    if (hci_btsnoop_fd != -1)
    {
        uint32_t value, value_hi;
        struct timeval tv;

        /* since these display functions are called from different contexts */
        utils_lock();

        /* store the length in both original and included fields */
        value = l_to_be(p[1] + 3);
        write(hci_btsnoop_fd, &value, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* flags: event received in the host */
        value = l_to_be(3);
        write(hci_btsnoop_fd, &value, 4);
        /* drops: none */
        value = 0;
        write(hci_btsnoop_fd, &value, 4);
        /* time */
        gettimeofday(&tv, NULL);
        tv_to_btsnoop_ts(&value, &value_hi, &tv);
        value_hi = l_to_be(value_hi);
        value = l_to_be(value);
        write(hci_btsnoop_fd, &value_hi, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* data */
        write(hci_btsnoop_fd, "\x4", 1);
        write(hci_btsnoop_fd, p, p[1] + 2);

        /* since these display functions are called from different contexts */
        utils_unlock();
    }
}

/*******************************************************************************
 **
 ** Function         btsnoop_sco_data
 **
 ** Description      Function to add a SCO data packet in the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
void btsnoop_sco_data(uint8_t *p, uint8_t is_rcvd)
{
    SNOOPDBG("btsnoop_sco_data: fd = %d", hci_btsnoop_fd);

    if (hci_btsnoop_fd != -1)
    {
        uint32_t value, value_hi;
        struct timeval tv;

        /* since these display functions are called from different contexts */
        utils_lock();

        /* store the length in both original and included fields */
        value = l_to_be(p[2] + 4);
        write(hci_btsnoop_fd, &value, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* flags: data can be sent or received */
        value = l_to_be(is_rcvd?1:0);
        write(hci_btsnoop_fd, &value, 4);
        /* drops: none */
        value = 0;
        write(hci_btsnoop_fd, &value, 4);
        /* time */
        gettimeofday(&tv, NULL);
        tv_to_btsnoop_ts(&value, &value_hi, &tv);
        value_hi = l_to_be(value_hi);
        value = l_to_be(value);
        write(hci_btsnoop_fd, &value_hi, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* data */
        write(hci_btsnoop_fd, "\x3", 1);
        write(hci_btsnoop_fd, p, p[2] + 3);

        /* since these display functions are called from different contexts */
        utils_unlock();
    }
}

/*******************************************************************************
 **
 ** Function         btsnoop_acl_data
 **
 ** Description      Function to add an ACL data packet in the BTSNOOP file
 **
 ** Returns          None
*******************************************************************************/
void btsnoop_acl_data(uint8_t *p, uint8_t is_rcvd)
{
    SNOOPDBG("btsnoop_acl_data: fd = %d", hci_btsnoop_fd);
    if (hci_btsnoop_fd != -1)
    {
        uint32_t value, value_hi;
        struct timeval tv;

        /* since these display functions are called from different contexts */
        utils_lock();

        /* store the length in both original and included fields */
        value = l_to_be((p[3]<<8) + p[2] + 5);
        write(hci_btsnoop_fd, &value, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* flags: data can be sent or received */
        value = l_to_be(is_rcvd?1:0);
        write(hci_btsnoop_fd, &value, 4);
        /* drops: none */
        value = 0;
        write(hci_btsnoop_fd, &value, 4);
        /* time */
        gettimeofday(&tv, NULL);
        tv_to_btsnoop_ts(&value, &value_hi, &tv);
        value_hi = l_to_be(value_hi);
        value = l_to_be(value);
        write(hci_btsnoop_fd, &value_hi, 4);
        write(hci_btsnoop_fd, &value, 4);
        /* data */
        write(hci_btsnoop_fd, "\x2", 1);
        write(hci_btsnoop_fd, p, (p[3]<<8) + p[2] + 4);

        /* since these display functions are called from different contexts */
        utils_unlock();
    }
}


/********************************************************************************
 ** API allow external realtime parsing of output using e.g hcidump
 *********************************************************************************/

#define EXT_PARSER_PORT 4330

static pthread_t thread_id;
static int s_listen = -1;
static int ext_parser_fd = -1;

static void ext_parser_detached(void);

static int ext_parser_accept(int port)
{
    socklen_t           clilen;
    struct sockaddr_in  cliaddr, servaddr;
    int s, srvlen;
    int n = 1;
    int size_n;
    int result = 0;

    ALOGD("waiting for connection on port %d", port);

    s_listen = socket(AF_INET, SOCK_STREAM, 0);

    if (s_listen < 0)
    {
        ALOGE("listener not created: listen fd %d", s_listen);
        return -1;
    }

    bzero(&servaddr, sizeof(servaddr));
    servaddr.sin_family      = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port        = htons(port);

    srvlen = sizeof(servaddr);

    /* allow reuse of sock addr upon bind */
    result = setsockopt(s_listen, SOL_SOCKET, SO_REUSEADDR, &n, sizeof(n));

    if (result<0)
    {
        perror("setsockopt");
    }

    result = bind(s_listen, (struct sockaddr *) &servaddr, srvlen);

    if (result < 0)
        perror("bind");

    result = listen(s_listen, 1);

    if (result < 0)
        perror("listen");

    clilen = sizeof(struct sockaddr_in);

    s = accept(s_listen, (struct sockaddr *) &cliaddr, &clilen);

    if (s < 0)
{
        perror("accept");
        return -1;
    }

    ALOGD("connected (%d)", s);

    return s;
}

static int send_ext_parser(char *p, int len)
{
    int n;

    /* check if io socket is connected */
    if (ext_parser_fd == -1)
        return 0;

    SNOOPDBG("write %d to snoop socket\n", len);

    n = write(ext_parser_fd, p, len);

    if (n<=0)
    {
        ext_parser_detached();
    }

    return n;
}

static void ext_parser_detached(void)
{
    ALOGD("ext parser detached");

    if (ext_parser_fd>0)
        close(ext_parser_fd);

    if (s_listen > 0)
        close(s_listen);

    ext_parser_fd = -1;
    s_listen = -1;
}

static void interruptFn (int sig)
{
    ALOGD("interruptFn");
    pthread_exit(0);
}

static void ext_parser_thread(void* param)
{
    int fd;
    int sig = SIGUSR2;
    sigset_t sigSet;
    sigemptyset (&sigSet);
    sigaddset (&sigSet, sig);

    ALOGD("ext_parser_thread");

    prctl(PR_SET_NAME, (unsigned long)"BtsnoopExtParser", 0, 0, 0);

    pthread_sigmask (SIG_UNBLOCK, &sigSet, NULL);

    struct sigaction act;
    act.sa_handler = interruptFn;
    sigaction (sig, &act, NULL );

    do
    {
        fd = ext_parser_accept(EXT_PARSER_PORT);

        ext_parser_fd = fd;

        ALOGD("ext parser attached on fd %d\n", ext_parser_fd);
    } while (1);
}

void btsnoop_stop_listener(void)
{
    ALOGD("btsnoop_init");
    ext_parser_detached();
}

void btsnoop_init(void)
{
#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE)
    ALOGD("btsnoop_init");

    /* always setup ext listener port */
    if (pthread_create(&thread_id, NULL,
                       (void*)ext_parser_thread,NULL)!=0)
      perror("pthread_create");
#endif
}

void btsnoop_open(char *p_path)
{
#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    ALOGD("btsnoop_open");
    btsnoop_log_open(p_path);
#endif // BTSNOOPDISP_INCLUDED
}

void btsnoop_close(void)
{
#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    ALOGD("btsnoop_close");
    btsnoop_log_close();
#endif
}

void btsnoop_cleanup (void)
{
#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE)
    ALOGD("btsnoop_cleanup");
    pthread_kill(thread_id, SIGUSR2);
    pthread_join(thread_id, NULL);
    ext_parser_detached();
#endif
}


#define HCIT_TYPE_COMMAND   1
#define HCIT_TYPE_ACL_DATA  2
#define HCIT_TYPE_SCO_DATA  3
#define HCIT_TYPE_EVENT     4

void btsnoop_capture(HC_BT_HDR *p_buf, uint8_t is_rcvd)
{
    uint8_t *p = (uint8_t *)(p_buf + 1) + p_buf->offset;

    SNOOPDBG("btsnoop_capture: fd = %d, type %x, rcvd %d, ext %d", \
             hci_btsnoop_fd, p_buf->event, is_rcvd, ext_parser_fd);

#if defined(BTSNOOP_EXT_PARSER_INCLUDED) && (BTSNOOP_EXT_PARSER_INCLUDED == TRUE)
    if (ext_parser_fd > 0)
    {
        uint8_t tmp = *p;

        /* borrow one byte for H4 packet type indicator */
        p--;

        switch (p_buf->event & MSG_EVT_MASK)
        {
              case MSG_HC_TO_STACK_HCI_EVT:
                  *p = HCIT_TYPE_EVENT;
                  break;
              case MSG_HC_TO_STACK_HCI_ACL:
              case MSG_STACK_TO_HC_HCI_ACL:
                  *p = HCIT_TYPE_ACL_DATA;
                  break;
              case MSG_HC_TO_STACK_HCI_SCO:
              case MSG_STACK_TO_HC_HCI_SCO:
                  *p = HCIT_TYPE_SCO_DATA;
                  break;
              case MSG_STACK_TO_HC_HCI_CMD:
                  *p = HCIT_TYPE_COMMAND;
                  break;
        }

        send_ext_parser((char*)p, p_buf->len+1);
        *(++p) = tmp;
        return;
    }
#endif

#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
    if (hci_btsnoop_fd == -1)
        return;

    switch (p_buf->event & MSG_EVT_MASK)
    {
        case MSG_HC_TO_STACK_HCI_EVT:
            SNOOPDBG("TYPE : EVT");
            btsnoop_hci_evt(p);
            break;
        case MSG_HC_TO_STACK_HCI_ACL:
        case MSG_STACK_TO_HC_HCI_ACL:
            SNOOPDBG("TYPE : ACL");
            btsnoop_acl_data(p, is_rcvd);
            break;
        case MSG_HC_TO_STACK_HCI_SCO:
        case MSG_STACK_TO_HC_HCI_SCO:
            SNOOPDBG("TYPE : SCO");
            btsnoop_sco_data(p, is_rcvd);
            break;
        case MSG_STACK_TO_HC_HCI_CMD:
            SNOOPDBG("TYPE : CMD");
            btsnoop_hci_cmd(p);
            break;
    }
#endif // BTSNOOPDISP_INCLUDED
}