static void accept_callback(int fd, uint32_t events, void *user_data)
{
struct bt_amp *amp = user_data;
struct sockaddr_un addr;
socklen_t len;
int new_fd;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
memset(&addr, 0, sizeof(addr));
len = sizeof(addr);
new_fd = accept4(fd, (struct sockaddr *) &addr, &len,
SOCK_CLOEXEC | SOCK_NONBLOCK);
if (new_fd < 0)
return;
mainloop_remove_fd(fd);
close(fd);
amp->phylink_fd = new_fd;
evt_phy_link_complete(amp);
mainloop_add_fd(new_fd, EPOLLIN, link_callback, amp, NULL);
}
static void connect_callback(int fd, uint32_t events, void *user_data)
{
struct bt_amp *amp = user_data;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
mainloop_remove_fd(fd);
evt_phy_link_complete(amp);
mainloop_add_fd(fd, EPOLLIN, link_callback, amp, NULL);
}
static void server_accept_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data;
struct sockaddr_un addr;
socklen_t len;
int nfd;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
memset(&addr, 0, sizeof(addr));
len = sizeof(addr);
nfd = accept(fd, (struct sockaddr *) &addr, &len);
if (nfd < 0) {
perror("Failed to accept client socket");
return;
}
printf("--- New monitor connection ---\n");
data = malloc(sizeof(*data));
if (!data) {
close(nfd);
return;
}
memset(data, 0, sizeof(*data));
data->channel = HCI_CHANNEL_MONITOR;
data->fd = nfd;
mainloop_add_fd(data->fd, EPOLLIN, client_callback, data, free_data);
}
static void system_socket_callback(int fd, uint32_t events, void *user_data)
{
char buf[4096];
ssize_t len;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
len = read(fd, buf, sizeof(buf));
if (len < 0)
return;
printf("Received %s\n", buf);
if (!strcmp(buf, "bluetooth.start=daemon")) {
if (daemon_pid > 0)
return;
ctl_start();
} else if (!strcmp(buf, "bluetooth.start=snoop")) {
if (snoop_pid > 0)
return;
snoop_start();
} else if (!strcmp(buf, "bluetooth.stop=snoop")) {
if (snoop_pid > 0)
snoop_stop();
}
}
void server_close(struct server *server)
{
if (!server)
return;
mainloop_remove_fd(server->fd);
}
static void cmd_disconn_phy_link(struct bt_amp *amp,
const void *data, uint8_t size)
{
const struct bt_hci_cmd_disconn_phy_link *cmd = data;
if (cmd->phy_handle == 0x00) {
cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
BT_HCI_CMD_DISCONN_PHY_LINK);
return;
}
if (amp->phy_mode == PHY_MODE_IDLE) {
cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
BT_HCI_CMD_DISCONN_PHY_LINK);
return;
}
if (cmd->phy_handle != amp->phy_handle) {
cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
BT_HCI_CMD_DISCONN_PHY_LINK);
return;
}
cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONN_PHY_LINK);
mainloop_remove_fd(amp->phylink_fd);
close(amp->phylink_fd);
evt_disconn_phy_link_complete(amp, cmd->reason);
amp->phy_mode = PHY_MODE_IDLE;
amp->phy_handle = 0x00;
}
static void server_accept_callback(int fd, uint32_t events, void *user_data)
{
struct server *server = user_data;
struct client *client;
enum btdev_type uninitialized_var(type);
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(server->fd);
return;
}
client = malloc(sizeof(*client));
if (!client)
return;
memset(client, 0, sizeof(*client));
client->fd = accept_client(server->fd);
if (client->fd < 0) {
free(client);
return;
}
switch (server->type) {
case SERVER_TYPE_BREDRLE:
type = BTDEV_TYPE_BREDRLE;
break;
case SERVER_TYPE_BREDR:
type = BTDEV_TYPE_BREDR;
break;
case SERVER_TYPE_LE:
type = BTDEV_TYPE_LE;
break;
case SERVER_TYPE_AMP:
type = BTDEV_TYPE_AMP;
break;
case SERVER_TYPE_MONITOR:
goto done;
}
client->btdev = btdev_create(type, server->id);
if (!client->btdev) {
close(client->fd);
free(client);
return;
}
btdev_set_send_handler(client->btdev, client_write_callback, client);
done:
if (mainloop_add_fd(client->fd, EPOLLIN, client_read_callback,
client, client_destroy) < 0) {
btdev_destroy(client->btdev);
close(client->fd);
free(client);
}
}
static void tty_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data = user_data;
ssize_t len;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(data->fd);
return;
}
len = read(data->fd, data->buf + data->offset,
sizeof(data->buf) - data->offset);
if (len < 0)
return;
data->offset += len;
while (data->offset >= sizeof(struct tty_hdr)) {
struct tty_hdr *hdr = (struct tty_hdr *) data->buf;
uint16_t pktlen, opcode, data_len;
struct timeval *tv = NULL;
struct timeval ctv;
uint32_t drops = 0;
data_len = le16_to_cpu(hdr->data_len);
if (data->offset < 2 + data_len)
return;
if (data->offset < sizeof(*hdr) + hdr->hdr_len) {
fprintf(stderr, "Received corrupted data from TTY\n");
memmove(data->buf, data->buf + 2 + data_len,
data->offset);
return;
}
if (!tty_parse_header(hdr->ext_hdr, hdr->hdr_len,
&tv, &ctv, &drops))
fprintf(stderr, "Unable to parse extended header\n");
opcode = le16_to_cpu(hdr->opcode);
pktlen = data_len - 4 - hdr->hdr_len;
btsnoop_write_hci(btsnoop_file, tv, 0, opcode, drops,
hdr->ext_hdr + hdr->hdr_len, pktlen);
ellisys_inject_hci(tv, 0, opcode, hdr->ext_hdr + hdr->hdr_len,
pktlen);
packet_monitor(tv, NULL, 0, opcode,
hdr->ext_hdr + hdr->hdr_len, pktlen);
data->offset -= 2 + data_len;
if (data->offset > 0)
memmove(data->buf, data->buf + 2 + data_len,
data->offset);
}
}
static void io_callback(int fd, uint32_t events, void *user_data)
{
struct io *io = user_data;
if ((events & (EPOLLRDHUP | EPOLLHUP | EPOLLERR))) {
io->read_callback = NULL;
io->write_callback = NULL;
if (!io->disconnect_callback) {
mainloop_remove_fd(io->fd);
return;
}
if (!io->disconnect_callback(io, io->disconnect_data)) {
if (io->disconnect_destroy)
io->disconnect_destroy(io->disconnect_data);
io->disconnect_callback = NULL;
io->disconnect_destroy = NULL;
io->disconnect_data = NULL;
io->events &= ~EPOLLRDHUP;
mainloop_modify_fd(io->fd, io->events);
}
}
if ((events & EPOLLIN) && io->read_callback) {
if (!io->read_callback(io, io->read_data)) {
if (io->read_destroy)
io->read_destroy(io->read_data);
io->read_callback = NULL;
io->read_destroy = NULL;
io->read_data = NULL;
io->events &= ~EPOLLIN;
mainloop_modify_fd(io->fd, io->events);
}
}
if ((events & EPOLLOUT) && io->write_callback) {
if (!io->write_callback(io, io->write_data)) {
if (io->write_destroy)
io->write_destroy(io->write_data);
io->write_callback = NULL;
io->write_destroy = NULL;
io->write_data = NULL;
io->events &= ~EPOLLOUT;
mainloop_modify_fd(io->fd, io->events);
}
}
}
void io_destroy(struct io *io)
{
if (!io)
return;
io->read_callback = NULL;
io->write_callback = NULL;
io->disconnect_callback = NULL;
mainloop_remove_fd(io->fd);
io_unref(io);
}
void bt_amp_unref(struct bt_amp *amp)
{
if (!amp)
return;
if (__sync_sub_and_fetch(&->ref_count, 1))
return;
mainloop_remove_fd(amp->vhci_fd);
close(amp->vhci_fd);
free(amp);
}
static void link_callback(int fd, uint32_t events, void *user_data)
{
struct bt_amp *amp = user_data;
if (events & (EPOLLERR | EPOLLHUP)) {
close(fd);
mainloop_remove_fd(fd);
evt_disconn_phy_link_complete(amp, 0x13);
amp->phy_mode = PHY_MODE_IDLE;
amp->phy_handle = 0x00;
return;
}
}
void bt_le_unref(struct bt_le *hci)
{
if (!hci)
return;
if (__sync_sub_and_fetch(&hci->ref_count, 1))
return;
bt_crypto_unref(hci->crypto);
mainloop_remove_fd(hci->vhci_fd);
close(hci->vhci_fd);
free(hci);
}
static void dev_read_destroy(void *user_data)
{
struct proxy *proxy = user_data;
printf("Closing device descriptor\n");
if (proxy->dev_shutdown)
shutdown(proxy->dev_fd, SHUT_RDWR);
close(proxy->dev_fd);
proxy->dev_fd = -1;
if (proxy->host_fd < 0) {
client_active = false;
free(proxy);
} else
mainloop_remove_fd(proxy->host_fd);
}
void gatt_server_stop(void)
{
if (att_fd < 0)
return;
mainloop_remove_fd(att_fd);
queue_destroy(conn_list, gatt_conn_destroy);
gatt_db_unref(gatt_cache);
gatt_cache = NULL;
gatt_db_unref(gatt_db);
gatt_db = NULL;
close(att_fd);
att_fd = -1;
}
static void att_conn_callback(int fd, uint32_t events, void *user_data)
{
struct gatt_conn *conn;
struct sockaddr_l2 addr;
socklen_t addrlen;
int new_fd;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
memset(&addr, 0, sizeof(addr));
addrlen = sizeof(addr);
new_fd = accept(att_fd, (struct sockaddr *) &addr, &addrlen);
if (new_fd < 0) {
fprintf(stderr, "Failed to accept new ATT connection: %m\n");
return;
}
conn = gatt_conn_new(new_fd);
if (!conn) {
fprintf(stderr, "Failed to create GATT connection\n");
close(new_fd);
return;
}
if (!queue_push_tail(conn_list, conn)) {
fprintf(stderr, "Failed to add GATT connection\n");
gatt_conn_destroy(conn);
close(new_fd);
}
printf("New device connected\n");
}
static void client_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data = user_data;
ssize_t len;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(data->fd);
return;
}
len = recv(data->fd, data->buf + data->offset,
sizeof(data->buf) - data->offset, MSG_DONTWAIT);
if (len < 0)
return;
data->offset += len;
if (data->offset > MGMT_HDR_SIZE) {
struct mgmt_hdr *hdr = (struct mgmt_hdr *) data->buf;
uint16_t pktlen = btohs(hdr->len);
if (data->offset > pktlen + MGMT_HDR_SIZE) {
uint16_t opcode = btohs(hdr->opcode);
uint16_t index = btohs(hdr->index);
packet_monitor(NULL, index, opcode,
data->buf + MGMT_HDR_SIZE, pktlen);
data->offset -= pktlen + MGMT_HDR_SIZE;
if (data->offset > 0)
memmove(data->buf, data->buf +
MGMT_HDR_SIZE + pktlen, data->offset);
}
}
}
static void attach_index_filter(int fd, uint16_t index)
{
struct sock_filter filters[] = {
/* Load MGMT index:
* A <- MGMT index
*/
BPF_STMT(BPF_LD + BPF_B + BPF_ABS,
offsetof(struct mgmt_hdr, index)),
/* Accept if index is HCI_DEV_NONE:
* A == HCI_DEV_NONE
*/
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, HCI_DEV_NONE, 0, 1),
/* return */
BPF_STMT(BPF_RET|BPF_K, 0x0fffffff), /* pass */
/* Accept if index match:
* A == index
*/
BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, index, 0, 1),
/* returns */
BPF_STMT(BPF_RET|BPF_K, 0x0fffffff), /* pass */
BPF_STMT(BPF_RET|BPF_K, 0), /* reject */
};
struct sock_fprog fprog = {
.len = sizeof(filters) / sizeof(filters[0]),
/* casting const away: */
.filter = filters,
};
setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog));
}
static int open_channel(uint16_t channel)
{
struct control_data *data;
data = malloc(sizeof(*data));
if (!data)
return -1;
memset(data, 0, sizeof(*data));
data->channel = channel;
data->fd = open_socket(channel);
if (data->fd < 0) {
free(data);
return -1;
}
if (filter_index != HCI_DEV_NONE)
attach_index_filter(data->fd, filter_index);
mainloop_add_fd(data->fd, EPOLLIN, data_callback, data, free_data);
return 0;
}
static void client_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data = user_data;
ssize_t len;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(data->fd);
return;
}
len = recv(data->fd, data->buf + data->offset,
sizeof(data->buf) - data->offset, MSG_DONTWAIT);
if (len < 0)
return;
data->offset += len;
while (data->offset >= MGMT_HDR_SIZE) {
struct mgmt_hdr *hdr = (struct mgmt_hdr *) data->buf;
uint16_t pktlen = le16_to_cpu(hdr->len);
uint16_t opcode, index;
if (data->offset < pktlen + MGMT_HDR_SIZE)
return;
opcode = le16_to_cpu(hdr->opcode);
index = le16_to_cpu(hdr->index);
packet_monitor(NULL, NULL, index, opcode,
data->buf + MGMT_HDR_SIZE, pktlen);
data->offset -= pktlen + MGMT_HDR_SIZE;
if (data->offset > 0)
memmove(data->buf, data->buf + MGMT_HDR_SIZE + pktlen,
data->offset);
}
}
static void server_accept_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data;
struct sockaddr_un addr;
socklen_t len;
int nfd;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
memset(&addr, 0, sizeof(addr));
len = sizeof(addr);
nfd = accept(fd, (struct sockaddr *) &addr, &len);
if (nfd < 0) {
perror("Failed to accept client socket");
return;
}
printf("--- New monitor connection ---\n");
data = malloc(sizeof(*data));
if (!data) {
close(nfd);
return;
}
memset(data, 0, sizeof(*data));
data->channel = HCI_CHANNEL_MONITOR;
data->fd = nfd;
mainloop_add_fd(data->fd, EPOLLIN, client_callback, data, free_data);
}
static int server_fd = -1;
void control_server(const char *path)
{
struct sockaddr_un addr;
size_t len;
int fd;
if (server_fd >= 0)
return;
len = strlen(path);
if (len > sizeof(addr.sun_path) - 1) {
fprintf(stderr, "Socket name too long\n");
return;
}
unlink(path);
fd = socket(PF_UNIX, SOCK_STREAM | SOCK_CLOEXEC, 0);
if (fd < 0) {
perror("Failed to open server socket");
return;
}
memset(&addr, 0, sizeof(addr));
addr.sun_family = AF_UNIX;
strncpy(addr.sun_path, path, len - 1);
if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("Failed to bind server socket");
close(fd);
return;
}
if (listen(fd, 5) < 0) {
perror("Failed to listen server socket");
close(fd);
return;
}
if (mainloop_add_fd(fd, EPOLLIN, server_accept_callback,
NULL, NULL) < 0) {
close(fd);
return;
}
server_fd = fd;
}
static bool parse_drops(uint8_t **data, uint8_t *len, uint8_t *drops,
uint32_t *total)
{
if (*len < 1)
return false;
*drops = **data;
*total += *drops;
(*data)++;
(*len)--;
return true;
}
static bool tty_parse_header(uint8_t *hdr, uint8_t len, struct timeval **tv,
struct timeval *ctv, uint32_t *drops)
{
uint8_t cmd = 0;
uint8_t evt = 0;
uint8_t acl_tx = 0;
uint8_t acl_rx = 0;
uint8_t sco_tx = 0;
uint8_t sco_rx = 0;
uint8_t other = 0;
uint32_t total = 0;
uint32_t ts32;
while (len) {
uint8_t type = hdr[0];
hdr++; len--;
switch (type) {
case TTY_EXTHDR_COMMAND_DROPS:
if (!parse_drops(&hdr, &len, &cmd, &total))
return false;
break;
case TTY_EXTHDR_EVENT_DROPS:
if (!parse_drops(&hdr, &len, &evt, &total))
return false;
break;
case TTY_EXTHDR_ACL_TX_DROPS:
if (!parse_drops(&hdr, &len, &acl_tx, &total))
return false;
break;
case TTY_EXTHDR_ACL_RX_DROPS:
if (!parse_drops(&hdr, &len, &acl_rx, &total))
return false;
break;
case TTY_EXTHDR_SCO_TX_DROPS:
if (!parse_drops(&hdr, &len, &sco_tx, &total))
return false;
break;
case TTY_EXTHDR_SCO_RX_DROPS:
if (!parse_drops(&hdr, &len, &sco_rx, &total))
return false;
break;
case TTY_EXTHDR_OTHER_DROPS:
if (!parse_drops(&hdr, &len, &other, &total))
return false;
break;
case TTY_EXTHDR_TS32:
if (len < sizeof(ts32))
return false;
ts32 = get_le32(hdr);
hdr += sizeof(ts32); len -= sizeof(ts32);
/* ts32 is in units of 1/10th of a millisecond */
ctv->tv_sec = ts32 / 10000;
ctv->tv_usec = (ts32 % 10000) * 100;
*tv = ctv;
break;
default:
printf("Unknown extended header type %u\n", type);
return false;
}
}
if (total) {
*drops += total;
printf("* Drops: cmd %u evt %u acl_tx %u acl_rx %u sco_tx %u "
"sco_rx %u other %u\n", cmd, evt, acl_tx, acl_rx,
sco_tx, sco_rx, other);
}
return true;
}
static void dev_read_callback(int fd, uint32_t events, void *user_data)
{
struct proxy *proxy = user_data;
struct bt_hci_evt_hdr *evt_hdr;
struct bt_hci_acl_hdr *acl_hdr;
struct bt_hci_sco_hdr *sco_hdr;
ssize_t len;
uint16_t pktlen;
if (events & (EPOLLERR | EPOLLHUP)) {
fprintf(stderr, "Error from device descriptor\n");
mainloop_remove_fd(proxy->dev_fd);
return;
}
if (events & EPOLLRDHUP) {
fprintf(stderr, "Remote hangup of device descriptor\n");
mainloop_remove_fd(proxy->host_fd);
return;
}
len = read(proxy->dev_fd, proxy->dev_buf + proxy->dev_len,
sizeof(proxy->dev_buf) - proxy->dev_len);
if (len < 0) {
if (errno == EAGAIN || errno == EINTR)
return;
fprintf(stderr, "Read from device descriptor failed\n");
mainloop_remove_fd(proxy->dev_fd);
return;
}
if (debug_enabled)
util_hexdump('>', proxy->dev_buf + proxy->dev_len, len,
hexdump_print, "D: ");
proxy->dev_len += len;
process_packet:
if (proxy->dev_len < 1)
return;
switch (proxy->dev_buf[0]) {
case BT_H4_EVT_PKT:
if (proxy->dev_len < 1 + sizeof(*evt_hdr))
return;
evt_hdr = (void *) (proxy->dev_buf + 1);
pktlen = 1 + sizeof(*evt_hdr) + evt_hdr->plen;
break;
case BT_H4_ACL_PKT:
if (proxy->dev_len < 1 + sizeof(*acl_hdr))
return;
acl_hdr = (void *) (proxy->dev_buf + 1);
pktlen = 1 + sizeof(*acl_hdr) + cpu_to_le16(acl_hdr->dlen);
break;
case BT_H4_SCO_PKT:
if (proxy->dev_len < 1 + sizeof(*sco_hdr))
return;
sco_hdr = (void *) (proxy->dev_buf + 1);
pktlen = 1 + sizeof(*sco_hdr) + sco_hdr->dlen;
break;
default:
fprintf(stderr, "Received unknown device packet type 0x%02x\n",
proxy->dev_buf[0]);
mainloop_remove_fd(proxy->dev_fd);
return;
}
if (proxy->dev_len < pktlen)
return;
if (!write_packet(proxy->host_fd, proxy->dev_buf, pktlen, "H: ")) {
fprintf(stderr, "Write to host descriptor failed\n");
mainloop_remove_fd(proxy->host_fd);
return;
}
if (proxy->dev_len > pktlen) {
memmove(proxy->dev_buf, proxy->dev_buf + pktlen,
proxy->dev_len - pktlen);
proxy->dev_len -= pktlen;
goto process_packet;
}
proxy->dev_len = 0;
}
static void data_callback(int fd, uint32_t events, void *user_data)
{
unsigned char control[32];
struct mgmt_hdr hdr;
struct msghdr msg;
struct iovec iov[2];
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(monitor_fd);
return;
}
iov[0].iov_base = &hdr;
iov[0].iov_len = MGMT_HDR_SIZE;
iov[1].iov_base = monitor_buf;
iov[1].iov_len = sizeof(monitor_buf);
memset(&msg, 0, sizeof(msg));
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
while (true) {
struct cmsghdr *cmsg;
struct timeval *tv = NULL;
struct timeval ctv;
uint16_t opcode, index, pktlen;
uint32_t flags;
ssize_t len;
len = recvmsg(monitor_fd, &msg, MSG_DONTWAIT);
if (len < 0)
break;
if (len < MGMT_HDR_SIZE)
break;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level != SOL_SOCKET)
continue;
if (cmsg->cmsg_type == SCM_TIMESTAMP) {
memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
tv = &ctv;
}
}
opcode = btohs(hdr.opcode);
index = btohs(hdr.index);
pktlen = btohs(hdr.len);
if (index)
continue;
flags = get_flags_from_opcode(opcode);
if (flags != 0xff)
btsnoop_write(snoop, tv, flags, monitor_buf, pktlen);
}
}
int mainloop_run(void)
{
unsigned int i;
if (signal_data) {
if (sigprocmask(SIG_BLOCK, &signal_data->mask, NULL) < 0)
return 1;
signal_data->fd = signalfd(-1, &signal_data->mask,
SFD_NONBLOCK | SFD_CLOEXEC);
if (signal_data->fd < 0)
return 1;
if (mainloop_add_fd(signal_data->fd, EPOLLIN,
signal_callback, signal_data, NULL) < 0) {
close(signal_data->fd);
return 1;
}
}
while (!epoll_terminate) {
struct epoll_event events[MAX_EPOLL_EVENTS];
int n, nfds;
nfds = epoll_wait(epoll_fd, events, MAX_EPOLL_EVENTS, -1);
if (nfds < 0)
continue;
for (n = 0; n < nfds; n++) {
struct mainloop_data *data = events[n].data.ptr;
data->callback(data->fd, events[n].events,
data->user_data);
}
}
if (signal_data) {
mainloop_remove_fd(signal_data->fd);
close(signal_data->fd);
if (signal_data->destroy)
signal_data->destroy(signal_data->user_data);
}
for (i = 0; i < MAX_MAINLOOP_ENTRIES; i++) {
struct mainloop_data *data = mainloop_list[i];
mainloop_list[i] = NULL;
if (data) {
epoll_ctl(epoll_fd, EPOLL_CTL_DEL, data->fd, NULL);
if (data->destroy)
data->destroy(data->user_data);
free(data);
}
}
close(epoll_fd);
epoll_fd = 0;
return 0;
}
static void stack_internal_callback(int fd, uint32_t events, void *user_data)
{
unsigned char buf[HCI_MAX_FRAME_SIZE];
unsigned char control[32];
struct msghdr msg;
struct iovec iov;
struct cmsghdr *cmsg;
ssize_t len;
hci_event_hdr *eh;
evt_stack_internal *si;
evt_si_device *sd;
struct timeval *tv = NULL;
struct timeval ctv;
uint8_t type = 0xff, bus = 0xff;
char str[18], name[8] = "";
bdaddr_t bdaddr;
bacpy(&bdaddr, BDADDR_ANY);
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
iov.iov_base = buf;
iov.iov_len = sizeof(buf);
memset(&msg, 0, sizeof(msg));
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
len = recvmsg(fd, &msg, MSG_DONTWAIT);
if (len < 0)
return;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level != SOL_HCI)
continue;
switch (cmsg->cmsg_type) {
case HCI_CMSG_TSTAMP:
memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
tv = &ctv;
break;
}
}
if (len < 1 + HCI_EVENT_HDR_SIZE + EVT_STACK_INTERNAL_SIZE +
EVT_SI_DEVICE_SIZE)
return;
if (buf[0] != HCI_EVENT_PKT)
return;
eh = (hci_event_hdr *) (buf + 1);
if (eh->evt != EVT_STACK_INTERNAL)
return;
si = (evt_stack_internal *) (buf + 1 + HCI_EVENT_HDR_SIZE);
if (si->type != EVT_SI_DEVICE)
return;
sd = (evt_si_device *) &si->data;
switch (sd->event) {
case HCI_DEV_REG:
device_info(fd, sd->dev_id, &type, &bus, &bdaddr, name);
ba2str(&bdaddr, str);
packet_new_index(tv, sd->dev_id, str, type, bus, name);
open_device(sd->dev_id);
break;
case HCI_DEV_UNREG:
ba2str(&bdaddr, str);
packet_del_index(tv, sd->dev_id, str);
break;
}
}
int mainloop_remove_timeout(int id)
{
return mainloop_remove_fd(id);
}
static void client_read_callback(int fd, uint32_t events, void *user_data)
{
struct client *client = user_data;
static uint8_t buf[4096];
uint8_t *ptr = buf;
ssize_t len;
uint16_t count;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(client->fd);
return;
}
again:
len = recv(fd, buf + client->pkt_offset,
sizeof(buf) - client->pkt_offset, MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN)
goto again;
return;
}
if (!client->btdev)
return;
count = client->pkt_offset + len;
while (count > 0) {
hci_command_hdr *cmd_hdr;
if (!client->pkt_data) {
client->pkt_type = ptr[0];
switch (client->pkt_type) {
case HCI_COMMAND_PKT:
if (count < HCI_COMMAND_HDR_SIZE + 1) {
client->pkt_offset += len;
return;
}
cmd_hdr = (hci_command_hdr *) (ptr + 1);
client->pkt_expect = HCI_COMMAND_HDR_SIZE +
cmd_hdr->plen + 1;
client->pkt_data = malloc(client->pkt_expect);
client->pkt_len = 0;
break;
default:
printf("packet error\n");
return;
}
client->pkt_offset = 0;
}
if (count >= client->pkt_expect) {
memcpy(client->pkt_data + client->pkt_len,
ptr, client->pkt_expect);
ptr += client->pkt_expect;
count -= client->pkt_expect;
btdev_receive_h4(client->btdev, client->pkt_data,
client->pkt_len + client->pkt_expect);
free(client->pkt_data);
client->pkt_data = NULL;
} else {
memcpy(client->pkt_data + client->pkt_len, ptr, count);
client->pkt_len += count;
client->pkt_expect -= count;
count = 0;
}
}
}
static void data_callback(int fd, uint32_t events, void *user_data)
{
struct control_data *data = user_data;
unsigned char control[64];
struct mgmt_hdr hdr;
struct msghdr msg;
struct iovec iov[2];
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(data->fd);
return;
}
iov[0].iov_base = &hdr;
iov[0].iov_len = MGMT_HDR_SIZE;
iov[1].iov_base = data->buf;
iov[1].iov_len = sizeof(data->buf);
memset(&msg, 0, sizeof(msg));
msg.msg_iov = iov;
msg.msg_iovlen = 2;
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
while (1) {
struct cmsghdr *cmsg;
struct timeval *tv = NULL;
struct timeval ctv;
struct ucred *cred = NULL;
struct ucred ccred;
uint16_t opcode, index, pktlen;
ssize_t len;
len = recvmsg(data->fd, &msg, MSG_DONTWAIT);
if (len < 0)
break;
if (len < MGMT_HDR_SIZE)
break;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level != SOL_SOCKET)
continue;
if (cmsg->cmsg_type == SCM_TIMESTAMP) {
memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
tv = &ctv;
}
if (cmsg->cmsg_type == SCM_CREDENTIALS) {
memcpy(&ccred, CMSG_DATA(cmsg), sizeof(ccred));
cred = &ccred;
}
}
opcode = le16_to_cpu(hdr.opcode);
index = le16_to_cpu(hdr.index);
pktlen = le16_to_cpu(hdr.len);
switch (data->channel) {
case HCI_CHANNEL_CONTROL:
packet_control(tv, cred, index, opcode,
data->buf, pktlen);
break;
case HCI_CHANNEL_MONITOR:
btsnoop_write_hci(btsnoop_file, tv, index, opcode, 0,
data->buf, pktlen);
ellisys_inject_hci(tv, index, opcode,
data->buf, pktlen);
packet_monitor(tv, cred, index, opcode,
data->buf, pktlen);
break;
}
}
}
static void device_callback(int fd, uint32_t events, void *user_data)
{
struct hcidump_data *data = user_data;
unsigned char buf[HCI_MAX_FRAME_SIZE * 2];
unsigned char control[64];
struct msghdr msg;
struct iovec iov;
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
iov.iov_base = buf;
iov.iov_len = sizeof(buf);
memset(&msg, 0, sizeof(msg));
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
while (1) {
struct cmsghdr *cmsg;
struct timeval *tv = NULL;
struct timeval ctv;
int dir = -1;
ssize_t len;
len = recvmsg(fd, &msg, MSG_DONTWAIT);
if (len < 0)
break;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level != SOL_HCI)
continue;
switch (cmsg->cmsg_type) {
case HCI_DATA_DIR:
memcpy(&dir, CMSG_DATA(cmsg), sizeof(dir));
break;
case HCI_CMSG_TSTAMP:
memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
tv = &ctv;
break;
}
}
if (dir < 0 || len < 1)
continue;
switch (buf[0]) {
case HCI_COMMAND_PKT:
packet_hci_command(tv, data->index, buf + 1, len - 1);
break;
case HCI_EVENT_PKT:
packet_hci_event(tv, data->index, buf + 1, len - 1);
break;
case HCI_ACLDATA_PKT:
packet_hci_acldata(tv, data->index, !!dir,
buf + 1, len - 1);
break;
case HCI_SCODATA_PKT:
packet_hci_scodata(tv, data->index, !!dir,
buf + 1, len - 1);
break;
}
}
}
