static void ping(char *svr) { struct sigaction sa; struct sockaddr_l2 addr; socklen_t optlen; unsigned char *send_buf; unsigned char *recv_buf; char str[18]; int i, sk, lost; uint8_t id; memset(&sa, 0, sizeof(sa)); sa.sa_handler = stat; sigaction(SIGINT, &sa, NULL); send_buf = malloc(L2CAP_CMD_HDR_SIZE + size); recv_buf = malloc(L2CAP_CMD_HDR_SIZE + size); if (!send_buf || !recv_buf) { perror("Can't allocate buffer"); exit(1); } /* Create socket */ sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP); if (sk < 0) { perror("Can't create socket"); goto error; } /* Bind to local address */ memset(&addr, 0, sizeof(addr)); addr.l2_family = AF_BLUETOOTH; bacpy(&addr.l2_bdaddr, &bdaddr); if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { perror("Can't bind socket"); goto error; } /* Connect to remote device */ memset(&addr, 0, sizeof(addr)); addr.l2_family = AF_BLUETOOTH; str2ba(svr, &addr.l2_bdaddr); if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) { perror("Can't connect"); goto error; } /* Get local address */ memset(&addr, 0, sizeof(addr)); optlen = sizeof(addr); if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) { perror("Can't get local address"); goto error; } ba2str(&addr.l2_bdaddr, str); printf("Ping: %s from %s (data size %d) ...\n", svr, str, size); /* Initialize send buffer */ for (i = 0; i < size; i++) send_buf[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A'; id = ident; while (count == -1 || count-- > 0) { struct timeval tv_send, tv_recv, tv_diff; l2cap_cmd_hdr *send_cmd = (l2cap_cmd_hdr *) send_buf; l2cap_cmd_hdr *recv_cmd = (l2cap_cmd_hdr *) recv_buf; /* Build command header */ send_cmd->ident = id; send_cmd->len = htobs(size); if (reverse) send_cmd->code = L2CAP_ECHO_RSP; else send_cmd->code = L2CAP_ECHO_REQ; gettimeofday(&tv_send, NULL); /* Send Echo Command */ if (send(sk, send_buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) { perror("Send failed"); goto error; } /* Wait for Echo Response */ lost = 0; while (1) { struct pollfd pf[1]; int err; pf[0].fd = sk; pf[0].events = POLLIN; if ((err = poll(pf, 1, timeout * 1000)) < 0) { perror("Poll failed"); goto error; } if (!err) { lost = 1; break; } if ((err = recv(sk, recv_buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) { perror("Recv failed"); goto error; } if (!err){ printf("Disconnected\n"); goto error; } recv_cmd->len = btohs(recv_cmd->len); /* Check for our id */ if (recv_cmd->ident != id) continue; /* Check type */ if (!reverse && recv_cmd->code == L2CAP_ECHO_RSP) break; if (recv_cmd->code == L2CAP_COMMAND_REJ) { printf("Peer doesn't support Echo packets\n"); goto error; } } sent_pkt++; if (!lost) { recv_pkt++; gettimeofday(&tv_recv, NULL); timersub(&tv_recv, &tv_send, &tv_diff); if (verify) { /* Check payload length */ if (recv_cmd->len != size) { fprintf(stderr, "Received %d bytes, expected %d\n", recv_cmd->len, size); goto error; } /* Check payload */ if (memcmp(&send_buf[L2CAP_CMD_HDR_SIZE], &recv_buf[L2CAP_CMD_HDR_SIZE], size)) { fprintf(stderr, "Response payload different.\n"); goto error; } } printf("%d bytes from %s id %d time %.2fms\n", recv_cmd->len, svr, id - ident, tv2fl(tv_diff)); if (delay) sleep(delay); } else { printf("no response from %s: id %d\n", svr, id - ident); } if (++id > 254) id = ident; } stat(0); free(send_buf); free(recv_buf); return; error: close(sk); free(send_buf); free(recv_buf); exit(1); }
static void read_dump(int fd) { struct hcidump_hdr dh; struct btsnoop_pkt dp; struct pktlog_hdr ph; struct frame frm; int err; frm.data = malloc(HCI_MAX_FRAME_SIZE); if (!frm.data) { perror("Can't allocate data buffer"); exit(1); } while (1) { if (parser.flags & DUMP_PKTLOG) err = read_n(fd, (void *) &ph, PKTLOG_HDR_SIZE); else if (parser.flags & DUMP_BTSNOOP) err = read_n(fd, (void *) &dp, BTSNOOP_PKT_SIZE); else err = read_n(fd, (void *) &dh, HCIDUMP_HDR_SIZE); if (err < 0) goto failed; if (!err) return; if (parser.flags & DUMP_PKTLOG) { switch (ph.type) { case 0x00: ((uint8_t *) frm.data)[0] = HCI_COMMAND_PKT; frm.in = 0; break; case 0x01: ((uint8_t *) frm.data)[0] = HCI_EVENT_PKT; frm.in = 1; break; case 0x02: ((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT; frm.in = 0; break; case 0x03: ((uint8_t *) frm.data)[0] = HCI_ACLDATA_PKT; frm.in = 1; break; default: lseek(fd, ntohl(ph.len) - 9, SEEK_CUR); continue; } frm.data_len = ntohl(ph.len) - 8; err = read_n(fd, frm.data + 1, frm.data_len - 1); } else if (parser.flags & DUMP_BTSNOOP) { uint32_t opcode; uint8_t pkt_type; switch (btsnoop_type) { case 1001: if (ntohl(dp.flags) & 0x02) { if (ntohl(dp.flags) & 0x01) pkt_type = HCI_EVENT_PKT; else pkt_type = HCI_COMMAND_PKT; } else pkt_type = HCI_ACLDATA_PKT; ((uint8_t *) frm.data)[0] = pkt_type; frm.data_len = ntohl(dp.len) + 1; err = read_n(fd, frm.data + 1, frm.data_len - 1); break; case 1002: frm.data_len = ntohl(dp.len); err = read_n(fd, frm.data, frm.data_len); break; case 2001: opcode = ntohl(dp.flags) & 0xffff; switch (opcode) { case 2: pkt_type = HCI_COMMAND_PKT; frm.in = 0; break; case 3: pkt_type = HCI_EVENT_PKT; frm.in = 1; break; case 4: pkt_type = HCI_ACLDATA_PKT; frm.in = 0; break; case 5: pkt_type = HCI_ACLDATA_PKT; frm.in = 1; break; case 6: pkt_type = HCI_SCODATA_PKT; frm.in = 0; break; case 7: pkt_type = HCI_SCODATA_PKT; frm.in = 1; break; default: pkt_type = 0xff; break; } ((uint8_t *) frm.data)[0] = pkt_type; frm.data_len = ntohl(dp.len) + 1; err = read_n(fd, frm.data + 1, frm.data_len - 1); } } else { frm.data_len = btohs(dh.len); err = read_n(fd, frm.data, frm.data_len); } if (err < 0) goto failed; if (!err) return; frm.ptr = frm.data; frm.len = frm.data_len; if (parser.flags & DUMP_PKTLOG) { uint64_t ts; ts = ntoh64(ph.ts); frm.ts.tv_sec = ts >> 32; frm.ts.tv_usec = ts & 0xffffffff; } else if (parser.flags & DUMP_BTSNOOP) {
static void print_sockaddr(struct tcb *tcp, const sockaddr_buf_t *addr, const int addrlen) { tprints("{sa_family="); printxval(addrfams, addr->sa.sa_family, "AF_???"); tprints(", "); switch (addr->sa.sa_family) { case AF_UNIX: if (addrlen == 2) { tprints("NULL"); } else if (addr->sau.sun_path[0]) { tprints("sun_path="); print_quoted_string(addr->sau.sun_path, sizeof(addr->sau.sun_path) + 1, QUOTE_0_TERMINATED); } else { tprints("sun_path=@"); print_quoted_string(addr->sau.sun_path + 1, sizeof(addr->sau.sun_path), QUOTE_0_TERMINATED); } break; case AF_INET: tprintf("sin_port=htons(%u), sin_addr=inet_addr(\"%s\")", ntohs(addr->sin.sin_port), inet_ntoa(addr->sin.sin_addr)); break; #ifdef HAVE_INET_NTOP case AF_INET6: { char string_addr[100]; inet_ntop(AF_INET6, &addr->sa6.sin6_addr, string_addr, sizeof(string_addr)); tprintf("sin6_port=htons(%u), inet_pton(AF_INET6" ", \"%s\", &sin6_addr), sin6_flowinfo=%u", ntohs(addr->sa6.sin6_port), string_addr, addr->sa6.sin6_flowinfo); # ifdef HAVE_STRUCT_SOCKADDR_IN6_SIN6_SCOPE_ID tprints(", sin6_scope_id="); # if defined IN6_IS_ADDR_LINKLOCAL && defined IN6_IS_ADDR_MC_LINKLOCAL if (IN6_IS_ADDR_LINKLOCAL(&addr->sa6.sin6_addr) || IN6_IS_ADDR_MC_LINKLOCAL(&addr->sa6.sin6_addr)) print_ifindex(addr->sa6.sin6_scope_id); else # endif tprintf("%u", addr->sa6.sin6_scope_id); # endif /* HAVE_STRUCT_SOCKADDR_IN6_SIN6_SCOPE_ID */ } break; #endif #if defined(AF_IPX) case AF_IPX: { int i; tprintf("sipx_port=htons(%u), ", ntohs(addr->sipx.sipx_port)); /* Yes, I know, this does not look too * strace-ish, but otherwise the IPX * addresses just look monstrous... * Anyways, feel free if you don't like * this way.. :) */ tprintf("%08lx:", (unsigned long)ntohl(addr->sipx.sipx_network)); for (i = 0; i < IPX_NODE_LEN; i++) tprintf("%02x", addr->sipx.sipx_node[i]); tprintf("/[%02x]", addr->sipx.sipx_type); } break; #endif /* AF_IPX */ #ifdef AF_PACKET case AF_PACKET: { int i; tprintf("proto=%#04x, if%d, pkttype=", ntohs(addr->ll.sll_protocol), addr->ll.sll_ifindex); printxval(af_packet_types, addr->ll.sll_pkttype, "PACKET_???"); tprintf(", addr(%d)={%d, ", addr->ll.sll_halen, addr->ll.sll_hatype); for (i = 0; i < addr->ll.sll_halen; i++) tprintf("%02x", addr->ll.sll_addr[i]); } break; #endif /* AF_PACKET */ #ifdef AF_NETLINK case AF_NETLINK: tprintf("pid=%d, groups=%08x", addr->nl.nl_pid, addr->nl.nl_groups); break; #endif /* AF_NETLINK */ #if defined(AF_BLUETOOTH) && defined(HAVE_BLUETOOTH_BLUETOOTH_H) case AF_BLUETOOTH: tprintf("{sco_bdaddr=%02X:%02X:%02X:%02X:%02X:%02X} or " "{rc_bdaddr=%02X:%02X:%02X:%02X:%02X:%02X, rc_channel=%d} or " "{l2_psm=htobs(%d), l2_bdaddr=%02X:%02X:%02X:%02X:%02X:%02X, l2_cid=htobs(%d)} or " "{hci_dev=htobs(%d)}", addr->sco.sco_bdaddr.b[0], addr->sco.sco_bdaddr.b[1], addr->sco.sco_bdaddr.b[2], addr->sco.sco_bdaddr.b[3], addr->sco.sco_bdaddr.b[4], addr->sco.sco_bdaddr.b[5], addr->rc.rc_bdaddr.b[0], addr->rc.rc_bdaddr.b[1], addr->rc.rc_bdaddr.b[2], addr->rc.rc_bdaddr.b[3], addr->rc.rc_bdaddr.b[4], addr->rc.rc_bdaddr.b[5], addr->rc.rc_channel, btohs(addr->l2.l2_psm), addr->l2.l2_bdaddr.b[0], addr->l2.l2_bdaddr.b[1], addr->l2.l2_bdaddr.b[2], addr->l2.l2_bdaddr.b[3], addr->l2.l2_bdaddr.b[4], addr->l2.l2_bdaddr.b[5], btohs(addr->l2.l2_cid), btohs(addr->hci.hci_dev)); break; #endif /* AF_BLUETOOTH && HAVE_BLUETOOTH_BLUETOOTH_H */ /* AF_AX25 AF_APPLETALK AF_NETROM AF_BRIDGE AF_AAL5 AF_X25 AF_ROSE etc. still need to be done */ default: tprints("sa_data="); print_quoted_string(addr->sa.sa_data, sizeof(addr->sa.sa_data), 0); break; } tprints("}"); }