M7350/external/bluetooth/bluez/emulator/b1ee.c
2024-09-09 08:57:42 +00:00

257 lines
5.7 KiB
C

/*
*
* BlueZ - Bluetooth protocol stack for Linux
*
* Copyright (C) 2011-2012 Intel Corporation
* Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include "monitor/mainloop.h"
#define DEFAULT_SERVER "b1ee.com"
#define DEFAULT_HOST_PORT "45550" /* 0xb1ee */
#define DEFAULT_SNIFFER_PORT "45551" /* 0xb1ef */
static int sniffer_fd;
static int server_fd;
static int vhci_fd;
static void sniffer_read_callback(int fd, uint32_t events, void *user_data)
{
static uint8_t buf[4096];
ssize_t len;
if (events & (EPOLLERR | EPOLLHUP))
return;
again:
len = recv(fd, buf, sizeof(buf), MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN)
goto again;
return;
}
printf("Sniffer received: %zi bytes\n", len);
}
static uint8_t *server_pkt_data;
static uint8_t server_pkt_type;
static uint16_t server_pkt_expect;
static uint16_t server_pkt_len;
static uint16_t server_pkt_offset;
static void server_read_callback(int fd, uint32_t events, void *user_data)
{
static uint8_t buf[4096];
uint8_t *ptr = buf;
ssize_t len;
uint16_t count;
if (events & (EPOLLERR | EPOLLHUP))
return;
again:
len = recv(fd, buf + server_pkt_offset,
sizeof(buf) - server_pkt_offset, MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN)
goto again;
return;
}
count = server_pkt_offset + len;
while (count > 0) {
hci_event_hdr *evt_hdr;
if (!server_pkt_data) {
server_pkt_type = ptr[0];
switch (server_pkt_type) {
case HCI_EVENT_PKT:
if (count < HCI_EVENT_HDR_SIZE + 1) {
server_pkt_offset += len;
return;
}
evt_hdr = (hci_event_hdr *) (ptr + 1);
server_pkt_expect = HCI_EVENT_HDR_SIZE +
evt_hdr->plen + 1;
server_pkt_data = malloc(server_pkt_expect);
server_pkt_len = 0;
break;
default:
fprintf(stderr, "Unknown packet from server\n");
return;
}
server_pkt_offset = 0;
}
if (count >= server_pkt_expect) {
ssize_t written;
memcpy(server_pkt_data + server_pkt_len,
ptr, server_pkt_expect);
ptr += server_pkt_expect;
count -= server_pkt_expect;
written = write(vhci_fd, server_pkt_data,
server_pkt_len + server_pkt_expect);
if (written != server_pkt_len + server_pkt_expect)
fprintf(stderr, "Write to /dev/vhci failed\n");
free(server_pkt_data);
server_pkt_data = NULL;
} else {
memcpy(server_pkt_data + server_pkt_len, ptr, count);
server_pkt_len += count;
server_pkt_expect -= count;
count = 0;
}
}
}
static void vhci_read_callback(int fd, uint32_t events, void *user_data)
{
unsigned char buf[4096];
ssize_t len, written;
if (events & (EPOLLERR | EPOLLHUP))
return;
len = read(fd, buf, sizeof(buf));
if (len < 0)
return;
written = write(server_fd, buf, len);
if (written != len)
fprintf(stderr, "Write to server failed\n");
}
static void signal_callback(int signum, void *user_data)
{
switch (signum) {
case SIGINT:
case SIGTERM:
mainloop_quit();
break;
}
}
static int do_connect(const char *node, const char *service)
{
struct addrinfo hints;
struct addrinfo *info, *res;
int err, fd = -1;
memset(&hints, 0, sizeof(hints));
hints.ai_family = PF_UNSPEC;
hints.ai_socktype = SOCK_STREAM;
err = getaddrinfo(DEFAULT_SERVER, DEFAULT_HOST_PORT, &hints, &res);
if (err) {
perror(gai_strerror(err));
exit(1);
}
for (info = res; info; info = info->ai_next) {
char str[INET6_ADDRSTRLEN];
inet_ntop(info->ai_family, info->ai_addr->sa_data,
str, sizeof(str));
fd = socket(info->ai_family, info->ai_socktype,
info->ai_protocol);
if (fd < 0)
continue;
printf("Trying to connect to %s on port %s\n", str, service);
if (connect(fd, res->ai_addr, res->ai_addrlen) < 0) {
perror("Failed to connect");
continue;
}
printf("Successfully connected to %s on port %s\n",
str, service);
break;
}
freeaddrinfo(res);
if (res == NULL)
exit(1);
return fd;
}
int main(int argc, char *argv[])
{
const char sniff_cmd[] = { 0x01, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
ssize_t written;
sigset_t mask;
server_fd = do_connect(DEFAULT_SERVER, DEFAULT_HOST_PORT);
sniffer_fd = do_connect(DEFAULT_SERVER, DEFAULT_SNIFFER_PORT);
written = write(sniffer_fd, sniff_cmd, sizeof(sniff_cmd));
if (written < 0)
perror("Failed to enable sniffer");
vhci_fd = open("/dev/vhci", O_RDWR | O_NONBLOCK);
if (vhci_fd < 0) {
perror("Failed to /dev/vhci");
close(server_fd);
exit(1);
}
mainloop_init();
sigemptyset(&mask);
sigaddset(&mask, SIGINT);
sigaddset(&mask, SIGTERM);
mainloop_set_signal(&mask, signal_callback, NULL, NULL);
mainloop_add_fd(sniffer_fd, EPOLLIN, sniffer_read_callback, NULL, NULL);
mainloop_add_fd(server_fd, EPOLLIN, server_read_callback, NULL, NULL);
mainloop_add_fd(vhci_fd, EPOLLIN, vhci_read_callback, NULL, NULL);
return mainloop_run();
}