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; } } }