Add connect and search options to the HID daemon

2 files changed, 255 insertions, 44 deletions

diff --git a/hidd/main.c b/hidd/main.c
index 46b25ec4..aa2a7b1f 100644
--- a/hidd/main.c
+++ b/hidd/main.c
@@ -63,6 +63,42 @@ static void sig_term(int sig)
 	__io_canceled = 1;
 }
 
+static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm)
+{
+	struct sockaddr_l2 addr;
+	struct l2cap_options opts;
+	int sk;
+
+	if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
+		return -1;
+
+	addr.l2_family  = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, src);
+	addr.l2_psm = 0;
+
+	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		close(sk);
+		return -1;
+	}
+
+	opts.imtu = HIDP_DEFAULT_MTU;
+	opts.omtu = HIDP_DEFAULT_MTU;
+	opts.flush_to = 0xffff;
+
+	setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));
+
+	addr.l2_family  = AF_BLUETOOTH;
+	bacpy(&addr.l2_bdaddr, dst);
+	addr.l2_psm = htobs(psm);
+
+	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		close(sk);
+		return -1;
+	}
+
+	return sk;
+}
+
 static int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm, int backlog)
 {
 	struct sockaddr_l2 addr;
@@ -115,7 +151,7 @@ static int l2cap_accept(int sk, bdaddr_t *bdaddr)
 	return nsk;
 }
 
-static int create(int ctl, int csk, int isk, int timeout)
+static int create_device(int ctl, int csk, int isk, int timeout)
 {
 	struct hidp_connadd_req req;
 	struct sockaddr_l2 addr;
@@ -140,16 +176,18 @@ static int create(int ctl, int csk, int isk, int timeout)
 
 	bacpy(&dst, &addr.l2_bdaddr);
 
-	ba2str(&dst, bda);
-	syslog(LOG_INFO, "New HID connection from %s", bda);
-
 	memset(&req, 0, sizeof(req));
 	req.ctrl_sock = csk;
 	req.intr_sock = isk;
 	req.flags     = 0;
 	req.idle_to   = timeout * 60;
 
-	get_hid_device_info(&src, &dst, &req);
+	err = get_hid_device_info(&src, &dst, &req);
+	if (err < 0)
+		return err;
+
+	ba2str(&dst, bda);
+	syslog(LOG_INFO, "New HID device %s (%s)", bda, req.name);
 
 	err = ioctl(ctl, HIDPCONNADD, &req);
 
@@ -159,7 +197,7 @@ static int create(int ctl, int csk, int isk, int timeout)
 	return err;
 }
 
-static void run(int ctl, int csk, int isk, int timeout)
+static void run_server(int ctl, int csk, int isk, int timeout)
 {
 	struct pollfd p[2];
 	short events;
@@ -185,7 +223,7 @@ static void run(int ctl, int csk, int isk, int timeout)
 			ncsk = l2cap_accept(csk, NULL);
 			nisk = l2cap_accept(isk, NULL);
 
-			err = create(ctl, ncsk, nisk, timeout);
+			err = create_device(ctl, ncsk, nisk, timeout);
 			if (err < 0)
 				syslog(LOG_ERR, "HID create error %d (%s)",
 						errno, strerror(errno));
@@ -196,12 +234,87 @@ static void run(int ctl, int csk, int isk, int timeout)
 	}
 }
 
