From 127e3aefba7e7a4baaa4461c75dd768a93c12f2d Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Sun, 25 Aug 2013 11:17:04 -0700 Subject: [PATCH] shared: Add functions for btsnoop file handling --- src/shared/btsnoop.c | 228 +++++++++++++++++++++++++++++++++++++++++++++++++++ src/shared/btsnoop.h | 58 +++++++++++++ 2 files changed, 286 insertions(+) create mode 100644 src/shared/btsnoop.c create mode 100644 src/shared/btsnoop.h diff --git a/src/shared/btsnoop.c b/src/shared/btsnoop.c new file mode 100644 index 000000000..57731daa6 --- /dev/null +++ b/src/shared/btsnoop.c @@ -0,0 +1,228 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include +#include +#include +#include +#include + +#include "btsnoop.h" + +static inline uint64_t ntoh64(uint64_t n) +{ + uint64_t h; + uint64_t tmp = ntohl(n & 0x00000000ffffffff); + + h = ntohl(n >> 32); + h |= tmp << 32; + + return h; +} + +#define hton64(x) ntoh64(x) + +struct btsnoop_hdr { + uint8_t id[8]; /* Identification Pattern */ + uint32_t version; /* Version Number = 1 */ + uint32_t type; /* Datalink Type */ +} __attribute__ ((packed)); +#define BTSNOOP_HDR_SIZE (sizeof(struct btsnoop_hdr)) + +struct btsnoop_pkt { + uint32_t size; /* Original Length */ + uint32_t len; /* Included Length */ + uint32_t flags; /* Packet Flags */ + uint32_t drops; /* Cumulative Drops */ + uint64_t ts; /* Timestamp microseconds */ + uint8_t data[0]; /* Packet Data */ +} __attribute__ ((packed)); +#define BTSNOOP_PKT_SIZE (sizeof(struct btsnoop_pkt)) + +static const uint8_t btsnoop_id[] = { 0x62, 0x74, 0x73, 0x6e, + 0x6f, 0x6f, 0x70, 0x00 }; + +static const uint32_t btsnoop_version = 1; + +struct btsnoop { + int ref_count; + int fd; + uint32_t type; +}; + +struct btsnoop *btsnoop_open(const char *path) +{ + struct btsnoop *btsnoop; + struct btsnoop_hdr hdr; + ssize_t len; + + btsnoop = calloc(1, sizeof(*btsnoop)); + if (!btsnoop) + return NULL; + + btsnoop->fd = open(path, O_RDONLY | O_CLOEXEC); + if (btsnoop->fd < 0) { + free(btsnoop); + return NULL; + } + + len = read(btsnoop->fd, &hdr, BTSNOOP_HDR_SIZE); + if (len < 0 || len != BTSNOOP_HDR_SIZE) + goto failed; + + if (memcmp(hdr.id, btsnoop_id, sizeof(btsnoop_id))) + goto failed; + + if (ntohl(hdr.version) != btsnoop_version) + goto failed; + + btsnoop->type = ntohl(hdr.type); + + return btsnoop_ref(btsnoop); + +failed: + close(btsnoop->fd); + free(btsnoop); + + return NULL; +} + +struct btsnoop *btsnoop_create(const char *path, uint32_t type) +{ + struct btsnoop *btsnoop; + struct btsnoop_hdr hdr; + ssize_t written; + + btsnoop = calloc(1, sizeof(*btsnoop)); + if (!btsnoop) + return NULL; + + btsnoop->fd = open(path, O_WRONLY | O_CREAT | O_TRUNC | O_CLOEXEC, + S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH); + if (btsnoop->fd < 0) { + free(btsnoop); + return NULL; + } + + btsnoop->type = type; + + memcpy(hdr.id, btsnoop_id, sizeof(btsnoop_id)); + hdr.version = htonl(btsnoop_version); + hdr.type = htonl(btsnoop->type); + + written = write(btsnoop->fd, &hdr, BTSNOOP_HDR_SIZE); + if (written < 0) { + close(btsnoop->fd); + free(btsnoop); + return NULL; + } + + return btsnoop_ref(btsnoop); +} + +struct btsnoop *btsnoop_ref(struct btsnoop *btsnoop) +{ + if (!btsnoop) + return NULL; + + __sync_fetch_and_add(&btsnoop->ref_count, 1); + + return btsnoop; +} + +void btsnoop_unref(struct btsnoop *btsnoop) +{ + if (!btsnoop) + return; + + if (__sync_sub_and_fetch(&btsnoop->ref_count, 1)) + return; + + if (btsnoop->fd >= 0) + close(btsnoop->fd); + + free(btsnoop); +} + +uint32_t btsnoop_get_type(struct btsnoop *btsnoop) +{ + if (!btsnoop) + return BTSNOOP_TYPE_INVALID; + + return btsnoop->type; +} + +bool btsnoop_write(struct btsnoop *btsnoop, struct timeval *tv, + uint32_t flags, const void *data, uint16_t size) +{ + struct btsnoop_pkt pkt; + uint64_t ts; + ssize_t written; + + if (!btsnoop || !tv) + return false; + + ts = (tv->tv_sec - 946684800ll) * 1000000ll + tv->tv_usec; + + pkt.size = htonl(size); + pkt.len = htonl(size); + pkt.flags = htonl(flags); + pkt.drops = htonl(0); + pkt.ts = hton64(ts + 0x00E03AB44A676000ll); + + written = write(btsnoop->fd, &pkt, BTSNOOP_PKT_SIZE); + if (written < 0) + return false; + + if (data && size > 0) { + written = write(btsnoop->fd, data, size); + if (written < 0) + return false; + } + + return true; +} + +bool btsnoop_write_phy(struct btsnoop *btsnoop, struct timeval *tv, + uint16_t frequency, const void *data, uint16_t size) +{ + uint32_t flags; + + if (!btsnoop) + return false; + + switch (btsnoop->type) { + case BTSNOOP_TYPE_EXTENDED_PHY: + flags = (1 << 16) | frequency; + break; + + default: + return false; + } + + return btsnoop_write(btsnoop, tv, flags, data, size); +} diff --git a/src/shared/btsnoop.h b/src/shared/btsnoop.h new file mode 100644 index 000000000..b94f99840 --- /dev/null +++ b/src/shared/btsnoop.h @@ -0,0 +1,58 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2012 Intel Corporation. All rights reserved. + * + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include +#include +#include + +#define BTSNOOP_TYPE_INVALID 0 +#define BTSNOOP_TYPE_HCI 1001 +#define BTSNOOP_TYPE_UART 1002 +#define BTSNOOP_TYPE_BCSP 1003 +#define BTSNOOP_TYPE_3WIRE 1004 +#define BTSNOOP_TYPE_EXTENDED_HCI 2001 +#define BTSNOOP_TYPE_EXTENDED_PHY 2002 + +#define BTSNOOP_OPCODE_NEW_INDEX 0 +#define BTSNOOP_OPCODE_DEL_INDEX 1 +#define BTSNOOP_OPCODE_COMMAND_PKT 2 +#define BTSNOOP_OPCODE_EVENT_PKT 3 +#define BTSNOOP_OPCODE_ACL_TX_PKT 4 +#define BTSNOOP_OPCODE_ACL_RX_PKT 5 +#define BTSNOOP_OPCODE_SCO_TX_PKT 6 +#define BTSNOOP_OPCODE_SCO_RX_PKT 7 + +struct btsnoop; + +struct btsnoop *btsnoop_open(const char *path); +struct btsnoop *btsnoop_create(const char *path, uint32_t type); + +struct btsnoop *btsnoop_ref(struct btsnoop *btsnoop); +void btsnoop_unref(struct btsnoop *btsnoop); + +uint32_t btsnoop_get_type(struct btsnoop *btsnoop); + +bool btsnoop_write(struct btsnoop *btsnoop, struct timeval *tv, + uint32_t flags, const void *data, uint16_t size); +bool btsnoop_write_phy(struct btsnoop *btsnoop, struct timeval *tv, + uint16_t frequency, const void *data, uint16_t size); -- 2.11.0