Clean up btsnoop code. (1ad79daa) · Commits · e / os / android_packages_modules_Bluetooth

system/hci/Android.mk

+1 −0

Original line number	Diff line number	Diff line
		@@ -28,6 +28,7 @@ LOCAL_CFLAGS += -std=c99

		LOCAL_C_INCLUDES += \
		$(LOCAL_PATH)/include \
		$(LOCAL_PATH)/../osi/include \
		$(LOCAL_PATH)/../utils/include

		LOCAL_MODULE := libbt-hci

system/hci/include/bt_hci_bdroid.h

+0 −9

Original line number	Diff line number	Diff line
		@@ -66,15 +66,6 @@
		#define BTHC_USERIAL_READ_MEM_SIZE (1024)
		#endif

		#ifndef BTSNOOPDISP_INCLUDED
		#define BTSNOOPDISP_INCLUDED TRUE
		#endif

		/* Disable external parser for production */
		#ifndef BTSNOOP_EXT_PARSER_INCLUDED
		#define BTSNOOP_EXT_PARSER_INCLUDED FALSE
		#endif

		/* Host/Controller lib internal event ID */
		#define HC_EVENT_PRELOAD 0x0001
		#define HC_EVENT_POSTLOAD 0x0002

system/hci/include/btsnoop.h

0 → 100644

+8 −0

Original line number	Diff line number	Diff line
		#pragma once

		#include <stdbool.h>

		void btsnoop_open(const char *p_path);
		void btsnoop_close(void);

		void btsnoop_capture(const HC_BT_HDR *p_buf, bool is_rcvd);

system/hci/src/bt_hci_bdroid.c

+1 −2

