static void recv_mode(int sk)
{
struct timeval tv_beg,tv_end,tv_diff;
long total;
syslog(LOG_INFO, "Receiving ...");
while (1) {
gettimeofday(&tv_beg, NULL);
total = 0;
while (total < data_size) {
int r;
if ((r = recv(sk, buf, data_size, 0)) <= 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
total += r;
}
gettimeofday(&tv_end, NULL);
timersub(&tv_end, &tv_beg, &tv_diff);
syslog(LOG_INFO,"%ld bytes in %.2fm speed %.2f kb", total,
tv2fl(tv_diff) / 60.0,
(float)( total / tv2fl(tv_diff) ) / 1024.0 );
}
}
int main() {
int btsock;
struct sockaddr_l2 addr;
unsigned char *send_buf;
unsigned char *recv_buf;
socklen_t optlen;
uint8_t id;
int i;
int lost;
char * svr = "58:3F:54:7A:F8:5E";
char str[18];
/* Default options */
bacpy(&bdaddr, BDADDR_ANY);
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 */
btsock = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
if (btsock < 0) {
perror("Can't create socket");
return -1;
}
set_l2cap_mtu(btsock, 1000);
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.l2_family = AF_BLUETOOTH;
bacpy(&addr.l2_bdaddr, &bdaddr);
if (bind(btsock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("Can't bind socket");
return -1;
}
/* Connect to remote device */
memset(&addr, 0, sizeof(addr));
addr.l2_family = AF_BLUETOOTH;
str2ba(svr, &addr.l2_bdaddr);
if (connect(btsock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("Can't connect");
return -1;
}/* Get local address */
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
if (getsockname(btsock, (struct sockaddr *) &addr, &optlen) < 0) {
perror("Can't get local address");
return -1;
}
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(btsock, send_buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) {
perror("Send failed");
return -1;
}
/* Wait for Echo Response */
lost = 0;
while (1) {
struct pollfd pf[1];
int err;
pf[0].fd = btsock;
pf[0].events = POLLIN;
if ((err = poll(pf, 1, timeout * 1000)) < 0) {
perror("Poll failed");
return -1;
}
if (!err) {
lost = 1;
break;
}
if ((err = recv(btsock, recv_buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) {
perror("Recv failed");
return -1;
}
if (!err){
printf("Disconnected\n");
return -1;
}
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");
return -1;
}
}
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);
return -1;
}
/* Check payload */
if (memcmp(&send_buf[L2CAP_CMD_HDR_SIZE],
&recv_buf[L2CAP_CMD_HDR_SIZE], size)) {
fprintf(stderr, "Response payload different.\n");
return -1;
}
}
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;
}
free(send_buf);
free(recv_buf);
return 0;
}
static void recv_mode(int sk)
{
struct timeval tv_beg, tv_end, tv_diff;
char ts[30];
long total;
syslog(LOG_INFO, "Receiving ...");
memset(ts, 0, sizeof(ts));
while (1) {
gettimeofday(&tv_beg,NULL);
total = 0;
while (total < data_size) {
//uint32_t sq;
//uint16_t l;
int r;
if ((r = recv(sk, buf, data_size, 0)) < 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
if (timestamp) {
struct timeval tv;
if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
timestamp = 0;
memset(ts, 0, sizeof(ts));
} else {
sprintf(ts, "[%ld.%ld] ",
tv.tv_sec, tv.tv_usec);
}
}
#if 0
/* Check sequence */
sq = btohl(*(uint32_t *) buf);
if (seq != sq) {
syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
seq = sq;
}
seq++;
/* Check length */
l = btohs(*(uint16_t *) (buf + 4));
if (r != l) {
syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
continue;
}
/* Verify data */
for (i = 6; i < r; i++) {
if (buf[i] != 0x7f)
syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
}
#endif
total += r;
}
gettimeofday(&tv_end,NULL);
timersub(&tv_end,&tv_beg,&tv_diff);
syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
}
}
// --------------------------------------------------------------------
// function : l2ping
// takes : bluetooth address, debug mode, continue on errors
// role : perform an l2ping
// returns : 1 if host is up, 0 if down, can cause code to exist as well
// --------------------------------------------------------------------
int l2ping(char *svr, int debug, int cont)
{
struct sockaddr_l2 addr;
struct sigaction sa;
char buf[2048];
char str[18];
int s, i, opt, lost;
uint8_t id;
memset(&sa, 0, sizeof(sa));
// calls the pretty stat handler when we exit
sa.sa_handler = stat;
sigaction(SIGINT, &sa, NULL);
if (debug){
fprintf(stdout,"\n[d] l2ping being called with address %s\n", svr);
}
if ((s = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP)) < 0) {
perror("[!] l2ping: Can't create socket.");
exit(1);
}
memset(&addr, 0, sizeof(addr));
addr.l2_family = AF_BLUETOOTH;
addr.l2_bdaddr = bdaddr;
if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("[!] l2ping: Can't bind socket.");
exit(1);
}
str2ba(svr, &addr.l2_bdaddr);
if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
perror("[!] l2ping: Can't connect.");
exit(1);
}
/* Get local address */
opt = sizeof(addr);
if (getsockname(s, (struct sockaddr *)&addr, &opt) < 0) {
perror("[!] l2ping: Can't get local address.");
exit(1);
}
ba2str(&addr.l2_bdaddr, str);
if (debug) printf("[*] l2ping ping: %s from %s (data size %d) ...\n", svr, str, size);
/* Initialize buffer */
for (i = L2CAP_CMD_HDR_SIZE; i < sizeof(buf); i++)
buf[i]=(i%40)+'A';
id = ident;
while( count == -1 || count-- > 0 ){
struct timeval tv_send, tv_recv, tv_diff;
l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf;
/* Build command header */
cmd->code = L2CAP_ECHO_REQ;
cmd->ident = id;
cmd->len = __cpu_to_le16(size);
gettimeofday(&tv_send, NULL);
/* Send Echo Request */
if (send(s, buf, size + L2CAP_CMD_HDR_SIZE, 0) <= 0) {
perror("[!] l2ping: send failed");
close(s);
exit(1);
}
/* Wait for Echo Response */
lost = 0;
while (1) {
struct pollfd pf[1];
register int err;
pf[0].fd = s; pf[0].events = POLLIN;
if ((err = poll(pf, 1, 10*1000)) < 0) {
if (!debug) fprintf(stdout,"\n");
perror("[!] l2ping: poll failed");
close(s);
if (cont) return 1;
exit(1);
}
if (!err) {
lost = 1;
break;
}
if ((err = recv(s, buf, sizeof(buf), 0)) < 0) {
if (!debug) fprintf(stdout,"\n");
perror("[!] l2ping: Recv failed");
if (cont) {
if (debug) fprintf(stderr,"[d] l2ping: continue anyway\n");
close(s);
return 1;
}
// exit(1);
// it is up it just reset the connection
close(s);
return 1;
}
if (!err){
if (!debug) fprintf(stdout,"\n");
printf("[!] l2ping: disconnected\n");
close(s);
if (cont) return 1;
exit(1);
}
cmd->len = __le16_to_cpu(cmd->len);
/* Check for our id */
if( cmd->ident != id )
continue;
/* Check type */
if (cmd->code == L2CAP_ECHO_RSP)
break;
if (cmd->code == L2CAP_COMMAND_REJ) {
if (!debug) fprintf(stdout,"\n");
fprintf(stdout,"[!] l2ping: peer doesn't support Echo packets\n");
close(s);
exit(1);
}
}
sent_pkt++;
close(s);
if (!lost) {
recv_pkt++;
gettimeofday(&tv_recv, NULL);
timersub(&tv_recv, &tv_send, &tv_diff);
if (debug) fprintf(stdout,"[*] l2ping: %d bytes from %s id %d time %.2fms\n", cmd->len, svr, id, tv2fl(tv_diff));
return 1;
if (delay) sleep(delay);
} else {
if (debug) fprintf(stdout,"[!] l2ping: no response from %s: id %d\n", svr, id);
return 0;
}
if (++id > 254) id = ident;
}
//stat(0);
}
static int do_connect(const char *svr)
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, opt;
if (uuid != 0x0000)
channel = get_channel(svr, uuid);
if (channel == 0) {
syslog(LOG_ERR, "Can't get channel number");
return -1;
}
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return -1;
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
goto error;
}
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Connect to remote device */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
str2ba(svr, &addr.rc_bdaddr);
addr.rc_channel = channel;
if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't connect: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//goto error;
}
syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
return sk;
error:
close(sk);
return -1;
}
static void do_listen(void (*handler)(int sk))
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, nsk, opt;
char ba[18];
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
exit(1);
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
addr.rc_channel = channel;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encrypt)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Enable deferred setup */
opt = defer_setup;
if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
&opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
strerror(errno), errno);
goto error;
}
/* Listen for connections */
if (listen(sk, 10)) {
syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Check for socket address */
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket name: %s (%d)",
strerror(errno), errno);
goto error;
}
channel = addr.rc_channel;
syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
while (1) {
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
if (nsk < 0) {
syslog(LOG_ERR,"Accept failed: %s (%d)",
strerror(errno), errno);
goto error;
}
if (fork()) {
/* Parent */
close(nsk);
continue;
}
/* Child */
close(sk);
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//close(nsk);
//goto error;
}
ba2str(&addr.rc_bdaddr, ba);
syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
ba, conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
}
/* Handle deferred setup */
if (defer_setup) {
syslog(LOG_INFO, "Waiting for %d seconds",
abs(defer_setup) - 1);
sleep(abs(defer_setup) - 1);
if (defer_setup < 0) {
close(nsk);
goto error;
}
}
handler(nsk);
syslog(LOG_INFO, "Disconnect: %m");
exit(0);
}
return;
error:
close(sk);
exit(1);
}
static void dump_mode(int sk)
{
int len;
syslog(LOG_INFO, "Receiving ...");
while ((len = read(sk, buf, data_size)) > 0)
syslog(LOG_INFO, "Recevied %d bytes", len);
}
static void recv_mode(int sk)
{
struct timeval tv_beg, tv_end, tv_diff;
char ts[30];
long total;
uint32_t seq;
syslog(LOG_INFO, "Receiving ...");
memset(ts, 0, sizeof(ts));
seq = 0;
while (1) {
gettimeofday(&tv_beg,NULL);
total = 0;
while (total < data_size) {
//uint32_t sq;
//uint16_t l;
int r;
if ((r = recv(sk, buf, data_size, 0)) < 0) {
if (r < 0)
syslog(LOG_ERR, "Read failed: %s (%d)",
strerror(errno), errno);
return;
}
if (timestamp) {
struct timeval tv;
if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
timestamp = 0;
memset(ts, 0, sizeof(ts));
} else {
sprintf(ts, "[%ld.%ld] ",
tv.tv_sec, tv.tv_usec);
}
}
#if 0
/* Check sequence */
sq = btohl(*(uint32_t *) buf);
if (seq != sq) {
syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
seq = sq;
}
seq++;
/* Check length */
l = btohs(*(uint16_t *) (buf + 4));
if (r != l) {
syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
continue;
}
/* Verify data */
for (i = 6; i < r; i++) {
if (buf[i] != 0x7f)
syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
}
#endif
total += r;
}
gettimeofday(&tv_end,NULL);
timersub(&tv_end,&tv_beg,&tv_diff);
syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
}
}
static void do_send(int sk)
{
uint32_t seq;
int i, fd, len;
syslog(LOG_INFO,"Sending ...");
if (filename) {
fd = open(filename, O_RDONLY);
if (fd < 0) {
syslog(LOG_ERR, "Open failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
len = read(fd, buf, data_size);
send(sk, buf, len, 0);
return;
} else {
for (i = 6; i < data_size; i++)
buf[i] = 0x7f;
}
seq = 0;
while ((num_frames == -1) || (num_frames-- > 0)) {
*(uint32_t *) buf = htobl(seq);
*(uint16_t *) (buf + 4) = htobs(data_size);
seq++;
if (send(sk, buf, data_size, 0) <= 0) {
syslog(LOG_ERR, "Send failed: %s (%d)",
strerror(errno), errno);
exit(1);
}
if (num_frames && delay && count && !(seq % count))
usleep(delay);
}
}
