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