4 files changed, 189 insertions, 0 deletions

diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.8 b/usr.sbin/bluetooth/hccontrol/hccontrol.8
index 0d457a7c8c3c..3dc709024db6 100644
--- a/usr.sbin/bluetooth/hccontrol/hccontrol.8
+++ b/usr.sbin/bluetooth/hccontrol/hccontrol.8
@@ -161,6 +161,7 @@ are:
 .It Cm LE_Clear_White_List
 .It Cm LE_Add_Device_To_White_List
 .It Cm LE_Remove_Device_From_White_List
+.It Cm LE_Connect
 .El
 .Pp
 The currently supported node commands in
diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.h b/usr.sbin/bluetooth/hccontrol/hccontrol.h
index 9c69146652b5..e6ea30de0cfd 100644
--- a/usr.sbin/bluetooth/hccontrol/hccontrol.h
+++ b/usr.sbin/bluetooth/hccontrol/hccontrol.h
@@ -80,6 +80,8 @@ char const *	hci_con_state2str   (int);
 char const *	hci_status2str      (int);
 char const *	hci_bdaddr2str      (bdaddr_t const *);
 char const * 	hci_addrtype2str    (int type);
+char const *    hci_role2str        (int role);
+char const *    hci_mc_accuracy2str (int accuracy);
 
 void dump_adv_data(int len, uint8_t* advdata);
 void print_adv_data(int len, uint8_t* advdata);
diff --git a/usr.sbin/bluetooth/hccontrol/le.c b/usr.sbin/bluetooth/hccontrol/le.c
index b5b77bc7d74f..357a7210fedc 100644
--- a/usr.sbin/bluetooth/hccontrol/le.c
+++ b/usr.sbin/bluetooth/hccontrol/le.c
@@ -67,6 +67,8 @@ static int le_read_white_list_size(int s, int argc, char *argv[]);
 static int le_clear_white_list(int s, int argc, char *argv[]);
 static int le_add_device_to_white_list(int s, int argc, char *argv[]);
 static int le_remove_device_from_white_list(int s, int argc, char *argv[]);
+static int le_connect(int s, int argc, char *argv[]);
+static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose);
 
 static int
 le_set_scan_param(int s, int argc, char *argv[])
@@ -933,6 +935,157 @@ le_remove_device_from_white_list(int s, int argc, char *argv[])
 	return (OK);
 }
 