+static void do_connect(int ctl, bdaddr_t *src, bdaddr_t *dst, int timeout)
+{
+	int csk, isk, err;
+
+	csk = l2cap_connect(src, dst, L2CAP_PSM_HIDP_CTRL);
+	if (csk < 0) {
+		perror("Can't create HID control channel");
+		close(ctl);
+		exit(1);
+	}
+
+	isk = l2cap_connect(src, dst, L2CAP_PSM_HIDP_INTR);
+	if (isk < 0) {
+		perror("Can't create HID interrupt channel");
+		close(csk);
+		close(ctl);
+		exit(1);
+	}
+
+	err = create_device(ctl, csk, isk, timeout);
+	if (err < 0) {
+		fprintf(stderr, "HID create error %d (%s)\n",
+						errno, strerror(errno));
+		close(isk);
+		sleep(1);
+		close(csk);
+		close(ctl);
+		exit(1);
+	}
+}
+
+static void do_search(int ctl, bdaddr_t *bdaddr, int timeout)
+{
+	inquiry_info *info = NULL;
+	bdaddr_t src, dst;
+	int i, dev_id, num_rsp, length, flags;
+	char addr[18];
+	uint8_t class[3];
+
+	ba2str(bdaddr, addr);
+	dev_id = hci_devid(addr);
+	if (dev_id < 0) {
+		dev_id = hci_get_route(NULL);
+		hci_devba(dev_id, &src);
+	} else
+		bacpy(&src, bdaddr);
+
+	length  = 8;	/* ~10 seconds */
+	num_rsp = 0;
+	flags   = 0;
+
+	printf("Searching ...\n");
+
+	num_rsp = hci_inquiry(dev_id, length, num_rsp, NULL, &info, flags);
+
+	for (i = 0; i < num_rsp; i++) {
+		memcpy(class, (info+i)->dev_class, 3);
+		if (class[1] == 0x25 && class[2] == 0x00) {
+			bacpy(&dst, &(info+i)->bdaddr);
+			ba2str(&dst, addr);
+
+			printf("\tConnecting to device %s\n", addr);
+			do_connect(ctl, &src, &dst, timeout);
+		}
+	}
+
+	free(info);
+
+	if (!num_rsp) {
+		fprintf(stderr, "\tNo devices in range or visible\n");
+		close(ctl);
+		exit(1);
+	}
+}
+
 static void usage(void)
 {
 	printf("hidd - Bluetooth HID daemon\n\n");
 
 	printf("Usage:\n"
-		"\thidd [options]\n"
+		"\thidd [options] [commands]\n"
 		"\n");
 
 	printf("Options:\n"
@@ -210,6 +323,12 @@ static void usage(void)
 		"\t-n, --nodaemon       Don't fork daemon to background\n"
 		"\t-h, --help           Display help\n"
 		"\n");
+
+	printf("Commands:\n"
+		"\t--server             Start HID server\n"
+		"\t--search             Search for HID devices\n"
+		"\t--connect <bdaddr>   Connect remote HID device\n"
+		"\n");
 }
 
 static struct option main_options[] = {
@@ -217,21 +336,25 @@ static struct option main_options[] = {
 	{ "nodaemon",	0, 0, 'n' },
 	{ "timeout",	1, 0, 't' },
 	{ "device",	1, 0, 'i' },
+	{ "server",	0, 0, 'd' },
+	{ "listen",	0, 0, 'd' },
+	{ "search",	0, 0, 's' },
+	{ "connect",	1, 0, 'c' },
 	{ 0, 0, 0, 0 }
 };
 
 int main(int argc, char *argv[])
 {
 	struct sigaction sa;
-	bdaddr_t bdaddr;
+	bdaddr_t bdaddr, dev;
 	char addr[18];
 	int log_option = LOG_NDELAY | LOG_PID;
 	int opt, fd, ctl, csk, isk;
-	int daemon = 1, timeout = 30;
+	int mode = 0, daemon = 1, timeout = 30;
 
 	bacpy(&bdaddr, BDADDR_ANY);
 
-	while ((opt = getopt_long(argc, argv, "+i:nt:h", main_options, NULL)) != -1) {
+	while ((opt = getopt_long(argc, argv, "+i:nt:dsc:h", main_options, NULL)) != -1) {
 		switch(opt) {
 		case 'i':
 			if (!strncasecmp(optarg, "hci", 3))
@@ -245,6 +368,16 @@ int main(int argc, char *argv[])
 		case 't':
 			timeout = atoi(optarg);
 			break;
+		case 'd':
+			mode = 1;
+			break;
+		case 's':
+			mode = 2;
+			break;
+		case 'c':
+			str2ba(optarg, &dev);
+			mode = 3;
+			break;
 		case 'h':
 			usage();
 			exit(0);
@@ -255,24 +388,42 @@ int main(int argc, char *argv[])
 
 	ba2str(&bdaddr, addr);
 
-	csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, 10);
-	if (csk < 0) {
-		perror("Can't listen on HID control channel");
+	ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP);
+	if (ctl < 0) {
+		perror("Can't open HIDP control socket");
 		exit(1);
 	}
 
-	isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, 10);
-	if (isk < 0) {
-		perror("Can't listen on HID interrupt channel");
-		close(csk);
-		exit(1);
-	}
+	switch (mode) {
+	case 1:
+		csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, 10);
+		if (csk < 0) {
+			perror("Can't listen on HID control channel");
+			close(ctl);
+			exit(1);
+		}
 
-	ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP);
-	if (ctl < 0) {
-		perror("Can't open HIDP control socket");
-		close(csk);
-		close(isk);
+		isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, 10);
+		if (isk < 0) {
+			perror("Can't listen on HID interrupt channel");
+			close(ctl);
+			close(csk);
+			exit(1);
+		}
+		break;
+
+	case 2:
+		do_search(ctl, &bdaddr, timeout);
+		close(ctl);
+		exit(0);
+
+	case 3:
+		do_connect(ctl, &bdaddr, &dev, timeout);
+		close(ctl);
+		exit(0);
+
+	default:
+		usage();
 		exit(1);
 	}
 
@@ -309,13 +460,13 @@ int main(int argc, char *argv[])
 	sigaction(SIGCHLD, &sa, NULL);
 	sigaction(SIGPIPE, &sa, NULL);
 
-	run(ctl, csk, isk, timeout);
+	run_server(ctl, csk, isk, timeout);
 
 	syslog(LOG_INFO, "Exit");
 
-	close(ctl);
 	close(csk);
 	close(isk);
+	close(ctl);
 
 	return 0;
 }
