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 );
	}
}
Exemplo n.º 2
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);
	}
}
Exemplo n.º 4
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);
 }
Exemplo n.º 5
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);
	}
}