lib: add start and stop discovery

index 4cfeac0..3f69365 100644
@@ -447,6 +447,21 @@ struct mgmt_cp_set_public_address {
 	bdaddr_t bdaddr;
 } __packed;
 
+#define MGMT_OP_START_SERVICE_DISCOVERY		0x003A
+struct mgmt_cp_start_service_discovery {
+	uint8_t type;
+	int8_t rssi_threshold;
+	uint16_t num_uuid;
+	uint8_t uuid[0][16];
+} __packed;
+#define MGMT_START_SERVICE_DISCOVERY_SIZE	1
+
+#define MGMT_OP_STOP_SERVICE_DISCOVERY		0x003B
+struct mgmt_cp_stop_service_discovery {
+	uint8_t type;
+} __packed;
+#define MGMT_STOP_SERVICE_DISCOVERY_SIZE	1
+
 #define MGMT_EV_CMD_COMPLETE		0x0001
 struct mgmt_ev_cmd_complete {
 	uint16_t opcode;

index e227a1c..32e4a23 100644
@@ -1547,6 +1547,127 @@ static void cmd_con(struct mgmt *mgmt, uint16_t index, int argc, char **argv)
 	}
 }
 
+static void find_service_rsp(uint8_t status, uint16_t len, const void *param,
+							void *user_data)
+{
+	if (status != 0) {
+		fprintf(stderr,
+			"Unable to start service discovery. status 0x%02x (%s)\n",
+			status, mgmt_errstr(status));
+		mainloop_quit();
+		return;
+	}
+
+	printf("Service discovery started\n");
+	discovery = true;
+}
+
+static void find_service_usage(void)
+{
+	printf("Usage: btmgmt find-service -u UUID [-r RSSI_Threshold] [-l|-b]>\n");
+}
+
+static struct option find_service_options[] = {
+	{ "help",	no_argument, 0, 'h' },
+	{ "le-only",	no_argument, 0, 'l' },
+	{ "bredr-only",	no_argument, 0, 'b' },
+	{ "uuid",	required_argument, 0, 'u' },
+	{ "rssi",	required_argument, 0, 'r' },
+	{ 0, 0, 0, 0 }
+};
+
+static void uuid_to_uuid128(uuid_t *uuid128, const uuid_t *uuid)
+{
+	if (uuid->type == SDP_UUID16)
+		sdp_uuid16_to_uuid128(uuid128, uuid);
+	else if (uuid->type == SDP_UUID32)
+		sdp_uuid32_to_uuid128(uuid128, uuid);
+	else
+		memcpy(uuid128, uuid, sizeof(*uuid));
+}
+
+static void cmd_find_service(struct mgmt *mgmt, uint16_t index, int argc,
+			     char **argv)
+{
+	struct mgmt_cp_start_service_discovery *cp;
+	uint8_t buf[sizeof(*cp) + 16];
+	int opt;
+	int total_size = sizeof(*cp) + 16;
+	uuid_t uuid;
+	uint128_t uint128;
+	uuid_t uuid128;
+	uint8_t type;
+
+	if (index == MGMT_INDEX_NONE)
+		index = 0;
+
+	cp = (void *) buf;
+	cp->rssi_threshold = -127;
+
+	type = 0;
+	hci_set_bit(BDADDR_BREDR, &type);
+	hci_set_bit(BDADDR_LE_PUBLIC, &type);
+	hci_set_bit(BDADDR_LE_RANDOM, &type);
+
+	if (argc == 1) {
+		find_service_usage();
+		exit(EXIT_FAILURE);
+	}
+
+	while ((opt = getopt_long(argc, argv, "+lbu:r:p:h",
+				  find_service_options, NULL)) != -1) {
+		switch (opt) {
+		case 'l':
+			hci_clear_bit(BDADDR_BREDR, &type);
+			hci_set_bit(BDADDR_LE_PUBLIC, &type);
+			hci_set_bit(BDADDR_LE_RANDOM, &type);
+			break;
+		case 'b':
+			hci_set_bit(BDADDR_BREDR, &type);
+			hci_clear_bit(BDADDR_LE_PUBLIC, &type);
+			hci_clear_bit(BDADDR_LE_RANDOM, &type);
+			break;
+		case 'u':
+			if (bt_string2uuid(&uuid, optarg) < 0) {
+				printf("Invalid UUID: %s\n", optarg);
+				exit(EXIT_FAILURE);
+			}
+			uuid_to_uuid128(&uuid128, &uuid);
+			ntoh128((uint128_t *) uuid128.value.uuid128.data,
+				&uint128);
+			htob128(&uint128, (uint128_t *) cp->uuid);
+			break;
+		case 'r':
+			cp->rssi_threshold = atoi(optarg);
+			break;
+		case 'h':
+			find_service_usage();
+			exit(EXIT_SUCCESS);
+		default:
+			find_service_usage();
+			exit(EXIT_FAILURE);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+	optind = 0;
+
+	if (argc > 0) {
+		find_service_usage();
+		exit(EXIT_FAILURE);
+	}
+
+	cp->type = type;
+	cp->num_uuid = 1;
+
+	if (mgmt_send(mgmt, MGMT_OP_START_SERVICE_DISCOVERY, index, total_size,
+		      cp, find_service_rsp, NULL, NULL) == 0) {
+		fprintf(stderr, "Unable to send start_service_discovery cmd\n");
+		exit(EXIT_FAILURE);
+	}
+}
+
 static void find_rsp(uint8_t status, uint16_t len, const void *param,
 							void *user_data)
 {

@@ -2145,16 +2266,6 @@ static void cmd_unblock(struct mgmt *mgmt, uint16_t index, int argc,
 	}
 }
 
-static void uuid_to_uuid128(uuid_t *uuid128, const uuid_t *uuid)
-{
-	if (uuid->type == SDP_UUID16)
-		sdp_uuid16_to_uuid128(uuid128, uuid);
-	else if (uuid->type == SDP_UUID32)
-		sdp_uuid32_to_uuid128(uuid128, uuid);
-	else
-		memcpy(uuid128, uuid, sizeof(*uuid));
-}
-
 static void cmd_add_uuid(struct mgmt *mgmt, uint16_t index, int argc,
 							char **argv)
 {

@@ -2978,6 +3089,7 @@ static struct {
 	{ "disconnect", cmd_disconnect, "Disconnect device"		},
 	{ "con",	cmd_con,	"List connections"		},
 	{ "find",	cmd_find,	"Discover nearby devices"	},
+	{ "find-service", cmd_find_service, "Discover nearby service"	},
 	{ "name",	cmd_name,	"Set local name"		},
 	{ "pair",	cmd_pair,	"Pair with a remote device"	},
 	{ "cancelpair",	cmd_cancel_pair,"Cancel pairing"		},

index 7d14fe4..35d3f9c 100644
@@ -1802,6 +1802,150 @@ static const struct generic_data stop_discovery_invalid_param_test_1 = {
 	.expect_len = sizeof(stop_discovery_bredrle_invalid_param),
 };
 
+static const char start_service_discovery_invalid_param[] = { 0x00, 0x00, 0x00 };
+static const char start_service_discovery_invalid_resp[] = { 0x00 };
+static const char start_service_discovery_bredr_param[] = { 0x01, 0x00, 0x00, 0x00};
+static const char start_service_discovery_bredr_resp[] = { 0x01 };
+static const char start_service_discovery_le_param[] = { 0x06, 0x00, 0x01, 0x00,
+			0xfa, 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00,
+			0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 };
+static const char start_service_discovery_le_resp[] = { 0x06 };
+static const char start_service_discovery_bredrle_param[] = { 0x07, 0x00, 0x01, 0x00,
+			0xfa, 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00,
+			0x80, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00 };
+static const char start_service_discovery_bredrle_resp[] = { 0x07 };
+static const char start_service_discovery_valid_hci[] = { 0x01, 0x01 };
+static const char start_service_discovery_evt[] = { 0x07, 0x01 };
+static const char start_service_discovery_le_evt[] = { 0x06, 0x01 };
+
+static const struct generic_data start_service_discovery_not_powered_test_1 = {
+	.send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.send_param = start_service_discovery_bredr_param,
+	.send_len = sizeof(start_service_discovery_bredr_param),
+	.expect_status = MGMT_STATUS_NOT_POWERED,
+	.expect_param = start_service_discovery_bredr_resp,
+	.expect_len = sizeof(start_service_discovery_bredr_resp),
+};
+
+static const struct generic_data start_service_discovery_invalid_param_test_1 = {
+	.setup_settings = settings_powered,
+	.send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.send_param = start_service_discovery_invalid_param,
+	.send_len = sizeof(start_service_discovery_invalid_param),
+	.expect_status = MGMT_STATUS_INVALID_PARAMS,
+	.expect_param = start_service_discovery_invalid_resp,
+	.expect_len = sizeof(start_service_discovery_invalid_resp),
+};
+
+static const struct generic_data start_service_discovery_not_supported_test_1 = {
+	.setup_settings = settings_powered,
+	.send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.send_param = start_service_discovery_le_param,
+	.send_len = sizeof(start_service_discovery_le_param),
+	.expect_status = MGMT_STATUS_REJECTED,
+	.expect_param = start_service_discovery_le_resp,
+	.expect_len = sizeof(start_service_discovery_le_resp),
+};
+
+static const struct generic_data start_service_discovery_valid_param_test_1 = {
+	.setup_settings = settings_powered_le,
+	.send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.send_param = start_service_discovery_bredrle_param,
+	.send_len = sizeof(start_service_discovery_bredrle_param),
+	.expect_status = MGMT_STATUS_SUCCESS,
+	.expect_param = start_service_discovery_bredrle_resp,
+	.expect_len = sizeof(start_service_discovery_bredrle_resp),
+	.expect_hci_command = BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+	.expect_hci_param = start_service_discovery_valid_hci,
+	.expect_hci_len = sizeof(start_service_discovery_valid_hci),
+	.expect_alt_ev = MGMT_EV_DISCOVERING,
+	.expect_alt_ev_param = start_service_discovery_evt,
+	.expect_alt_ev_len = sizeof(start_service_discovery_evt),
+};
+
+static const struct generic_data start_service_discovery_valid_param_test_2 = {
+	.setup_settings = settings_powered,
+	.send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.send_param = start_service_discovery_le_param,
+	.send_len = sizeof(start_service_discovery_le_param),
+	.expect_status = MGMT_STATUS_SUCCESS,
+	.expect_param = start_service_discovery_le_resp,
+	.expect_len = sizeof(start_service_discovery_le_resp),
+	.expect_hci_command = BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+	.expect_hci_param = start_service_discovery_valid_hci,
+	.expect_hci_len = sizeof(start_service_discovery_valid_hci),
+	.expect_alt_ev = MGMT_EV_DISCOVERING,
+	.expect_alt_ev_param = start_service_discovery_le_evt,
+	.expect_alt_ev_len = sizeof(start_service_discovery_le_evt),
+};
+
+static const char stop_service_discovery_bredrle_param[] = { 0x07 };
+static const char stop_service_discovery_bredrle_invalid_param[] = { 0x06 };
+static const char stop_service_discovery_valid_hci[] = { 0x00, 0x00 };
+static const char stop_service_discovery_evt[] = { 0x07, 0x00 };
+static const char stop_service_discovery_bredr_param[] = { 0x01 };
+static const char stop_service_discovery_bredr_discovering[] = { 0x01, 0x00 };
+static const char stop_service_discovery_inq_param[] = { 0x33, 0x8b, 0x9e, 0x08, 0x00 };
+
+static const struct generic_data stop_service_discovery_success_test_1 = {
+	.setup_settings = settings_powered_le,
+	.setup_send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.setup_send_param = start_service_discovery_bredrle_param,
+	.setup_send_len = sizeof(start_service_discovery_bredrle_param),
+	.send_opcode = MGMT_OP_STOP_SERVICE_DISCOVERY,
+	.send_param = stop_service_discovery_bredrle_param,
+	.send_len = sizeof(stop_service_discovery_bredrle_param),
+	.expect_status = MGMT_STATUS_SUCCESS,
+	.expect_param = stop_service_discovery_bredrle_param,
+	.expect_len = sizeof(stop_service_discovery_bredrle_param),
+	.expect_hci_command = BT_HCI_CMD_LE_SET_SCAN_ENABLE,
+	.expect_hci_param = stop_service_discovery_valid_hci,
+	.expect_hci_len = sizeof(stop_service_discovery_valid_hci),
+	.expect_alt_ev = MGMT_EV_DISCOVERING,
+	.expect_alt_ev_param = stop_service_discovery_evt,
+	.expect_alt_ev_len = sizeof(stop_service_discovery_evt),
+};
+
+static const struct generic_data stop_service_discovery_bredr_success_test_1 = {
+	.setup_settings = settings_powered,
+	.setup_send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.setup_send_param = start_service_discovery_bredr_param,
+	.setup_send_len = sizeof(start_service_discovery_bredr_param),
+	.send_opcode = MGMT_OP_STOP_SERVICE_DISCOVERY,
+	.send_param = stop_service_discovery_bredr_param,
+	.send_len = sizeof(stop_service_discovery_bredr_param),
+	.expect_status = MGMT_STATUS_SUCCESS,
+	.expect_param = stop_service_discovery_bredr_param,
+	.expect_len = sizeof(stop_service_discovery_bredr_param),
+	.expect_hci_command = BT_HCI_CMD_INQUIRY_CANCEL,
+	.expect_alt_ev = MGMT_EV_DISCOVERING,
+	.expect_alt_ev_param = stop_service_discovery_bredr_discovering,
+	.expect_alt_ev_len = sizeof(stop_service_discovery_bredr_discovering),
+};
+
+static const struct generic_data stop_service_discovery_rejected_test_1 = {
+	.setup_settings = settings_powered_le,
+	.send_opcode = MGMT_OP_STOP_SERVICE_DISCOVERY,
+	.send_param = stop_service_discovery_bredrle_param,
+	.send_len = sizeof(stop_service_discovery_bredrle_param),
+	.expect_status = MGMT_STATUS_REJECTED,
+	.expect_param = stop_service_discovery_bredrle_param,
+	.expect_len = sizeof(stop_service_discovery_bredrle_param),
+};
+
+static const struct generic_data stop_service_discovery_invalid_param_test_1 = {
+	.setup_settings = settings_powered_le,
+	.setup_send_opcode = MGMT_OP_START_SERVICE_DISCOVERY,
+	.setup_send_param = start_service_discovery_bredrle_param,
+	.setup_send_len = sizeof(start_service_discovery_bredrle_param),
+	.send_opcode = MGMT_OP_STOP_SERVICE_DISCOVERY,
+	.send_param = stop_service_discovery_bredrle_invalid_param,
+	.send_len = sizeof(stop_service_discovery_bredrle_invalid_param),
+	.expect_status = MGMT_STATUS_INVALID_PARAMS,
+	.expect_param = stop_service_discovery_bredrle_invalid_param,
+	.expect_len = sizeof(stop_service_discovery_bredrle_invalid_param),
+};
+
 static const char set_dev_class_valid_param[] = { 0x01, 0x0c };
 static const char set_dev_class_zero_rsp[] = { 0x00, 0x00, 0x00 };
 static const char set_dev_class_valid_rsp[] = { 0x0c, 0x01, 0x00 };

@@ -4568,6 +4712,38 @@ int main(int argc, char *argv[])
 				&stop_discovery_invalid_param_test_1,
 				setup_start_discovery, test_command_generic);
 
+	test_bredrle("Start Service Discovery - Not powered 1",
+				&start_service_discovery_not_powered_test_1,
+				NULL, test_command_generic);
+	test_bredrle("Start Service Discovery - Invalid parameters 1",
+				&start_service_discovery_invalid_param_test_1,
+				NULL, test_command_generic);
+	test_bredrle("Start Service Discovery - Not supported 1",
+				&start_service_discovery_not_supported_test_1,
+				NULL, test_command_generic);
+	test_bredrle("Start Service Discovery - Success 1",
+				&start_service_discovery_valid_param_test_1,
+				NULL, test_command_generic);
+	test_le("Start Service Discovery - Success 2",
+				&start_service_discovery_valid_param_test_2,
+				NULL, test_command_generic);
+
+	test_bredrle("Stop Service Discovery - Success 1",
+				&stop_service_discovery_success_test_1,
+				setup_start_discovery,
+				test_command_generic);
+	test_bredr("Stop Service Discovery - BR/EDR (Inquiry) Success 1",
+				&stop_service_discovery_bredr_success_test_1,
+				setup_start_discovery,
+				test_command_generic);
+	test_bredrle("Stop Service Discovery - Rejected 1",
+				&stop_service_discovery_rejected_test_1,
+				NULL, test_command_generic);
+	test_bredrle("Stop Service Discovery - Invalid parameters 1",
+				&stop_service_discovery_invalid_param_test_1,
+				setup_start_discovery,
+				test_command_generic);
+
 	test_bredrle("Set Device Class - Success 1",
 				&set_dev_class_valid_param_test_1,
 				NULL, test_command_generic);