diff --git a/hidd/sdp.c b/hidd/sdp.c
index 4c92aa66..4db9a4c8 100644
--- a/hidd/sdp.c
+++ b/hidd/sdp.c
@@ -51,43 +51,103 @@
 
 int get_hid_device_info(bdaddr_t *src, bdaddr_t *dst, struct hidp_connadd_req *req)
 {
+	uint32_t range = 0x0000ffff;
 	sdp_session_t *s;
-	sdp_list_t *srch, *attrs, *rsp;
+	sdp_list_t *search, *attrid, *pnp_rsp, *hid_rsp;
 	sdp_record_t *rec;
-	sdp_data_t *pdlist;
+	sdp_data_t *pdlist, *pdlist2;
 	uuid_t svclass;
-	uint16_t attr;
 	int err;
 
 	s = sdp_connect(src, dst, 0);
 	if (!s)
 		return -1;
 
+	sdp_uuid16_create(&svclass, PNP_INFO_SVCLASS_ID);
+	search = sdp_list_append(NULL, &svclass);
+	attrid = sdp_list_append(NULL, &range);
+
+	err = sdp_service_search_attr_req(s, search, SDP_ATTR_REQ_RANGE, attrid, &pnp_rsp);
+
+	sdp_list_free(search, 0);
+	sdp_list_free(attrid, 0);
+
 	sdp_uuid16_create(&svclass, HID_SVCLASS_ID);
-	srch  = sdp_list_append(NULL, &svclass);
+	search = sdp_list_append(NULL, &svclass);
+	attrid = sdp_list_append(NULL, &range);
 
-	attr  = 0x0206;
-	attrs = sdp_list_append(NULL, &attr);
+	err = sdp_service_search_attr_req(s, search, SDP_ATTR_REQ_RANGE, attrid, &hid_rsp);
 
-	err = sdp_service_search_attr_req(s, srch, SDP_ATTR_REQ_INDIVIDUAL, attrs, &rsp);
+	sdp_list_free(search, 0);
+	sdp_list_free(attrid, 0);
 
 	sdp_close(s);
 
-	if (err || !rsp)
+	if (err || !hid_rsp)
 		return -1;
 
-	rec = (sdp_record_t *) rsp->data;
+	if (pnp_rsp) {
+		rec = (sdp_record_t *) pnp_rsp->data;
+
+		pdlist = sdp_data_get(rec, 0x0201);
+		if (pdlist)
+			req->vendor = pdlist->val.uint16;
+
+		pdlist = sdp_data_get(rec, 0x0202);
+		if (pdlist)
+			req->product = pdlist->val.uint16;
+
+		pdlist = sdp_data_get(rec, 0x0203);
+		if (pdlist)
+			req->version = pdlist->val.uint16;
+
+		sdp_record_free(rec);
+	}
+
+	rec = (sdp_record_t *) hid_rsp->data;
+
+	pdlist = sdp_data_get(rec, 0x0101);
+	pdlist2 = sdp_data_get(rec, 0x0102);
+	if (pdlist) {
+		if (pdlist2) {
+			if (strncmp(pdlist->val.str, pdlist2->val.str, 5)) {
+				strncpy(req->name, pdlist2->val.str, sizeof(req->name) - 1);
+				strcat(req->name, " ");
+			}
+			strncat(req->name, pdlist->val.str,
+					sizeof(req->name) - strlen(req->name));
+		} else
+			strncpy(req->name, pdlist->val.str, sizeof(req->name));
+	} else {
+		pdlist2 = sdp_data_get(rec, 0x0100);
+		if (pdlist2)
+			strncpy(req->name, pdlist2->val.str, sizeof(req->name));
+	}
+
+	pdlist = sdp_data_get(rec, 0x0201);
+	if (pdlist)
+		req->parser = pdlist->val.uint16;
+	else
+		req->parser = 0x0100;
+
+	pdlist = sdp_data_get(rec, 0x0203);
+	if (pdlist)
+		req->country = pdlist->val.uint8;
 
 	pdlist = sdp_data_get(rec, 0x0206);
-	pdlist = pdlist->val.dataseq;
-	pdlist = pdlist->val.dataseq;
-	pdlist = pdlist->next;
-
-	req->rd_data = malloc(pdlist->unitSize);
-	if (req->rd_data) {
-		memcpy(req->rd_data, (unsigned char *) pdlist->val.str, pdlist->unitSize);
-		req->rd_size = pdlist->unitSize;
+	if (pdlist) {
+		pdlist = pdlist->val.dataseq;
+		pdlist = pdlist->val.dataseq;
+		pdlist = pdlist->next;
+
+		req->rd_data = malloc(pdlist->unitSize);
+		if (req->rd_data) {
+			memcpy(req->rd_data, (unsigned char *) pdlist->val.str, pdlist->unitSize);
+			req->rd_size = pdlist->unitSize;
+		}
 	}
 
+	sdp_record_free(rec);
+
 	return 0;
 }