+static int
+le_connect(int s, int argc, char *argv[])
+{ 
+	ng_hci_le_create_connection_cp cp;
+	ng_hci_status_rp rp;
+	char 			b[512];
+	ng_hci_event_pkt_t	*e = (ng_hci_event_pkt_t *) b;
+
+	int n, scancount, bufsize;
+	char ch;
+	bool addr_set = false;
+	bool verbose = false;
+
+	optreset = 1;
+	optind = 0;
+
+	/* minimal scan interval (2.5ms) */ 
+	cp.scan_interval = htole16(4);
+	cp.scan_window = htole16(4);
+
+	/* Don't use the whitelist */
+	cp.filter_policy = 0x00;
+
+	/* Default to public peer address */
+	cp.peer_addr_type = 0x00;
+
+	/* Own address type public */
+	cp.own_address_type = 0x00;
+
+	/* 18.75ms min connection interval */
+	cp.conn_interval_min = htole16(0x000F);
+	/* 18.75ms max connection interval */
+	cp.conn_interval_max = htole16(0x000F);
+
+	/* 0 events connection latency */
+	cp.conn_latency = htole16(0x0000);
+
+	/* 32s supervision timeout */
+	cp.supervision_timeout = htole16(0x0C80);
+
+	/* Min CE Length 0.625 ms */
+	cp.min_ce_length = htole16(1);
+	/* Max CE Length 0.625 ms */
+	cp.max_ce_length = htole16(1);
+
+	while ((ch = getopt(argc, argv , "a:t:v")) != -1) {
+		switch(ch) {
+		case 't':
+			if (strcmp(optarg, "public") == 0)
+				cp.peer_addr_type = 0x00;
+			else if (strcmp(optarg, "random") == 0)
+				cp.peer_addr_type = 0x01;
+			else 
+				return (USAGE);
+			break;
+		case 'a':
+			addr_set = true;
+			if (!bt_aton(optarg, &cp.peer_addr)) {
+				struct hostent	*he = NULL;
+
+				if ((he = bt_gethostbyname(optarg)) == NULL)
+					return (USAGE);
+
+				memcpy(&cp.peer_addr, he->h_addr,
+					sizeof(cp.peer_addr));
+			}
+			break;
+		case 'v':
+			verbose = true;
+			break;
+		}
+	}
+
+	if (addr_set == false) 
+		return (USAGE);
+
+	n = sizeof(rp);
+	if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
+		NG_HCI_OCF_LE_CREATE_CONNECTION), 
+		(void *)&cp, sizeof(cp), (void *)&rp, &n) == ERROR)
+		return (ERROR);
+
+	if (rp.status != 0x00) {
+		fprintf(stdout,
+			"Create connection failed. Status: %s [%#02x]\n", 
+			hci_status2str(rp.status), rp.status);
+		return (FAILED);
+	}
+
+	scancount = 0;
+	while (scancount < 3) {
+		/* wait for connection events */
+		bufsize = sizeof(b);
+		if (hci_recv(s, b, &bufsize) == ERROR) {
+			return (ERROR);
+		}
+
+		if (bufsize < sizeof(*e)) {
+			errno = EIO;
+			return (ERROR);
+		}
+		scancount++;
+		if (e->event == NG_HCI_EVENT_LE) {
+			handle_le_connection_event(e, verbose);
+			break;
+		}
+	}
+
+	return (OK);
+}
+
+static void handle_le_connection_event(ng_hci_event_pkt_t* e, bool verbose) 
+{
+	ng_hci_le_ep	*ev_pkt;
+	ng_hci_le_connection_complete_ep *conn_event;
+
+	ev_pkt = (ng_hci_le_ep *)(e + 1);
+
+	if (ev_pkt->subevent_code == NG_HCI_LEEV_CON_COMPL) {
+		conn_event =(ng_hci_le_connection_complete_ep *)(ev_pkt + 1);
+		fprintf(stdout, "Handle: %d\n", le16toh(conn_event->handle));
+		if (verbose) {
+			fprintf(stdout,
+				"Status: %s\n",
+				hci_status2str(conn_event->status));
+			fprintf(stdout,
+				"Role: %s\n",
+				hci_role2str(conn_event->role));
+			fprintf(stdout,
+				"Address Type: %s\n",
+				hci_addrtype2str(conn_event->address_type));
+			fprintf(stdout,
+				"Address: %s\n",
+				hci_bdaddr2str(&conn_event->address));
+			fprintf(stdout,
+				"Interval: %.2fms\n", 
+				6.25 * le16toh(conn_event->interval));
+			fprintf(stdout,
+				"Latency: %d events\n", conn_event->latency);
+			fprintf(stdout,
+				"Supervision timeout: %dms\n",
+				 10 * le16toh(conn_event->supervision_timeout));
+			fprintf(stdout,
+				"Master clock accuracy: %sn",
+				hci_mc_accuracy2str(
+					conn_event->master_clock_accuracy));
+		}
+	}
+	return;
+}
+
 struct hci_command le_commands[] = {
 {
 	"le_enable",
@@ -1037,4 +1190,10 @@ struct hci_command le_commands[] = {
 	  "Remove device from the white list",
 	  &le_remove_device_from_white_list
   },
+  {
+	  "le_connect",
+	  "le_connect -a address [-t public|random] [-v]\n"
+	  "Connect to an LE device",
+	  &le_connect
+  },
 };
diff --git a/usr.sbin/bluetooth/hccontrol/util.c b/usr.sbin/bluetooth/hccontrol/util.c
index 2e6ecc78a5ac..56fd2582c3dd 100644
--- a/usr.sbin/bluetooth/hccontrol/util.c
+++ b/usr.sbin/bluetooth/hccontrol/util.c
@@ -3295,3 +3295,30 @@ hci_addrtype2str(int type)
 	return (type >= SIZE(t)? "?" : t[type]);
 } /* hci_addrtype2str */
 
+char const *
+hci_role2str(int role)
+{
+	static char const * const	roles[] = {
+		/* 0x00 */ "Master",
+		/* 0x01 */ "Slave",
+	};
+
+	return (role >= SIZE(roles)? "Unknown role" : roles[role]);
+} /* hci_role2str */
+
+char const *
+hci_mc_accuracy2str(int accuracy)
+{
+	static char const * const	acc[] = {
+		/* 0x00 */ "500 ppm",
+		/* 0x01 */ "250 ppm",
+		/* 0x02 */ "150 ppm",
+		/* 0x03 */ "100 ppm",
+		/* 0x04 */ "75 ppm",
+		/* 0x05 */ "50 ppm",
+		/* 0x06 */ "30 ppm",
+		/* 0x07 */ "20 ppm",
+	};
+
+	return (accuracy >= SIZE(acc)? "Unknown accuracy" : acc[accuracy]);
+} /* hci_mc_accuracy2str */