/* * *  BlueZ - Bluetooth protocol stack for Linux * *  Copyright (C) 2000-2001  Qualcomm Incorporated *  Copyright (C) 2002-2003  Maxim Krasnyansky <maxk@qualcomm.com> *  Copyright (C) 2002-2008  Marcel Holtmann <marcel@holtmann.org> * * *  This program is free software; you can redistribute it and/or modify *  it under the terms of the GNU General Public License as published by *  the Free Software Foundation; either version 2 of the License, or *  (at your option) any later version. * *  This program is distributed in the hope that it will be useful, *  but WITHOUT ANY WARRANTY; without even the implied warranty of *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the *  GNU General Public License for more details. * *  You should have received a copy of the GNU General Public License *  along with this program; if not, write to the Free Software *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA * */#ifdef HAVE_CONFIG_H#include <config.h>#endif#include <stdio.h>#include <errno.h>#include <ctype.h>#include <fcntl.h>#include <unistd.h>#include <stdlib.h>#include <getopt.h>#include <syslog.h>#include <signal.h>#include <sys/time.h>#include <sys/poll.h>#include <sys/socket.h>#include <bluetooth/bluetooth.h>#include <bluetooth/hci.h>#include <bluetooth/hci_lib.h>#include <bluetooth/l2cap.h>#define NIBBLE_TO_ASCII(c)  ((c) < 0x0a ? (c) + 0x30 : (c) + 0x57)/* Test modes */enum {	SEND,	RECV,	RECONNECT,	MULTY,	DUMP,	CONNECT,	CRECV,	LSEND,	SENDDUMP,	LSENDDUMP,	INFOREQ};static unsigned char *buf;/* Default mtu */static int imtu = 672;static int omtu = 0;/* Default data size */static long data_size = -1;/* Default addr and psm */static bdaddr_t bdaddr;static unsigned short psm = 10;/* Default number of frames to send (-1 = infinite) */static int num_frames = -1;/* Default number of consecutive frames before the delay */static int count = 1;/* Default delay after sending count number of frames */static unsigned long delay = 0;static char *filename = NULL;static int flowctl = 0;static int master = 0;static int auth = 0;static int encrypt = 0;static int secure = 0;static int socktype = SOCK_SEQPACKET;static int linger = 0;static int reliable = 0;static float tv2fl(struct timeval tv){	return (float)tv.tv_sec + (float)(tv.tv_usec/1000000.0);}static char *ltoh(unsigned long c, char* s){	int c1;	c1     = (c >> 28) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >> 24) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >> 20) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >> 16) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >> 12) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >>  8) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = (c >>  4) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = c & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	*s     = 0;	return s;}static char *ctoh(char c, char* s){	char c1;	c1     = (c >> 4) & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	c1     = c & 0x0f;	*(s++) = NIBBLE_TO_ASCII (c1);	*s     = 0;	return s;}static void hexdump(unsigned char *s, unsigned long l){	char bfr[80];	char *pb;	unsigned long i, n = 0;	if (l == 0)		return;	while (n < l) {		pb = bfr;		pb = ltoh (n, pb);		*(pb++) = ':';		*(pb++) = ' ';		for (i = 0; i < 16; i++) {			if (n + i >= l) {				*(pb++) = ' ';				*(pb++) = ' ';			} else				pb = ctoh (*(s + i), pb);			*(pb++) = ' ';		}		*(pb++) = ' ';		for (i = 0; i < 16; i++) {			if (n + i >= l)				break;			else				*(pb++) = (isprint (*(s + i)) ? *(s + i) : '.');		}		*pb = 0;		n += 16;		s += 16;		puts(bfr);	}}static int do_connect(char *svr){	struct sockaddr_l2 addr;	struct l2cap_options opts;	struct l2cap_conninfo conn;	socklen_t optlen;	int sk, opt;	/* Create socket */	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP);	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.l2_family = AF_BLUETOOTH;	bacpy(&addr.l2_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;	}	/* Get default options */	memset(&opts, 0, sizeof(opts));	optlen = sizeof(opts);	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {		syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Set new options */	opts.omtu = omtu;	opts.imtu = imtu;	if (flowctl)		opts.mode = 2;	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) {		syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)",							strerror(errno), errno);		goto error;	}	/* 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);			return -1;		}	}	/* Set link mode */	opt = 0;	if (reliable)		opt |= L2CAP_LM_RELIABLE;	if (setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {		syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Connect to remote device */	memset(&addr, 0, sizeof(addr));	addr.l2_family = AF_BLUETOOTH;	str2ba(svr, &addr.l2_bdaddr);	addr.l2_psm = htobs(psm);	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0 ) {		syslog(LOG_ERR, "Can't connect: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Get current options */	memset(&opts, 0, sizeof(opts));	optlen = sizeof(opts);	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {		syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Get connection information */	memset(&conn, 0, sizeof(conn));	optlen = sizeof(conn);	if (getsockopt(sk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) {		syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)",							strerror(errno), errno);		goto error;	}	syslog(LOG_INFO, "Connected [imtu %d, omtu %d, flush_to %d, "				"mode %d, handle %d, class 0x%02x%02x%02x]",		opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle,		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);	if (data_size > opts.omtu)		data_size = opts.omtu;	return sk;error:	close(sk);	return -1;}static void do_listen(void (*handler)(int sk)){	struct sockaddr_l2 addr;	struct l2cap_options opts;	struct l2cap_conninfo conn;	socklen_t optlen;	int sk, nsk, opt;	char ba[18];	/* Create socket */	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_L2CAP);	if (sk < 0) {		syslog(LOG_ERR, "Can't create socket: %s (%d)",							strerror(errno), errno);		exit(1);	}	/* Bind to local address */	addr.l2_family = AF_BLUETOOTH;	bacpy(&addr.l2_bdaddr, &bdaddr);	addr.l2_psm = htobs(psm);	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 (reliable)		opt |= L2CAP_LM_RELIABLE;	if (master)		opt |= L2CAP_LM_MASTER;	if (auth)		opt |= L2CAP_LM_AUTH;	if (encrypt)		opt |= L2CAP_LM_ENCRYPT;	if (secure)		opt |= L2CAP_LM_SECURE;	if (opt && setsockopt(sk, SOL_L2CAP, L2CAP_LM, &opt, sizeof(opt)) < 0) {		syslog(LOG_ERR, "Can't set L2CAP link mode: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Get default options */	memset(&opts, 0, sizeof(opts));	optlen = sizeof(opts);	if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {		syslog(LOG_ERR, "Can't get default L2CAP options: %s (%d)",							strerror(errno), errno);		goto error;	}	/* Set new options */	opts.omtu = omtu;	opts.imtu = imtu;	if (flowctl)		opts.mode = 2;	if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts)) < 0) {		syslog(LOG_ERR, "Can't set L2CAP options: %s (%d)",							strerror(errno), errno);		goto error;	}	if (socktype == SOCK_DGRAM) {		handler(sk);		return;	}	/* 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;	}	psm = btohs(addr.l2_psm);	syslog(LOG_INFO, "Waiting for connection on psm %d ...", psm);	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 current options */		memset(&opts, 0, sizeof(opts));		optlen = sizeof(opts);		if (getsockopt(nsk, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {			syslog(LOG_ERR, "Can't get L2CAP options: %s (%d)",							strerror(errno), errno);			close(nsk);			goto error;		}		/* Get connection information */		memset(&conn, 0, sizeof(conn));		optlen = sizeof(conn);		if (getsockopt(nsk, SOL_L2CAP, L2CAP_CONNINFO, &conn, &optlen) < 0) {			syslog(LOG_ERR, "Can't get L2CAP connection information: %s (%d)",							strerror(errno), errno);			close(nsk);			goto error;		}		ba2str(&addr.l2_bdaddr, ba);		syslog(LOG_INFO, "Connect from %s [imtu %d, omtu %d, flush_to %d, "					"mode %d, handle %d, class 0x%02x%02x%02x]",			ba, opts.imtu, opts.omtu, opts.flush_to, opts.mode, conn.hci_handle,			conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);		/* 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;			}		}		handler(nsk);		syslog(LOG_INFO, "Disconnect: %m");		exit(0);	}	return;error:	close(sk);	exit(1);}static void dump_mode(int sk){	socklen_t optlen;	int opt, len;	syslog(LOG_INFO, "Receiving ...");	while (1) {		fd_set rset;		FD_ZERO(&rset);		FD_SET(sk, &rset);		if (select(sk + 1, &rset, NULL, NULL, NULL) < 0)			return;		if (!FD_ISSET(sk, &rset))			continue;		len = read(sk, buf, data_size);		if (len <= 0) {			if (len < 0) {				if (reliable && (errno == ECOMM)) {					syslog(LOG_INFO, "L2CAP Error ECOMM - clearing error and continuing.");					optlen = sizeof(opt);					if (getsockopt(sk, SOL_SOCKET, SO_ERROR, &opt, &optlen) < 0) {						syslog(LOG_ERR, "Couldn't getsockopt(SO_ERROR): %s (%d)",							strerror(errno), errno);						return;					}					continue;				} else {					syslog(LOG_ERR, "Read error: %s(%d)",							strerror(errno), errno);				}			}			return;		}		syslog(LOG_INFO, "Recevied %d bytes", len);		hexdump(buf, len);	}}static void recv_mode(int sk){	struct timeval tv_beg, tv_end, tv_diff;	struct pollfd p;	long total;	uint32_t seq;	socklen_t optlen;	int opt;	syslog(LOG_INFO,"Receiving ...");	p.fd = sk;	p.events = POLLIN | POLLERR | POLLHUP;	seq = 0;	while (1) {		gettimeofday(&tv_beg, NULL);		total = 0;		while (total < data_size) {			uint32_t sq;			uint16_t l;			int i, len;			p.revents = 0;