Attachment #602956 for bug #705166

View | Details | Raw Unified | Return to bug 705166
Collapse All | Expand All




#endif /* DEBUG */

/*
** derived from bluez-utils/tools/hcitool.c
** tim - tim@spacetim.de
*/

/* Improved connection timeout 
** David De Sousa - davidesousa@gmail.com
*/

int bluescan
(struct entrybuff *enbfptr)
{
	struct sockaddr_l2 addr;
	socklen_t optlen;
	unsigned char *buf;
	char str[18];
	int i, sk, lost;
	int size    = 44;
	int ident   = 200;
	int timeout;
	bdaddr_t bdaddr;
	long skctl; // Socket Flags
	/* Setting variables for select to work. */
	fd_set skset; 
	struct timeval tv;
	int retval;
	char name[248];
	int opt, dd;
	int dev_id = -1;

	retval = false;

	bacpy(&bdaddr, BDADDR_ANY);


		timeout = enbfptr->timeout;


	buf = malloc(L2CAP_CMD_HDR_SIZE + size);
	if (!buf) {
		show_debug("Can't allocate buffer");
		return(false);
	}

	/* Create socket */
	sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
	if (sk < 0) {
		show_debug("Can't create socket");
		free(buf);
		return(false);
	}

	/* 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) {
		show_debug("Can't bind socket");
		close(sk);
		free(buf);
		return(false);
	}

	/* Set non-blocking state for the socket. DDS */
        skctl = fcntl(sk, F_GETFL, NULL);
        skctl |= O_NONBLOCK;
        fcntl(sk, F_SETFL, skctl);
				

	/* Connect to remote device */
	str2ba(enbfptr->btmac, &bdaddr);
	addr.l2_family = AF_BLUETOOTH;
	str2ba(enbfptr->btmac, &addr.l2_bdaddr);

	/* Connecting with timeout. */
	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		if (errno == EINPROGRESS) {
			tv.tv_sec = timeout; 
		        tv.tv_usec = 0;
			FD_ZERO(&skset); 
		        FD_SET(sk, &skset);
			if (select(sk+1, NULL, &skset, NULL, &tv) > 0) { 
				lon = sizeof(int); 
				getsockopt(sk, SOL_SOCKET, SO_ERROR, (void*)(&retval), &lon); 
				if (retval)
					show_debug ("Error Connecting!!!");
				else
					show_debug ("Connected!!!");
        		} 
			else {
				show_debug ("Connection Timeout [bluetooth device up?]");
				retval = 1;
			}
		}
		else {
			show_debug ("Error Connecting");
			retval = 1;
		}
		
		if (retval) {
	                close(sk);
        	        free(buf);
                	return(false);
		}
	}

	if (dev_id < 0) {
		dev_id = hci_get_route(&bdaddr);
		if (dev_id < 0) {
			fprintf(stderr, "Device is not available.\n");
			exit(1);
		}
	/* Get local address */
	memset(&addr, 0, sizeof(addr));
	optlen = sizeof(addr);

	if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
		show_debug("Can't get local address");
		close(sk);
		free(buf);
		return(false);
	}

	dd = hci_open_dev(dev_id);
	if (dd < 0) {
		perror("HCI device open failed");
		exit(1);
		buf[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A';

	l2cap_cmd_hdr *cmd = (l2cap_cmd_hdr *) buf;

	/* Build command header */
	cmd->code  = L2CAP_ECHO_REQ;
	cmd->ident = ident;
	cmd->len   = htobs(size);

	/* Send Echo Request */
	if (send(sk, buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) {
		show_debug("Send failed");
		close(sk);
		free(buf);
		return(false);
	}

	if (hci_read_remote_name(dd, &bdaddr, sizeof(name), name, 25000) == 0)
		retval = true;
	while (1) {
		struct pollfd pf[1];
		int err;

		pf[0].fd = sk;
		pf[0].events = POLLIN;

		if ((err = poll(pf, 1, timeout * 1000)) < 0) {
			show_debug("Poll failed");
			close(sk);
			free(buf);
			return(false);
		}

		if (!err) {
			lost = 1;
			break;
		}

		if ((err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) {
			show_debug("Recv failed");
			close(sk);
			free(buf);
			return(false);
		}

		if (!err){
			show_debug("Disconnected\n");
			close(sk);
			free(buf);
			return(false);
		}

		cmd->len = btohs(cmd->len);

	close(dd);
			continue;

		if (cmd->code == L2CAP_ECHO_RSP)
			break;
		if (cmd->code == L2CAP_COMMAND_REJ) {
			show_debug("Peer doesn't support Echo packets\n");
			close(sk);
			free(buf);
			return(false);
		}

	}
	show_debug("pam_blue successful!");
	close(sk);
	free(buf);
	return(retval);
}

Return to bug 705166

Lines 62-92 Link Here

(-)src/bluescan.c.orig (-167 / +22 lines)
62	#endif /* DEBUG */	62	#endif /* DEBUG */
63		63
64	/*	64	/*
65	** derived form bluez-utils/tools/l2ping.c	65	** derived from bluez-utils/tools/hcitool.c
66	** tim - tim@spacetim.de
67	*/
68
69	/* Improved connection timeout
70	** David De Sousa - davidesousa@gmail.com
71	*/	66	*/
72		67
73	int bluescan	68	int bluescan
74	(struct entrybuff *enbfptr)	69	(struct entrybuff *enbfptr)
75	{	70	{
76	struct sockaddr_l2 addr;
77	socklen_t optlen;
78	unsigned char *buf;	71	unsigned char *buf;
79	char str[18];
80	int i, sk, lost;
81	int size = 44;
82	int ident = 200;
83	int timeout;	72	int timeout;
84	bdaddr_t bdaddr;	73	bdaddr_t bdaddr;
85	long skctl; // Socket Flags
86	/* Setting variables for select to work. */
87	fd_set skset;
88	struct timeval tv;	74	struct timeval tv;
89	int lon, retval;	75	int retval;
		76	char name[248];
		77	int opt, dd;
		78	int dev_id = -1;
		79
		80	retval = false;
90		81
91	bacpy(&bdaddr, BDADDR_ANY);	82	bacpy(&bdaddr, BDADDR_ANY);
92		83
Lines 96-260 Link Here
96	timeout = enbfptr->timeout;	87	timeout = enbfptr->timeout;
97		88
98		89
99	buf = malloc(L2CAP_CMD_HDR_SIZE + size);
100	if (!buf) {
101	show_debug("Can't allocate buffer");
102	return(false);
103	}
104
105	/* Create socket */
106	sk = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_L2CAP);
107	if (sk < 0) {
108	show_debug("Can't create socket");
109	free(buf);
110	return(false);
111	}
112
113	/* Bind to local address */
114	memset(&addr, 0, sizeof(addr));
115	addr.l2_family = AF_BLUETOOTH;
116	bacpy(&addr.l2_bdaddr, &bdaddr);
117
118	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
119	show_debug("Can't bind socket");
120	close(sk);
121	free(buf);
122	return(false);
123	}
124
125	/* Set non-blocking state for the socket. DDS */
126	skctl = fcntl(sk, F_GETFL, NULL);
127	skctl \|= O_NONBLOCK;
128	fcntl(sk, F_SETFL, skctl);
129
130
131	/* Connect to remote device */	90	/* Connect to remote device */
132	memset(&addr, 0, sizeof(addr));	91	str2ba(enbfptr->btmac, &bdaddr);
133	addr.l2_family = AF_BLUETOOTH;
134	str2ba(enbfptr->btmac, &addr.l2_bdaddr);
135
136	/* Connecting with timeout. */
137	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
138	if (errno == EINPROGRESS) {
139	tv.tv_sec = timeout;
140	tv.tv_usec = 0;
141	FD_ZERO(&skset);
142	FD_SET(sk, &skset);
143	if (select(sk+1, NULL, &skset, NULL, &tv) > 0) {
144	lon = sizeof(int);
145	getsockopt(sk, SOL_SOCKET, SO_ERROR, (void*)(&retval), &lon);
146	if (retval)
147	show_debug ("Error Connecting!!!");
148	else
149	show_debug ("Connected!!!");
150	}
151	else {
152	show_debug ("Connection Timeout [bluetooth device up?]");
153	retval = 1;
154	}
155	}
156	else {
157	show_debug ("Error Connecting");
158	retval = 1;
159	}
160
161	if (retval) {
162	close(sk);
163	free(buf);
164	return(false);
165	}
166	}
167		92
168	/* Set socket to original mode (blocking). */	93	if (dev_id < 0) {
169		94	dev_id = hci_get_route(&bdaddr);
170	skctl = fcntl(sk, F_GETFL, NULL);	95	if (dev_id < 0) {
171	skctl &= (~O_NONBLOCK);	96	fprintf(stderr, "Device is not available.\n");
172	fcntl(sk, F_SETFL, skctl);	97	exit(1);
173		98	}
174	/* Get local address */
175	memset(&addr, 0, sizeof(addr));
176	optlen = sizeof(addr);
177
178	if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
179	show_debug("Can't get local address");
180	close(sk);
181	free(buf);
182	return(false);
183	}	99	}
184		100
185	ba2str(&addr.l2_bdaddr, str);	101	dd = hci_open_dev(dev_id);
186		102	if (dd < 0) {
187	/* Initialize buffer */	103	perror("HCI device open failed");
188	for (i = 0; i < size; i++)	104	exit(1);
189	buf[L2CAP_CMD_HDR_SIZE + i] = (i % 40) + 'A';
190
191	l2cap_cmd_hdr cmd = (l2cap_cmd_hdr ) buf;
192
193	/* Build command header */
194	cmd->code = L2CAP_ECHO_REQ;
195	cmd->ident = ident;
196	cmd->len = htobs(size);
197
198	/* Send Echo Request */
199	if (send(sk, buf, L2CAP_CMD_HDR_SIZE + size, 0) <= 0) {
200	show_debug("Send failed");
201	close(sk);
202	free(buf);
203	return(false);
204	}	105	}
205		106
206	/* Wait for Echo Response */	107	if (hci_read_remote_name(dd, &bdaddr, sizeof(name), name, 25000) == 0)
207	lost = 0;	108	retval = true;
208	while (1) {
209	struct pollfd pf[1];
210	int err;
211
212	pf[0].fd = sk;
213	pf[0].events = POLLIN;
214
215	if ((err = poll(pf, 1, timeout * 1000)) < 0) {
216	show_debug("Poll failed");
217	close(sk);
218	free(buf);
219	return(false);
220	}
221
222	if (!err) {
223	lost = 1;
224	break;
225	}
226
227	if ((err = recv(sk, buf, L2CAP_CMD_HDR_SIZE + size, 0)) < 0) {
228	show_debug("Recv failed");
229	close(sk);
230	free(buf);
231	return(false);
232	}
233
234	if (!err){
235	show_debug("Disconnected\n");
236	close(sk);
237	free(buf);
238	return(false);
239	}
240
241	cmd->len = btohs(cmd->len);
242		109
243	if (cmd->ident != ident)	110	close(dd);
244	continue;
245		111
246	if (cmd->code == L2CAP_ECHO_RSP)
247	break;
248	if (cmd->code == L2CAP_COMMAND_REJ) {
249	show_debug("Peer doesn't support Echo packets\n");
250	close(sk);
251	free(buf);
252	return(false);
253	}
254
255	}
256	show_debug("pam_blue successful!");	112	show_debug("pam_blue successful!");
257	close(sk);
258	free(buf);	113	free(buf);
259	return(true);	114	return(retval);
260	}	115	}