1/*
2 * hci.c
3 */
4
5/*-
6 * Copyright (c) 2009 Maksim Yevmenkin <m_evmenkin@yahoo.com>
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 * 1. Redistributions of source code must retain the above copyright
13 *    notice, this list of conditions and the following disclaimer.
14 * 2. Redistributions in binary form must reproduce the above copyright
15 *    notice, this list of conditions and the following disclaimer in the
16 *    documentation and/or other materials provided with the distribution.
17 *
18 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
19 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
21 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
24 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
28 * SUCH DAMAGE.
29 *
30 * $FreeBSD$
31 */
32
33#include <assert.h>
34#include <bluetooth.h>
35#include <inttypes.h>
36#include <stdio.h>
37#include <stdlib.h>
38#include <string.h>
39#include <unistd.h>
40
41#undef	MIN
42#define	MIN(a, b)	(((a) < (b))? (a) : (b))
43
44static int    bt_devany_cb(int s, struct bt_devinfo const *di, void *xdevname);
45static char * bt_dev2node (char const *devname, char *nodename, int nnlen);
46
47int
48bt_devopen(char const *devname)
49{
50	struct sockaddr_hci	ha;
51	bdaddr_t		ba;
52	int			s;
53
54	if (devname == NULL) {
55		errno = EINVAL;
56		return (-1);
57	}
58
59	memset(&ha, 0, sizeof(ha));
60	ha.hci_len = sizeof(ha);
61	ha.hci_family = AF_BLUETOOTH;
62
63	if (bt_aton(devname, &ba)) {
64		if (!bt_devname(ha.hci_node, &ba))
65			return (-1);
66	} else if (bt_dev2node(devname, ha.hci_node,
67					sizeof(ha.hci_node)) == NULL) {
68		errno = ENXIO;
69		return (-1);
70	}
71
72	s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI);
73	if (s < 0)
74		return (-1);
75
76	if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
77	    connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0) {
78		close(s);
79		return (-1);
80	}
81
82	return (s);
83}
84
85int
86bt_devclose(int s)
87{
88	return (close(s));
89}
90
91int
92bt_devsend(int s, uint16_t opcode, void *param, size_t plen)
93{
94	ng_hci_cmd_pkt_t	h;
95	struct iovec		iv[2];
96	int			ivn;
97
98	if ((plen == 0 && param != NULL) ||
99	    (plen > 0 && param == NULL) ||
100	    plen > UINT8_MAX) {
101		errno = EINVAL;
102		return (-1);
103	}
104
105	iv[0].iov_base = &h;
106	iv[0].iov_len = sizeof(h);
107	ivn = 1;
108
109	h.type = NG_HCI_CMD_PKT;
110	h.opcode = htole16(opcode);
111	if (plen > 0) {
112		h.length = plen;
113
114		iv[1].iov_base = param;
115		iv[1].iov_len = plen;
116		ivn = 2;
117	} else
118		h.length = 0;
119
120	while (writev(s, iv, ivn) < 0) {
121		if (errno == EAGAIN || errno == EINTR)
122			continue;
123
124		return (-1);
125	}
126
127	return (0);
128}
129
130ssize_t
131bt_devrecv(int s, void *buf, size_t size, time_t to)
132{
133	ssize_t	n;
134
135	if (buf == NULL || size == 0) {
136		errno = EINVAL;
137		return (-1);
138	}
139
140	if (to >= 0) {
141		fd_set		rfd;
142		struct timeval	tv;
143
144		FD_ZERO(&rfd);
145		FD_SET(s, &rfd);
146
147		tv.tv_sec = to;
148		tv.tv_usec = 0;
149
150		while ((n = select(s + 1, &rfd, NULL, NULL, &tv)) < 0) {
151			if (errno == EAGAIN || errno == EINTR)
152				continue;
153
154			return (-1);
155		}
156
157		if (n == 0) {
158			errno = ETIMEDOUT;
159			return (-1);
160		}
161
162		assert(FD_ISSET(s, &rfd));
163	}
164
165	while ((n = read(s, buf, size)) < 0) {
166		if (errno == EAGAIN || errno == EINTR)
167			continue;
168
169		return (-1);
170	}
171
172	switch (*((uint8_t *) buf)) {
173	case NG_HCI_CMD_PKT: {
174		ng_hci_cmd_pkt_t	*h = (ng_hci_cmd_pkt_t *) buf;
175
176		if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
177			return (n);
178		} break;
179
180	case NG_HCI_ACL_DATA_PKT: {
181		ng_hci_acldata_pkt_t	*h = (ng_hci_acldata_pkt_t *) buf;
182
183		if (n >= sizeof(*h) && n == (sizeof(*h) + le16toh(h->length)))
184			return (n);
185		} break;
186
187	case NG_HCI_SCO_DATA_PKT: {
188		ng_hci_scodata_pkt_t	*h = (ng_hci_scodata_pkt_t *) buf;
189
190		if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
191			return (n);
192		} break;
193
194	case NG_HCI_EVENT_PKT: {
195		ng_hci_event_pkt_t	*h = (ng_hci_event_pkt_t *) buf;
196
197		if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
198			return (n);
199		} break;
200	}
201
202	errno = EIO;
203	return (-1);
204}
205
206int
207bt_devreq(int s, struct bt_devreq *r, time_t to)
208{
209	uint8_t				buf[320]; /* more than enough */
210	ng_hci_event_pkt_t		*e = (ng_hci_event_pkt_t *) buf;
211	ng_hci_command_compl_ep		*cc = (ng_hci_command_compl_ep *)(e+1);
212	ng_hci_command_status_ep	*cs = (ng_hci_command_status_ep*)(e+1);
213	struct bt_devfilter		old, new;
214	time_t				t_end;
215	uint16_t			opcode;
216	ssize_t				n;
217	int				error;
218
219	if (s < 0 || r == NULL || to < 0) {
220		errno = EINVAL;
221		return (-1);
222	}
223
224	if ((r->rlen == 0 && r->rparam != NULL) ||
225	    (r->rlen > 0 && r->rparam == NULL)) {
226		errno = EINVAL;
227		return (-1);
228	}
229
230	memset(&new, 0, sizeof(new));
231	bt_devfilter_pkt_set(&new, NG_HCI_EVENT_PKT);
232	bt_devfilter_evt_set(&new, NG_HCI_EVENT_COMMAND_COMPL);
233	bt_devfilter_evt_set(&new, NG_HCI_EVENT_COMMAND_STATUS);
234	if (r->event != 0)
235		bt_devfilter_evt_set(&new, r->event);
236
237	if (bt_devfilter(s, &new, &old) < 0)
238		return (-1);
239
240	error = 0;
241
242	n = bt_devsend(s, r->opcode, r->cparam, r->clen);
243	if (n < 0) {
244		error = errno;
245		goto out;
246	}
247
248	opcode = htole16(r->opcode);
249	t_end = time(NULL) + to;
250
251	do {
252		to = t_end - time(NULL);
253		if (to < 0)
254			to = 0;
255
256		n = bt_devrecv(s, buf, sizeof(buf), to);
257		if (n < 0) {
258			error = errno;
259			goto out;
260		}
261
262		if (e->type != NG_HCI_EVENT_PKT) {
263			error = EIO;
264			goto out;
265		}
266
267		n -= sizeof(*e);
268
269		switch (e->event) {
270		case NG_HCI_EVENT_COMMAND_COMPL:
271			if (cc->opcode == opcode) {
272				n -= sizeof(*cc);
273
274				if (r->rlen >= n) {
275					r->rlen = n;
276					memcpy(r->rparam, cc + 1, r->rlen);
277				}
278
279				goto out;
280			}
281			break;
282
283		case NG_HCI_EVENT_COMMAND_STATUS:
284			if (cs->opcode == opcode) {
285				if (r->event != NG_HCI_EVENT_COMMAND_STATUS) {
286					if (cs->status != 0) {
287						error = EIO;
288						goto out;
289					}
290				} else {
291					if (r->rlen >= n) {
292						r->rlen = n;
293						memcpy(r->rparam, cs, r->rlen);
294					}
295
296					goto out;
297				}
298			}
299			break;
300
301		default:
302			if (e->event == r->event) {
303				if (r->rlen >= n) {
304					r->rlen = n;
305					memcpy(r->rparam, e + 1, r->rlen);
306				}
307
308				goto out;
309			}
310			break;
311		}
312	} while (to > 0);
313
314	error = ETIMEDOUT;
315out:
316	bt_devfilter(s, &old, NULL);
317
318	if (error != 0) {
319		errno = error;
320		return (-1);
321	}
322
323	return (0);
324}
325
326int
327bt_devfilter(int s, struct bt_devfilter const *new, struct bt_devfilter *old)
328{
329	struct ng_btsocket_hci_raw_filter	f;
330	socklen_t				len;
331
332	if (new == NULL && old == NULL) {
333		errno = EINVAL;
334		return (-1);
335	}
336
337	if (old != NULL) {
338		len = sizeof(f);
339		if (getsockopt(s, SOL_HCI_RAW, SO_HCI_RAW_FILTER, &f, &len) < 0)
340			return (-1);
341
342		memset(old, 0, sizeof(*old));
343		memcpy(old->packet_mask, &f.packet_mask,
344			MIN(sizeof(old->packet_mask), sizeof(f.packet_mask)));
345		memcpy(old->event_mask, &f.event_mask,
346			MIN(sizeof(old->event_mask), sizeof(f.packet_mask)));
347	}
348
349	if (new != NULL) {
350		memset(&f, 0, sizeof(f));
351		memcpy(&f.packet_mask, new->packet_mask,
352			MIN(sizeof(f.packet_mask), sizeof(new->event_mask)));
353		memcpy(&f.event_mask, new->event_mask,
354			MIN(sizeof(f.event_mask), sizeof(new->event_mask)));
355
356		len = sizeof(f);
357		if (setsockopt(s, SOL_HCI_RAW, SO_HCI_RAW_FILTER, &f, len) < 0)
358			return (-1);
359	}
360
361	return (0);
362}
363
364void
365bt_devfilter_pkt_set(struct bt_devfilter *filter, uint8_t type)
366{
367	bit_set(filter->packet_mask, type - 1);
368}
369
370void
371bt_devfilter_pkt_clr(struct bt_devfilter *filter, uint8_t type)
372{
373	bit_clear(filter->packet_mask, type - 1);
374}
375
376int
377bt_devfilter_pkt_tst(struct bt_devfilter const *filter, uint8_t type)
378{
379	return (bit_test(filter->packet_mask, type - 1));
380}
381
382void
383bt_devfilter_evt_set(struct bt_devfilter *filter, uint8_t event)
384{
385	bit_set(filter->event_mask, event - 1);
386}
387
388void
389bt_devfilter_evt_clr(struct bt_devfilter *filter, uint8_t event)
390{
391	bit_clear(filter->event_mask, event - 1);
392}
393
394int
395bt_devfilter_evt_tst(struct bt_devfilter const *filter, uint8_t event)
396{
397	return (bit_test(filter->event_mask, event - 1));
398}
399
400int
401bt_devinquiry(char const *devname, time_t length, int num_rsp,
402		struct bt_devinquiry **ii)
403{
404	uint8_t				buf[320];
405	char				_devname[HCI_DEVNAME_SIZE];
406	struct bt_devfilter		f;
407	ng_hci_inquiry_cp		*cp = (ng_hci_inquiry_cp *) buf;
408	ng_hci_event_pkt_t		*e = (ng_hci_event_pkt_t *) buf;
409	ng_hci_inquiry_result_ep	*ep = (ng_hci_inquiry_result_ep *)(e+1);
410	ng_hci_inquiry_response		*ir;
411	struct bt_devinquiry		*i;
412	int				s, n;
413
414	if (ii == NULL) {
415		errno = EINVAL;
416		return (-1);
417	}
418
419	if (devname == NULL) {
420		memset(_devname, 0, sizeof(_devname));
421		devname = _devname;
422
423		n = bt_devenum(bt_devany_cb, _devname);
424		if (n <= 0) {
425			if (n == 0)
426				*ii = NULL;
427
428			return (n);
429		}
430	}
431
432	s = bt_devopen(devname);
433	if (s < 0)
434		return (-1);
435
436	if (bt_devfilter(s, NULL, &f) < 0) {
437		bt_devclose(s);
438		return (-1);
439	}
440
441	bt_devfilter_evt_set(&f, NG_HCI_EVENT_INQUIRY_COMPL);
442	bt_devfilter_evt_set(&f, NG_HCI_EVENT_INQUIRY_RESULT);
443
444	if (bt_devfilter(s, &f, NULL) < 0) {
445		bt_devclose(s);
446		return (-1);
447	}
448
449	/* Always use GIAC LAP */
450	cp->lap[0] = 0x33;
451	cp->lap[1] = 0x8b;
452	cp->lap[2] = 0x9e;
453
454	/*
455	 * Calculate inquire length in 1.28 second units
456	 * v2.x specification says that 1.28 -> 61.44 seconds
457	 * range is acceptable
458	 */
459
460	if (length <= 0)
461		length = 5;
462	else if (length == 1)
463		length = 2;
464	else if (length > 62)
465		length = 62;
466
467	cp->inquiry_length = (uint8_t)((length * 100) / 128);
468
469	if (num_rsp <= 0 || num_rsp > 255)
470		num_rsp = 8;
471	cp->num_responses = (uint8_t) num_rsp;
472
473	i = *ii = calloc(num_rsp, sizeof(struct bt_devinquiry));
474	if (i == NULL) {
475		bt_devclose(s);
476		errno = ENOMEM;
477		return (-1);
478	}
479
480	if (bt_devsend(s,
481		NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL, NG_HCI_OCF_INQUIRY),
482			cp, sizeof(*cp)) < 0) {
483		free(i);
484		bt_devclose(s);
485		return (-1);
486	}
487
488wait_for_more:
489
490	n = bt_devrecv(s, buf, sizeof(buf), length);
491	if (n < 0) {
492		free(i);
493		bt_devclose(s);
494		return (-1);
495	}
496
497	if (n < sizeof(ng_hci_event_pkt_t)) {
498		free(i);
499		bt_devclose(s);
500		errno = EIO;
501		return (-1);
502	}
503
504	switch (e->event) {
505	case NG_HCI_EVENT_INQUIRY_COMPL:
506		break;
507
508	case NG_HCI_EVENT_INQUIRY_RESULT:
509		ir = (ng_hci_inquiry_response *)(ep + 1);
510
511		for (n = 0; n < MIN(ep->num_responses, num_rsp); n ++) {
512			bdaddr_copy(&i->bdaddr, &ir->bdaddr);
513			i->pscan_rep_mode = ir->page_scan_rep_mode;
514			i->pscan_period_mode = ir->page_scan_period_mode;
515			memcpy(i->dev_class, ir->uclass, sizeof(i->dev_class));
516			i->clock_offset = le16toh(ir->clock_offset);
517
518			ir ++;
519			i ++;
520			num_rsp --;
521		}
522		/* FALLTHROUGH */
523
524	default:
525		goto wait_for_more;
526		/* NOT REACHED */
527	}
528
529	bt_devclose(s);
530
531	return (i - *ii);
532}
533
534int
535bt_devinfo(struct bt_devinfo *di)
536{
537	union {
538		struct ng_btsocket_hci_raw_node_state		r0;
539		struct ng_btsocket_hci_raw_node_bdaddr		r1;
540		struct ng_btsocket_hci_raw_node_features	r2;
541		struct ng_btsocket_hci_raw_node_buffer		r3;
542		struct ng_btsocket_hci_raw_node_stat		r4;
543		struct ng_btsocket_hci_raw_node_link_policy_mask r5;
544		struct ng_btsocket_hci_raw_node_packet_mask	r6;
545		struct ng_btsocket_hci_raw_node_role_switch	r7;
546		struct ng_btsocket_hci_raw_node_debug		r8;
547	}						rp;
548	struct sockaddr_hci				ha;
549	socklen_t					halen;
550	int						s, rval;
551
552	if (di == NULL) {
553		errno = EINVAL;
554		return (-1);
555	}
556
557	s = bt_devopen(di->devname);
558	if (s < 0)
559		return (-1);
560
561	rval = -1;
562
563	halen = sizeof(ha);
564	if (getsockname(s, (struct sockaddr *) &ha, &halen) < 0)
565		goto bad;
566	strlcpy(di->devname, ha.hci_node, sizeof(di->devname));
567
568	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STATE, &rp.r0, sizeof(rp.r0)) < 0)
569		goto bad;
570	di->state = rp.r0.state;
571
572	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BDADDR, &rp.r1, sizeof(rp.r1)) < 0)
573		goto bad;
574	bdaddr_copy(&di->bdaddr, &rp.r1.bdaddr);
575
576	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_FEATURES, &rp.r2, sizeof(rp.r2)) < 0)
577		goto bad;
578	memcpy(di->features, rp.r2.features, sizeof(di->features));
579
580	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BUFFER, &rp.r3, sizeof(rp.r3)) < 0)
581		goto bad;
582	di->cmd_free = rp.r3.buffer.cmd_free;
583	di->sco_size = rp.r3.buffer.sco_size;
584	di->sco_pkts = rp.r3.buffer.sco_pkts;
585	di->sco_free = rp.r3.buffer.sco_free;
586	di->acl_size = rp.r3.buffer.acl_size;
587	di->acl_pkts = rp.r3.buffer.acl_pkts;
588	di->acl_free = rp.r3.buffer.acl_free;
589
590	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STAT, &rp.r4, sizeof(rp.r4)) < 0)
591		goto bad;
592	di->cmd_sent = rp.r4.stat.cmd_sent;
593	di->evnt_recv = rp.r4.stat.evnt_recv;
594	di->acl_recv = rp.r4.stat.acl_recv;
595	di->acl_sent = rp.r4.stat.acl_sent;
596	di->sco_recv = rp.r4.stat.sco_recv;
597	di->sco_sent = rp.r4.stat.sco_sent;
598	di->bytes_recv = rp.r4.stat.bytes_recv;
599	di->bytes_sent = rp.r4.stat.bytes_sent;
600
601	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_LINK_POLICY_MASK,
602			&rp.r5, sizeof(rp.r5)) < 0)
603		goto bad;
604	di->link_policy_info = rp.r5.policy_mask;
605
606	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_PACKET_MASK,
607			&rp.r6, sizeof(rp.r6)) < 0)
608		goto bad;
609	di->packet_type_info = rp.r6.packet_mask;
610
611	 if (ioctl(s, SIOC_HCI_RAW_NODE_GET_ROLE_SWITCH,
612			&rp.r7, sizeof(rp.r7)) < 0)
613		goto bad;
614	di->role_switch_info = rp.r7.role_switch;
615
616	if (ioctl(s, SIOC_HCI_RAW_NODE_GET_DEBUG, &rp.r8, sizeof(rp.r8)) < 0)
617		goto bad;
618	di->debug = rp.r8.debug;
619
620	rval = 0;
621bad:
622	bt_devclose(s);
623
624	return (rval);
625}
626
627int
628bt_devenum(bt_devenum_cb_t cb, void *arg)
629{
630	struct ng_btsocket_hci_raw_node_list_names	rp;
631	struct bt_devinfo				di;
632	struct sockaddr_hci				ha;
633	int						s, i, count;
634
635	rp.num_names = HCI_DEVMAX;
636	rp.names = (struct nodeinfo *) calloc(rp.num_names,
637						sizeof(struct nodeinfo));
638	if (rp.names == NULL) {
639		errno = ENOMEM;
640		return (-1);
641	}
642
643	memset(&ha, 0, sizeof(ha));
644	ha.hci_len = sizeof(ha);
645	ha.hci_family = AF_BLUETOOTH;
646	ha.hci_node[0] = 'x';
647
648	s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI);
649	if (s < 0) {
650		free(rp.names);
651
652		return (-1);
653	}
654
655	if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
656	    connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
657	    ioctl(s, SIOC_HCI_RAW_NODE_LIST_NAMES, &rp, sizeof(rp)) < 0) {
658		close(s);
659		free(rp.names);
660
661		return (-1);
662	}
663
664	for (count = 0, i = 0; i < rp.num_names; i ++) {
665		strlcpy(di.devname, rp.names[i].name, sizeof(di.devname));
666		if (bt_devinfo(&di) < 0)
667			continue;
668
669		count ++;
670
671		if (cb == NULL)
672			continue;
673
674		strlcpy(ha.hci_node, rp.names[i].name, sizeof(ha.hci_node));
675		if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
676		    connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0)
677			continue;
678
679		if ((*cb)(s, &di, arg) > 0)
680			break;
681	}
682
683	close (s);
684	free(rp.names);
685
686	return (count);
687}
688
689static int
690bt_devany_cb(int s, struct bt_devinfo const *di, void *xdevname)
691{
692	strlcpy((char *) xdevname, di->devname, HCI_DEVNAME_SIZE);
693	return (1);
694}
695
696static char *
697bt_dev2node(char const *devname, char *nodename, int nnlen)
698{
699	static char const *	 bt_dev_prefix[] = {
700		"btccc",	/* 3Com Bluetooth PC-CARD */
701		"h4",		/* UART/serial Bluetooth devices */
702		"ubt",		/* Bluetooth USB devices */
703		NULL		/* should be last */
704	};
705
706	static char		_nodename[HCI_DEVNAME_SIZE];
707	char const		**p;
708	char			*ep;
709	int			plen, unit;
710
711	if (nodename == NULL) {
712		nodename = _nodename;
713		nnlen = HCI_DEVNAME_SIZE;
714	}
715
716	for (p = bt_dev_prefix; *p != NULL; p ++) {
717		plen = strlen(*p);
718		if (strncmp(devname, *p, plen) != 0)
719			continue;
720
721		unit = strtoul(devname + plen, &ep, 10);
722		if (*ep != '\0' &&
723		    strcmp(ep, "hci") != 0 &&
724		    strcmp(ep, "l2cap") != 0)
725			return (NULL);	/* can't make sense of device name */
726
727		snprintf(nodename, nnlen, "%s%uhci", *p, unit);
728
729		return (nodename);
730	}
731
732	return (NULL);
733}
734
735