Add support for limited discovery mode

6 files changed, 250 insertions, 104 deletions

diff --git a/hcid/dbus-adapter.c b/hcid/dbus-adapter.c
index 3586582c..1dfe1f6c 100644
--- a/hcid/dbus-adapter.c
+++ b/hcid/dbus-adapter.c
@@ -228,27 +228,34 @@ static struct bonding_request_info *bonding_request_new(bdaddr_t *peer,
 	return bonding;
 }
 
-static const char *hcimode2str(uint8_t mode)
-{
-	const char *scan_mode;
-
-	switch (mode) {
-	case SCAN_DISABLED:
-		scan_mode = MODE_OFF;
-		break;
-	case SCAN_PAGE:
-		scan_mode = MODE_CONNECTABLE;
-		break;
-	case (SCAN_PAGE | SCAN_INQUIRY):
-		scan_mode = MODE_DISCOVERABLE;
-		break;
-	case SCAN_INQUIRY:
-		/* inquiry scan mode is not handled, return unknown */
+const char *mode2str(uint8_t mode)
+{
+	switch(mode) {
+	case MODE_OFF:
+		return "off";
+	case MODE_CONNECTABLE:
+		return "connectable";
+	case MODE_DISCOVERABLE:
+		return "discoverable";
+	case MODE_LIMITED:
+		return "limited";
 	default:
-		/* reserved */
-		scan_mode = MODE_UNKNOWN;
+		return "unknown";
 	}
-	return scan_mode;
+}
+
+uint8_t str2mode(const char *mode)
+{
+	if (strcasecmp("off", mode) == 0)
+		return MODE_OFF;
+	else if (strcasecmp("connectable", mode) == 0)
+		return MODE_CONNECTABLE;
+	else if (strcasecmp("discoverable", mode) == 0)
+		return MODE_DISCOVERABLE;
+	else if (strcasecmp("limited", mode) == 0)
+		return MODE_LIMITED;
+	else
+		return MODE_UNKNOWN;
 }
 
 static DBusHandlerResult adapter_get_info(DBusConnection *conn,
@@ -301,8 +308,9 @@ static DBusHandlerResult adapter_get_info(DBusConnection *conn,
 	dbus_message_iter_append_dict_entry(&dict, "company",
 			DBUS_TYPE_STRING, &property);
 
-	property = hcimode2str(adapter->mode);
-	dbus_message_iter_append_dict_entry(&dict, "mode", 
+	property = mode2str(adapter->mode);
+
+	dbus_message_iter_append_dict_entry(&dict, "mode",
 			DBUS_TYPE_STRING, &property);
 
 	dbus_message_iter_append_dict_entry(&dict, "discoverable_timeout",
@@ -454,7 +462,7 @@ static DBusHandlerResult adapter_get_mode(DBusConnection *conn,
 {
 	const struct adapter *adapter = data;
 	DBusMessage *reply = NULL;
-	const char *scan_mode;
+	const char *mode;
 
 	if (!dbus_message_has_signature(msg, DBUS_TYPE_INVALID_AS_STRING))
 		return error_invalid_arguments(conn, msg);
@@ -463,8 +471,9 @@ static DBusHandlerResult adapter_get_mode(DBusConnection *conn,
 	if (!reply)
 		return DBUS_HANDLER_RESULT_NEED_MEMORY;
 
-	scan_mode = hcimode2str(adapter->mode);
-	dbus_message_append_args(reply, DBUS_TYPE_STRING, &scan_mode,
+	mode = mode2str(adapter->mode);
+
+	dbus_message_append_args(reply, DBUS_TYPE_STRING, &mode,
 					DBUS_TYPE_INVALID);
 
 	return send_message_and_unref(conn, reply);
@@ -473,32 +482,38 @@ static DBusHandlerResult adapter_get_mode(DBusConnection *conn,
 static DBusHandlerResult adapter_set_mode(DBusConnection *conn,
 						DBusMessage *msg, void *data)
 {
-	const struct adapter *adapter = data;
+	struct adapter *adapter = data;
 	DBusMessage *reply;
-	const char* scan_mode;
-	uint8_t hci_mode;
-	const uint8_t current_mode = adapter->mode;
+	const char *mode;
+	uint8_t scan_enable;
+	uint8_t new_mode, current_scan = adapter->scan_enable;
 	bdaddr_t local;
-	int dd;
+	gboolean limited;
+	int err, dd;
 
 	if (!dbus_message_get_args(msg, NULL,
-				DBUS_TYPE_STRING, &scan_mode,
+				DBUS_TYPE_STRING, &mode,
 				DBUS_TYPE_INVALID))
 		return error_invalid_arguments(conn, msg);
 
-	if (!scan_mode)
+	if (!mode)
 		return error_invalid_arguments(conn, msg);
 
-	if (strcasecmp(MODE_OFF, scan_mode) == 0)
-		hci_mode = SCAN_DISABLED;
-	else if (strcasecmp(MODE_CONNECTABLE, scan_mode) == 0)
-		hci_mode = SCAN_PAGE;
-	else if (strcasecmp(MODE_DISCOVERABLE, scan_mode) == 0)
-		hci_mode = (SCAN_PAGE | SCAN_INQUIRY);
-	else
+	new_mode = str2mode(mode);
+	switch(new_mode) {
+	case MODE_OFF:
+		scan_enable = SCAN_DISABLED;
+		break;
+	case MODE_CONNECTABLE:
+		scan_enable = SCAN_PAGE;
+		break;
+	case MODE_DISCOVERABLE:
+	case MODE_LIMITED:
+		scan_enable = (SCAN_PAGE | SCAN_INQUIRY);
+		break;
+	default:
 		return error_invalid_arguments(conn, msg);
-
-	str2ba(adapter->address, &local);
+	}
 
 	dd = hci_open_dev(adapter->dev_id);
 	if (dd < 0)
@@ -507,13 +522,13 @@ static DBusHandlerResult adapter_set_mode(DBusConnection *conn,
 	if (!adapter->up &&
 			(hcid.offmode == HCID_OFFMODE_NOSCAN ||
 			 (hcid.offmode == HCID_OFFMODE_DEVDOWN &&
-			  hci_mode != SCAN_DISABLED))) {
+			  scan_enable != SCAN_DISABLED))) {
 		/* Start HCI device */
 		if (ioctl(dd, HCIDEVUP, adapter->dev_id) == 0)
 			goto done; /* on success */
 
 		if (errno != EALREADY) {
-			int err = errno;
+			err = errno;
 			error("Can't init device hci%d: %s (%d)\n",
 				adapter->dev_id, strerror(errno), errno);
 
@@ -522,7 +537,7 @@ static DBusHandlerResult adapter_set_mode(DBusConnection *conn,
 		}
 	}
 
-	if (adapter->up && hci_mode == SCAN_DISABLED &&
+	if (adapter->up && scan_enable == SCAN_DISABLED &&
 			hcid.offmode == HCID_OFFMODE_DEVDOWN) {
 		if (ioctl(dd, HCIDEVDOWN, adapter->dev_id) < 0) {
 			hci_close_dev(dd);
@@ -532,22 +547,28 @@ static DBusHandlerResult adapter_set_mode(DBusConnection *conn,
 		goto done;
 	}
 
-	/* Check if the new requested mode is different from the current */
-	if (current_mode != hci_mode) {
+	limited = (new_mode == MODE_LIMITED ? TRUE : FALSE);
+	err = set_limited_discoverable(dd, adapter->class, limited);
+	if (err < 0) {
+		hci_close_dev(dd);
+		return error_failed(conn, msg, -err);
+	}
+
+	if (current_scan != scan_enable) {
 		struct hci_request rq;
 		uint8_t status = 0;
 
 		memset(&rq, 0, sizeof(rq));
 		rq.ogf    = OGF_HOST_CTL;
 		rq.ocf    = OCF_WRITE_SCAN_ENABLE;
-		rq.cparam = &hci_mode;
-		rq.clen   = sizeof(hci_mode);
+		rq.cparam = &scan_enable;
+		rq.clen   = sizeof(scan_enable);
 		rq.rparam = &status;
 		rq.rlen   = sizeof(status);
 		rq.event = EVT_CMD_COMPLETE;
 
 		if (hci_send_req(dd, &rq, 1000) < 0) {
-			int err = errno;
+			err = errno;
 			error("Sending write scan enable command failed: %s (%d)",
 					strerror(errno), errno);
 			hci_close_dev(dd);
@@ -560,10 +581,27 @@ static DBusHandlerResult adapter_set_mode(DBusConnection *conn,
 			hci_close_dev(dd);
 			return error_failed(conn, msg, bt_error(status));
 		}
-	}
+	} else {
+		/* discoverable or limited */
+		if ((scan_enable & SCAN_INQUIRY) && (new_mode != adapter->mode)) {
+			dbus_connection_emit_signal(conn,
+					dbus_message_get_path(msg),
+					ADAPTER_INTERFACE,
+					"ModeChanged",
+					DBUS_TYPE_STRING, &mode,
+					DBUS_TYPE_INVALID);
+
+			if (adapter->timeout_id)
+				g_source_remove(adapter->timeout_id);
 
+			if (adapter->discov_timeout != 0)
+				adapter->timeout_id = g_timeout_add(adapter->discov_timeout * 1000,
+						discov_timeout_handler, adapter);
+		}
+	}
 done:
-	write_device_mode(&local, scan_mode);
+	str2ba(adapter->address, &local);
+	write_device_mode(&local, mode);
 
 	hci_close_dev(dd);
 
@@ -571,6 +609,8 @@ done:
 	if (!reply)
 		return DBUS_HANDLER_RESULT_NEED_MEMORY;
 
+	adapter->mode = new_mode;
+
 	return send_message_and_unref(conn, reply);
 }
 
@@ -620,7 +660,7 @@ static DBusHandlerResult adapter_set_discoverable_to(DBusConnection *conn,
 		adapter->timeout_id = 0;
 	}
 
-	if ((timeout != 0) && (adapter->mode & SCAN_INQUIRY))
+	if ((timeout != 0) && (adapter->scan_enable & SCAN_INQUIRY))
 		adapter->timeout_id = g_timeout_add(timeout * 1000,
 						discov_timeout_handler,
 						adapter);
@@ -644,13 +684,13 @@ static DBusHandlerResult adapter_is_connectable(DBusConnection *conn,
 {
 	const struct adapter *adapter = data;
 	DBusMessage *reply;
-	const uint8_t hci_mode = adapter->mode;
+	const uint8_t scan_enable = adapter->scan_enable;
 	dbus_bool_t connectable = FALSE;
 
 	if (!dbus_message_has_signature(msg, DBUS_TYPE_INVALID_AS_STRING))
 		return error_invalid_arguments(conn, msg);
 
-	if (hci_mode & SCAN_PAGE)
+	if (scan_enable & SCAN_PAGE)
 		connectable = TRUE;
 
 	reply = dbus_message_new_method_return(msg);
@@ -668,13 +708,13 @@ static DBusHandlerResult adapter_is_discoverable(DBusConnection *conn,
 {
 	const struct adapter *adapter = data;
 	DBusMessage *reply;
-	const uint8_t hci_mode = adapter->mode;
+	const uint8_t scan_enable = adapter->scan_enable;
 	dbus_bool_t discoverable = FALSE;
 
 	if (!dbus_message_has_signature(msg, DBUS_TYPE_INVALID_AS_STRING))
 		return error_invalid_arguments(conn, msg);
 
-	if (hci_mode & SCAN_INQUIRY)
+	if (scan_enable & SCAN_INQUIRY)
 		discoverable = TRUE;
 
 	reply = dbus_message_new_method_return(msg);
@@ -1897,7 +1937,7 @@ static DBusHandlerResult adapter_dc_remote_device(DBusConnection *conn,
 	if (!l)
 		return error_not_connected(conn, msg);
 
-	if(adapter->pending_dc)
+	if (adapter->pending_dc)
 		return error_disconnect_in_progress(conn, msg);
 
 	adapter->pending_dc = g_new0(struct pending_dc_info, 1);
@@ -1908,7 +1948,7 @@ static DBusHandlerResult adapter_dc_remote_device(DBusConnection *conn,
 			      dc_pending_timeout_handler,
 			      adapter);
 
-	if(!adapter->pending_dc->timeout_id) {
+	if (!adapter->pending_dc->timeout_id) {
 		g_free(adapter->pending_dc);
 		adapter->pending_dc = NULL;
 		return DBUS_HANDLER_RESULT_NEED_MEMORY;
diff --git a/hcid/dbus-adapter.h b/hcid/dbus-adapter.h
index 2c75c5dc..c87a9829 100644
--- a/hcid/dbus-adapter.h
+++ b/hcid/dbus-adapter.h
@@ -90,7 +90,8 @@ struct adapter {
 	char address[18];		/* adapter Bluetooth Address */
 	guint timeout_id;		/* discoverable timeout id */
 	uint32_t discov_timeout;	/* discoverable time(msec) */
-	uint8_t mode;			/* scan mode */
+	uint8_t scan_enable;		/* scan mode: SCAN_DISABLED, SCAN_PAGE, SCAN_INQUIRY */
+	uint8_t mode;			/* off, connectable, discoverable, limited */
 	uint8_t class[3];		/* device class */
 	int discov_active;		/* standard discovery active: includes name resolution step */
 	int pdiscov_active;		/* periodic discovery active */
@@ -117,6 +118,8 @@ const char *major_class_str(uint32_t class);
 
 const char *minor_class_str(uint32_t class);
 
+const char *mode2str(uint8_t mode);
+
 GSList *service_classes_str(uint32_t class);
 
 int pending_remote_name_cancel(struct adapter *adapter);
diff --git a/hcid/dbus-hci.c b/hcid/dbus-hci.c
index 10e61a0c..fa84053f 100644
--- a/hcid/dbus-hci.c
+++ b/hcid/dbus-hci.c
@@ -262,24 +262,34 @@ static dbus_bool_t send_adapter_signal(DBusConnection *conn, int devid,
 	return ret;
 }
 
-static void adapter_mode_changed(struct adapter *adapter, const char *path, uint8_t mode)
+static void adapter_mode_changed(struct adapter *adapter,
+			const char *path, uint8_t scan_enable)
 {
-	const char *scan_mode;
+	const char *mode;
 
-	adapter->mode = mode;
+	adapter->scan_enable = scan_enable;
 
-	switch (mode) {
+	switch (scan_enable) {
 	case SCAN_DISABLED:
-		scan_mode = MODE_OFF;
-		break; 
+		mode = "off";
+		adapter->mode = MODE_OFF;
+		break;
 	case SCAN_PAGE:
-		scan_mode = MODE_CONNECTABLE;
+		mode = "connectable";
+		adapter->mode = MODE_CONNECTABLE;
 		break;
 	case (SCAN_PAGE | SCAN_INQUIRY):
-		scan_mode = MODE_DISCOVERABLE;
+
 		if (adapter->discov_timeout != 0)
 			adapter->timeout_id = g_timeout_add(adapter->discov_timeout * 1000,
 					discov_timeout_handler, adapter);
+
+		if (adapter->mode == MODE_LIMITED) {
+			mode = "limited";
+		} else {
+			adapter->mode = MODE_DISCOVERABLE;
+			mode = "discoverable";
+		}
 		break;
 	case SCAN_INQUIRY:
 		/* Address the scenario where another app changed the scan mode */
@@ -294,7 +304,7 @@ static void adapter_mode_changed(struct adapter *adapter, const char *path, uint
 
 	dbus_connection_emit_signal(connection, path, ADAPTER_INTERFACE,
 					"ModeChanged",
-					DBUS_TYPE_STRING, &scan_mode,
+					DBUS_TYPE_STRING, &mode,
 					DBUS_TYPE_INVALID);
 }
 
@@ -542,7 +552,7 @@ int hcid_dbus_start_device(uint16_t id)
 	struct adapter* adapter;
 	struct hci_conn_list_req *cl = NULL;
 	struct hci_conn_info *ci;
-	uint8_t mode;
+	const char *mode;
 	int i, err, dd = -1, ret = -1;
 
 	snprintf(path, sizeof(path), "%s/hci%d", BASE_PATH, id);
@@ -570,14 +580,13 @@ int hcid_dbus_start_device(uint16_t id)
 	adapter->discov_timeout = get_discoverable_timeout(id);
 	adapter->discov_type = DISCOVER_TYPE_NONE;
 
-	mode = get_startup_mode(id);
-
 	dd = hci_open_dev(id);
 	if (dd < 0)
 		goto failed;
 
-	hci_send_cmd(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE, 1, &mode);
-
+	adapter->scan_enable = get_startup_scan(id);
+	hci_send_cmd(dd, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE,
+					1, &adapter->scan_enable);
 	/*
 	 * Get the adapter Bluetooth address
 	 */
@@ -590,7 +599,10 @@ int hcid_dbus_start_device(uint16_t id)
 	if (err < 0)
 		goto failed;
 
-	adapter_mode_changed(adapter, path, mode);
+	adapter->mode = get_startup_mode(id);
+	if (adapter->mode == MODE_LIMITED)
+		set_limited_discoverable(dd, adapter->class, TRUE);
+
 	/* 
 	 * retrieve the active connections: address the scenario where
 	 * the are active connections before the daemon've started
@@ -611,6 +623,12 @@ int hcid_dbus_start_device(uint16_t id)
 
 	ret = 0;
 
+	mode = mode2str(adapter->mode);
+	dbus_connection_emit_signal(connection, path, ADAPTER_INTERFACE,
+					"ModeChanged",
+					DBUS_TYPE_STRING, &mode,
+					DBUS_TYPE_INVALID);
+
 failed:
 	if (ret == 0 && get_default_adapter() < 0)
 		set_default_adapter(id);
@@ -640,7 +658,7 @@ int hcid_dbus_stop_device(uint16_t id)
 {
 	char path[MAX_PATH_LENGTH];
 	struct adapter *adapter;
-	const char *scan_mode = MODE_OFF;
+	const char *mode = "off";
 
 	snprintf(path, sizeof(path), "%s/hci%d", BASE_PATH, id);
 
@@ -704,11 +722,12 @@ int hcid_dbus_stop_device(uint16_t id)
 	}
 
 	send_adapter_signal(connection, adapter->dev_id, "ModeChanged",
-				DBUS_TYPE_STRING, &scan_mode,
+				DBUS_TYPE_STRING, &mode,
 				DBUS_TYPE_INVALID);
 
 	adapter->up = 0;
-	adapter->mode = SCAN_DISABLED;
+	adapter->scan_enable = SCAN_DISABLED;
+	adapter->mode = MODE_OFF;
 	adapter->discov_active = 0;
 	adapter->pdiscov_active = 0;
 	adapter->pinq_idle = 0;
@@ -1615,16 +1634,55 @@ void hcid_dbus_disconn_complete(bdaddr_t *local, uint8_t status,
 
 }
 
+int set_limited_discoverable(int dd, const uint8_t *cls, gboolean limited)
+{
+	uint32_t dev_class;
+	int err;
+	int num = (limited ? 2 : 1);
+	uint8_t lap[] = { 0x33, 0x8b, 0x9e, 0x00, 0x8b, 0x9e };
+	/*
+	 * 1: giac
+	 * 2: giac + liac
+	 */
+	if (hci_write_current_iac_lap(dd, num, lap, 1000) < 0) {
+		err = errno;
+		error("Can't write current IAC LAP: %s(%d)",
+				strerror(err), err);
+		return -err;
+	}
+
+	if (limited) {
+		if (cls[1] & 0x20)
+			return 0; /* Already limited */
+
+		dev_class = (cls[2] << 16) | ((cls[1] | 0x20) << 8) | cls[0];
+	} else {
+		if (!(cls[1] & 0x20))
+			return 0; /* Already clear */
+
+		dev_class = (cls[2] << 16) | ((cls[1] & 0xdf) << 8) | cls[0];
+	}
+
+	if (hci_write_class_of_dev(dd, dev_class, 1000) < 0) {
+		err = errno;
+		error("Can't write class of device: %s(%d)",
+				strerror(err), err);
+		return -err;
+	}
+
+	return 0;
+}
+
 gboolean discov_timeout_handler(void *data)
 {
 	struct adapter *adapter = data;
 	struct hci_request rq;
 	int dd;
-	uint8_t hci_mode = adapter->mode;
+	uint8_t scan_enable = adapter->scan_enable;
 	uint8_t status = 0;
 	gboolean retval = TRUE;
 
-	hci_mode &= ~SCAN_INQUIRY;
+	scan_enable &= ~SCAN_INQUIRY;
 
 	dd = hci_open_dev(adapter->dev_id);
 	if (dd < 0) {
@@ -1635,8 +1693,8 @@ gboolean discov_timeout_handler(void *data)
 	memset(&rq, 0, sizeof(rq));
 	rq.ogf    = OGF_HOST_CTL;
 	rq.ocf    = OCF_WRITE_SCAN_ENABLE;
-	rq.cparam = &hci_mode;
-	rq.clen   = sizeof(hci_mode);
+	rq.cparam = &scan_enable;
+	rq.clen   = sizeof(scan_enable);
 	rq.rparam = &status;
 	rq.rlen   = sizeof(status);
 	rq.event  = EVT_CMD_COMPLETE;
@@ -1651,6 +1709,8 @@ gboolean discov_timeout_handler(void *data)
 		goto failed;
 	}
 
+	set_limited_discoverable(dd, adapter->class, FALSE);
+
 	adapter->timeout_id = 0;
 	retval = FALSE;
 
@@ -1769,7 +1829,7 @@ void hcid_dbus_setscan_enable_complete(bdaddr_t *local)
 		adapter->timeout_id = 0;
 	}
 
-	if (adapter->mode != rp.enable)
+	if (adapter->scan_enable != rp.enable)
 		adapter_mode_changed(adapter, path, rp.enable);
 
 failed:
@@ -1812,6 +1872,7 @@ void hcid_dbus_write_class_complete(bdaddr_t *local)
 
 	write_local_class(local, cls);
 	memcpy(adapter->class, cls, 3);
+
 failed:
 	if (dd >= 0)
 		hci_close_dev(dd);
diff --git a/hcid/dbus-hci.h b/hcid/dbus-hci.h
index eb81415f..a98e38d8 100644
--- a/hcid/dbus-hci.h
+++ b/hcid/dbus-hci.h
@@ -73,6 +73,8 @@ int found_device_add(GSList **list, bdaddr_t *bdaddr, int8_t rssi,
 			name_status_t name_status);
 int found_device_req_name(struct adapter *dbus_data);
 
+int set_limited_discoverable(int dd, const uint8_t *cls, gboolean limited);
+
 int discov_timeout_handler(void *data);
 
 void set_dbus_connection(DBusConnection *conn);
diff --git a/hcid/hcid.h b/hcid/hcid.h
index 50451a6b..a16ecd58 100644
--- a/hcid/hcid.h
+++ b/hcid/hcid.h
@@ -40,19 +40,6 @@
 /* When all services should trust a remote device */
 #define GLOBAL_TRUST "[all]"
 
-/*
- * Scanning modes, used by DEV_SET_MODE
- * off: remote devices are not allowed to find or connect to this device
- * connectable: remote devices are allowed to connect, but they are not
- *              allowed to find it.
- * discoverable: remote devices are allowed to connect and find this device
- * unknown: reserved to not allowed/future modes
- */
-#define MODE_OFF		"off"
-#define MODE_CONNECTABLE	"connectable"
-#define MODE_DISCOVERABLE	"discoverable"
-#define MODE_UNKNOWN		"unknown"
-
 enum {
 	HCID_SET_NAME,
 	HCID_SET_CLASS,
@@ -65,6 +52,22 @@ enum {
 	HCID_SET_LP,
 };
 
+/*
+ * Scanning modes, used by DEV_SET_MODE
+ * off: remote devices are not allowed to find or connect to this device
+ * connectable: remote devices are allowed to connect, but they are not
+ *              allowed to find it.
+ * discoverable: remote devices are allowed to connect and find this device
+ * limited: limited discoverable - GIAC + IAC enabled and set limited
+ *          bit on device class.
+ */
+
+#define MODE_OFF		0x00
+#define MODE_CONNECTABLE	0x01
+#define MODE_DISCOVERABLE	0x02
+#define MODE_LIMITED		0x03
+#define MODE_UNKNOWN		0xff
+
 struct device_opts {
 	unsigned long flags;
 	char    *name;
@@ -76,6 +79,7 @@ struct device_opts {
 	uint16_t link_mode;
 	uint16_t link_policy;
 	uint8_t  scan;
+	uint8_t  mode;
 	uint32_t discovto;
 };
 
@@ -140,6 +144,7 @@ int read_config(char *file);
 
 struct device_opts *alloc_device_opts(char *ref);
 
+uint8_t get_startup_scan(int hdev);
 uint8_t get_startup_mode(int hdev);
 int get_discoverable_timeout(int dev_id);
 
diff --git a/hcid/main.c b/hcid/main.c
index c73c03e0..850f1086 100644
--- a/hcid/main.c
+++ b/hcid/main.c
@@ -65,6 +65,7 @@ static inline void init_device_defaults(struct device_opts *device_opts)
 {
 	memset(device_opts, 0, sizeof(*device_opts));
 	device_opts->scan = SCAN_PAGE;
+	device_opts->mode = MODE_CONNECTABLE;
 	device_opts->name = g_strdup("BlueZ");
 	device_opts->discovto = HCID_DEFAULT_DISCOVERABLE_TIMEOUT;
 }
@@ -143,7 +144,7 @@ static struct device_opts *get_device_opts(int hdev)
 	return device_opts;
 }
 
-uint8_t get_startup_mode(int hdev)
+static struct device_opts *get_opts(int hdev)
 {
 	struct device_opts *device_opts = NULL;
 	struct hci_dev_info di;
@@ -151,7 +152,7 @@ uint8_t get_startup_mode(int hdev)
 	int sock;
 
 	if (hdev < 0)
-		return SCAN_DISABLED;
+		return NULL;
 
 	sock = hci_open_dev(hdev);
 	if (sock < 0)
@@ -177,9 +178,27 @@ no_address:
 	if (!device_opts)
 		device_opts = &default_device;
 
+	return device_opts;
+}
+
+uint8_t get_startup_scan(int hdev)
+{
+	struct device_opts *device_opts = get_opts(hdev);
+	if (!device_opts)
+		return SCAN_DISABLED;
+
 	return device_opts->scan;
 }
 
+uint8_t get_startup_mode(int hdev)
+{
+	struct device_opts *device_opts = get_opts(hdev);
+	if (!device_opts)
+		return MODE_OFF;
+
+	return device_opts->mode;
+}
+
 int get_discoverable_timeout(int hdev)
 {
 	struct device_opts *device_opts = NULL;
@@ -326,16 +345,30 @@ static void configure_device(int dev_id)
 
 	/* Set scan mode */
 	if (read_device_mode(&di.bdaddr, mode, sizeof(mode)) == 0) {
-		if (!strcmp(mode, MODE_OFF) && hcid.offmode == HCID_OFFMODE_NOSCAN)
+		if (!strcmp(mode, "off") && hcid.offmode == HCID_OFFMODE_NOSCAN) {
+			device_opts->mode = MODE_OFF;
 			device_opts->scan = SCAN_DISABLED;
-		else if (!strcmp(mode, MODE_CONNECTABLE))
+		} else if (!strcmp(mode, "connectable")) {
+			device_opts->mode = MODE_CONNECTABLE;
 			device_opts->scan = SCAN_PAGE;
-		else if (!strcmp(mode, MODE_DISCOVERABLE)) {
+		} else if (!strcmp(mode, "discoverable")) {
+			/* Set discoverable only if timeout is 0 */
+			if (!get_discoverable_timeout(dev_id)) {
+				device_opts->scan = SCAN_PAGE | SCAN_INQUIRY;
+				device_opts->mode = MODE_DISCOVERABLE;
+			} else {
+				device_opts->scan = SCAN_PAGE;
+				device_opts->mode = MODE_CONNECTABLE;
+			}
+		} else if (!strcmp(mode, "limited")) {
 			/* Set discoverable only if timeout is 0 */
-			if (!get_discoverable_timeout(dev_id))
+			if (!get_discoverable_timeout(dev_id)) {
 				device_opts->scan = SCAN_PAGE | SCAN_INQUIRY;
-			else
+				device_opts->mode = MODE_LIMITED;
+			} else {
 				device_opts->scan = SCAN_PAGE;
+				device_opts->mode = MODE_CONNECTABLE;
+			}
 		}
 	}
 
@@ -431,9 +464,11 @@ static void configure_device(int dev_id)
 		if (read_local_class(&di.bdaddr, cls) < 0) {
 			class = htobl(device_opts->class);
 			memcpy(cp.dev_class, &class, 3);
-		} else
+		} else {
+			if (!(device_opts->scan & SCAN_INQUIRY))
+				cls[1] &= 0xdf; /* Clear discoverable bit */
 			memcpy(cp.dev_class, cls, 3);
-
+		}
 		hci_send_cmd(dd, OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV,
 					WRITE_CLASS_OF_DEV_CP_SIZE, &cp);
 	}
@@ -500,7 +535,7 @@ static void init_device(int dev_id)
 		char mode[16];
 
 		if (read_device_mode(&di.bdaddr, mode, sizeof(mode)) == 0 &&
-						strcmp(mode, MODE_OFF) == 0) {
+						strcmp(mode, "off") == 0) {
 			ioctl(dd, HCIDEVDOWN, dev_id);
 			exit(0);
 		}