From 4f72c38007343612d4a074d4f4c3997c2c8ab724 Mon Sep 17 00:00:00 2001 From: Marcel Holtmann Date: Fri, 3 Jan 2014 22:43:05 -0800 Subject: monitor: Add support for Ellisys HCI Injection --- monitor/ellisys.c | 161 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 monitor/ellisys.c (limited to 'monitor/ellisys.c') diff --git a/monitor/ellisys.c b/monitor/ellisys.c new file mode 100644 index 000000000..d21c16425 --- /dev/null +++ b/monitor/ellisys.c @@ -0,0 +1,161 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2011-2012 Intel Corporation + * Copyright (C) 2004-2010 Marcel Holtmann + * + * + * 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 + +#include +#include + +#include "btsnoop.h" +#include "ellisys.h" + +static int ellisys_fd = -1; +static uint16_t ellisys_index = 0xffff; + +void ellisys_enable(const char *server, uint16_t port) +{ + struct sockaddr_in addr; + int fd; + + if (ellisys_fd >= 0) { + fprintf(stderr, "Ellisys injection already enabled\n"); + return; + } + + fd = socket(PF_INET, SOCK_DGRAM | SOCK_CLOEXEC, 0); + if (fd < 0) { + perror("Failed to open UDP injection socket"); + return; + } + + memset(&addr, 0, sizeof(addr)); + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = inet_addr(server); + addr.sin_port = htons(port); + + if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { + perror("Failed to connect UDP injection socket"); + close(fd); + return; + } + + ellisys_fd = fd; +} + +void ellisys_inject_hci(struct timeval *tv, uint16_t index, uint16_t opcode, + const void *data, uint16_t size) +{ + uint8_t msg[] = { + /* HCI Injection Service, Version 1 */ + 0x02, 0x00, 0x01, + /* DateTimeNs Object */ + 0x02, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* Bitrate Object, 12000000 bps */ + 0x80, 0x00, 0x1b, 0x37, 0x4b, + /* HCI Packet Type Object */ + 0x81, 0x00, + /* HCI Packet Data Object */ + 0x82 + }; + long nsec; + time_t t; + struct tm tm; + struct iovec iov[2]; + int iovcnt; + + if (!tv) + return; + + if (ellisys_fd < 0) + return; + + if (ellisys_index == 0xffff) + ellisys_index = index; + + if (index != ellisys_index) + return; + + t = tv->tv_sec; + localtime_r(&t, &tm); + + nsec = ((tm.tm_sec + (tm.tm_min * 60) + + (tm.tm_hour * 3600)) * 1000000l + tv->tv_usec) * 1000l; + + msg[4] = (1900 + tm.tm_year) & 0xff; + msg[5] = (1900 + tm.tm_year) >> 8; + msg[6] = (tm.tm_mon + 1) & 0xff; + msg[7] = tm.tm_mday & 0xff; + msg[8] = (nsec & 0x0000000000ffl); + msg[9] = (nsec & 0x00000000ff00l) >> 8; + msg[10] = (nsec & 0x000000ff0000l) >> 16; + msg[11] = (nsec & 0x0000ff000000l) >> 24; + msg[12] = (nsec & 0x00ff00000000l) >> 32; + msg[13] = (nsec & 0xff0000000000l) >> 40; + + switch (opcode) { + case BTSNOOP_OPCODE_COMMAND_PKT: + msg[20] = 0x01; + break; + case BTSNOOP_OPCODE_EVENT_PKT: + msg[20] = 0x84; + break; + case BTSNOOP_OPCODE_ACL_TX_PKT: + msg[20] = 0x02; + break; + case BTSNOOP_OPCODE_ACL_RX_PKT: + msg[20] = 0x82; + break; + case BTSNOOP_OPCODE_SCO_TX_PKT: + msg[20] = 0x03; + break; + case BTSNOOP_OPCODE_SCO_RX_PKT: + msg[20] = 0x83; + break; + default: + return; + } + + iov[0].iov_base = msg; + iov[0].iov_len = sizeof(msg); + + if (size > 0) { + iov[1].iov_base = (void *) data; + iov[1].iov_len = size; + iovcnt = 2; + } else + iovcnt = 1; + + if (writev(ellisys_fd, iov, iovcnt) < 0) + perror("Failed to send Ellisys injection packet"); +} -- cgit v1.2.1