bool btsnoop_write_hci(struct btsnoop *btsnoop, struct timeval *tv, uint16_t index, uint16_t opcode, const void *data, uint16_t size) { uint32_t flags; if (!btsnoop) return false; switch (btsnoop->type) { case BTSNOOP_TYPE_HCI: if (btsnoop->index == 0xffff) btsnoop->index = index; if (index != btsnoop->index) return false; flags = get_flags_from_opcode(opcode); if (flags == 0xff) return false; break; case BTSNOOP_TYPE_MONITOR: flags = (index << 16) | opcode; break; default: return false; } return btsnoop_write(btsnoop, tv, flags, data, size); }
void packet_hci_acldata(struct timeval *tv, uint16_t index, bool in, const void *data, uint16_t size) { const hci_acl_hdr *hdr = data; uint16_t handle = btohs(hdr->handle); uint16_t dlen = btohs(hdr->dlen); uint8_t flags = acl_flags(handle); btsnoop_write(tv, index, in ? 0x01 : 0x00, data, size); print_header(tv, index); if (size < HCI_ACL_HDR_SIZE) { printf("* Malformed ACL Data %s packet\n", in ? "RX" : "TX"); return; } printf("%c ACL Data: handle %d flags 0x%2.2x dlen %d\n", in ? '>' : '<', acl_handle(handle), flags, dlen); data += HCI_ACL_HDR_SIZE; size -= HCI_ACL_HDR_SIZE; if (filter_mask & PACKET_FILTER_SHOW_ACL_DATA) packet_hexdump(data, size); }
void packet_hci_command(struct timeval *tv, uint16_t index, const void *data, uint16_t size) { const hci_command_hdr *hdr = data; uint16_t opcode = btohs(hdr->opcode); uint16_t ogf = cmd_opcode_ogf(opcode); uint16_t ocf = cmd_opcode_ocf(opcode); btsnoop_write(tv, index, 0x02, data, size); print_header(tv, index); if (size < HCI_COMMAND_HDR_SIZE) { printf("* Malformed HCI Command packet\n"); return; } printf("< HCI Command: %s (0x%2.2x|0x%4.4x) plen %d\n", opcode2str(opcode), ogf, ocf, hdr->plen); data += HCI_COMMAND_HDR_SIZE; size -= HCI_COMMAND_HDR_SIZE; packet_hexdump(data, size); }
static void btsnoop_write_packet(packet_type_t type, const uint8_t *packet, bool is_received) { int length_he = 0; int length; int flags; int drops = 0; switch (type) { case kCommandPacket: length_he = packet[2] + 4; flags = 2; break; case kAclPacket: length_he = (packet[3] << 8) + packet[2] + 5; flags = is_received; break; case kScoPacket: length_he = packet[2] + 4; flags = is_received; break; case kEventPacket: length_he = packet[1] + 3; flags = 3; break; } uint64_t timestamp = btsnoop_timestamp(); uint32_t time_hi = timestamp >> 32; uint32_t time_lo = timestamp & 0xFFFFFFFF; length = htonl(length_he); flags = htonl(flags); drops = htonl(drops); time_hi = htonl(time_hi); time_lo = htonl(time_lo); // This function is called from different contexts. utils_lock(); btsnoop_write(&length, 4); btsnoop_write(&length, 4); btsnoop_write(&flags, 4); btsnoop_write(&drops, 4); btsnoop_write(&time_hi, 4); btsnoop_write(&time_lo, 4); btsnoop_write(&type, 1); btsnoop_write(packet, length_he - 1); utils_unlock(); }
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_SIMULATOR: flags = (1 << 16) | frequency; break; default: return false; } return btsnoop_write(btsnoop, tv, flags, data, size); }
void packet_hci_event(struct timeval *tv, uint16_t index, const void *data, uint16_t size) { const hci_event_hdr *hdr = data; btsnoop_write(tv, index, 0x03, data, size); print_header(tv, index); if (size < HCI_EVENT_HDR_SIZE) { printf("* Malformed HCI Event packet\n"); return; } printf("> HCI Event: %s (0x%2.2x) plen %d\n", event2str(hdr->evt), hdr->evt, hdr->plen); data += HCI_EVENT_HDR_SIZE; size -= HCI_EVENT_HDR_SIZE; packet_hexdump(data, size); }
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); } }
static void data_callback(int fd, uint32_t events, void *user_data) { struct control_data *data = user_data; unsigned char control[32]; 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; 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; } } opcode = btohs(hdr.opcode); index = btohs(hdr.index); pktlen = btohs(hdr.len); switch (data->channel) { case HCI_CHANNEL_CONTROL: packet_control(tv, index, opcode, data->buf, pktlen); break; case HCI_CHANNEL_MONITOR: packet_monitor(tv, index, opcode, data->buf, pktlen); btsnoop_write(tv, index, opcode, data->buf, pktlen); break; } } }