Original line number	Diff line number	Diff line
		@@ -36,6 +36,7 @@
		#include "hci.h"
		#include "userial.h"
		#include "bt_utils.h"
		#include "btsnoop.h"
		#include <sys/prctl.h>

		#ifndef BTHC_DBG
		@@ -66,8 +67,6 @@ void lpm_wake_deassert(void);
		void lpm_allow_bt_device_sleep(void);
		void lpm_wake_assert(void);
		void init_vnd_if(unsigned char *local_bdaddr);
		void btsnoop_open(char *p_path);
		void btsnoop_close(void);

		/******************************************************************************
		** Variables

system/hci/src/btsnoop.c

+135 −556

Original line number	Diff line number	Diff line
		@@ -16,281 +16,62 @@
		*
		******************************************************************************/

		/****************************************************************************
		*
		* 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>
		#define LOG_TAG "btsnoop"

		#include <arpa/inet.h>
		#include <assert.h>
		#include <ctype.h>
		#include <cutils/log.h>
		#include <errno.h>
		#include <fcntl.h>
		#include <stdbool.h>

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

		/* for gettimeofday */
		#include <sys/time.h>
		/* for the S_* open parameters */
		#include <stdio.h>
		#include <stdlib.h>
		#include <string.h>
		#include <sys/stat.h>
		/* for write */
		#include <sys/time.h>
		#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"
		#include "bt_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
		#include "utils.h"

		typedef enum {
		kCommandPacket = 1,
		kAclPacket = 2,
		kScoPacket = 3,
		kEventPacket = 4
		} packet_type;

		void btsnoop_net_init();
		void btsnoop_net_cleanup();
		void btsnoop_net_write(const void *data, size_t length);

		/* 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:
		* (hi12^16 + lo1)(hi22^16 + lo2) = lo1lo2 + (hi1hi2)2^32 + (hi1lo2 + hi2lo1)*2^16
		* and the carry is propagated 16 bit by 16 bit:
		* result[15:0] = lo1*lo2 & 0xFFFF
		* result[31:16] = ((lo1lo2) >> 16) + (hi1lo2 + 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); /lo1lo2*/ \
		u32OutHi = ((u32In1Tmp >> 16) & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF); /hi1hi2*/ \
		u32Tmp = (u32In1Tmp & 0xFFFF) * ((u32In2Tmp >> 16) & 0xFFFF); /lo1hi2*/ \
		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

		} packet_type_t;

		/*******************************************************************************
		**
		** 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);
		// Epoch in microseconds since 01/01/0000.
		static const uint64_t BTSNOOP_EPOCH_DELTA = 0x00dcddb30f2f8000ULL;

		/* add the microseconds */
		HCIDISP_ADD_64((uint32_t)tv->tv_usec, 0, out_lo, out_hi);
		// File descriptor for btsnoop file.
		static int hci_btsnoop_fd = -1;

		/* 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);
		void btsnoop_net_open();
		void btsnoop_net_close();
		void btsnoop_net_write(const void *data, size_t length);

		/* 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 */
		}
		static uint64_t btsnoop_timestamp(void) {
		struct timeval tv;
		gettimeofday(&tv, NULL);

		/*******************************************************************************
		**
		** 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
		// Timestamp is in microseconds.
		uint64_t timestamp = tv.tv_sec * 1000 * 1000LL;
		timestamp += tv.tv_usec;
		timestamp += BTSNOOP_EPOCH_DELTA;
		return timestamp;
		}

		/*******************************************************************************
		**
		** Function btsnoop_write
		**
		** Description Writes raw bytes to the BTSNOOP sinks.
		**
		** Returns None
		*******************************************************************************/
		static void btsnoop_write(const void *data, size_t length) {
		if (hci_btsnoop_fd != -1) {
		if (hci_btsnoop_fd != -1)
		write(hci_btsnoop_fd, data, length);
		}

		btsnoop_net_write(data, length);
		}

		/*******************************************************************************
		**
		** Function btsnoop_write_packet
		**
		** Description Writes a single HCI packet to BTSNOOP sinks.
		**
		** Returns None
		*******************************************************************************/
		static void btsnoop_write_packet(packet_type type,
		const uint8_t *packet,
		bool is_received) {
		static void btsnoop_write_packet(packet_type_t type, const uint8_t *packet, bool is_received) {
		int length_he;
		int length;
		int flags;
		@@ -314,18 +95,17 @@ static void btsnoop_write_packet(packet_type type,
		break;
		}

		uint32_t time_hi, time_lo;
		struct timeval tv;
		gettimeofday(&tv, NULL);
		tv_to_btsnoop_ts(&time_lo, &time_hi, &tv);
		uint64_t timestamp = btsnoop_timestamp();
		uint32_t time_hi = timestamp >> 32;
		uint32_t time_lo = timestamp & 0xFFFFFFFF;

		length = l_to_be(length_he);
		flags = l_to_be(flags);
		drops = l_to_be(drops);
		time_hi = l_to_be(time_hi);
		time_lo = l_to_be(time_lo);
		length = htonl(length_he);
		flags = htonl(flags);
		drops = htonl(drops);
		time_hi = htonl(time_hi);
		time_lo = htonl(time_lo);

		/* since these display functions are called from different contexts */
		// This function is called from different contexts.
		utils_lock();

		btsnoop_write(&length, 4);
		@@ -340,258 +120,57 @@ static void btsnoop_write_packet(packet_type type,
		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);
		void btsnoop_open(const char *p_path) {
		assert(p_path != NULL);
		assert(*p_path != '\0');

		s_listen = socket(AF_INET, SOCK_STREAM, 0);
		btsnoop_net_open();

		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)
		{
		UNUSED(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);
		UNUSED(param);

		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
		btsnoop_net_init();
		if (hci_btsnoop_fd != -1) {
		ALOGE("%s btsnoop log file is already open.", __func__);
		return;
		}

		void btsnoop_open(char *p_path)
		{
		#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
		ALOGD("btsnoop_open");
		btsnoop_log_open(p_path);
		#endif // BTSNOOPDISP_INCLUDED
		}
		hci_btsnoop_fd = open(p_path,
		O_WRONLY \| O_CREAT \| O_TRUNC,
		S_IRUSR \| S_IWUSR \| S_IRGRP \| S_IWGRP \| S_IROTH);

		void btsnoop_close(void)
		{
		#if defined(BTSNOOPDISP_INCLUDED) && (BTSNOOPDISP_INCLUDED == TRUE)
		ALOGD("btsnoop_close");
		btsnoop_log_close();
		#endif
		if (hci_btsnoop_fd == -1) {
		ALOGE("%s unable to open '%s': %s", __func__, p_path, strerror(errno));
		return;
		}

		void btsnoop_cleanup (void)
		{
		btsnoop_net_cleanup();
		#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
		write(hci_btsnoop_fd, "btsnoop\0\0\0\0\1\0\0\x3\xea", 16);
		}

		void btsnoop_close(void) {
		if (hci_btsnoop_fd != -1)
		close(hci_btsnoop_fd);
		hci_btsnoop_fd = -1;

		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;
		btsnoop_net_close();
		}

		send_ext_parser((char*)p, p_buf->len+1);
		*(++p) = tmp;
		return;
		}
		#endif
		void btsnoop_capture(const HC_BT_HDR *p_buf, bool is_rcvd) {
		const uint8_t p = (const uint8_t )(p_buf + 1) + p_buf->offset;

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

		switch (p_buf->event & MSG_EVT_MASK)
		{
		switch (p_buf->event & MSG_EVT_MASK) {
		case MSG_HC_TO_STACK_HCI_EVT:
		SNOOPDBG("TYPE : EVT");
		btsnoop_write_packet(kEventPacket, p, false);
		break;
		case MSG_HC_TO_STACK_HCI_ACL:
		case MSG_STACK_TO_HC_HCI_ACL:
		SNOOPDBG("TYPE : ACL");
		btsnoop_write_packet(kAclPacket, p, is_rcvd);
		break;
		case MSG_HC_TO_STACK_HCI_SCO:
		case MSG_STACK_TO_HC_HCI_SCO:
		SNOOPDBG("TYPE : SCO");
		btsnoop_write_packet(kScoPacket, p, is_rcvd);
		break;
		case MSG_STACK_TO_HC_HCI_CMD:
		SNOOPDBG("TYPE : CMD");
		btsnoop_write_packet(kCommandPacket, p, true);
		break;
		}
		#endif // BTSNOOPDISP_INCLUDED
		}