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