fix adding and setting up devices

2 files changed, 61 insertions, 48 deletions

diff --git a/hcid/device.c b/hcid/device.c
index 5c3518dd..883d6feb 100644
--- a/hcid/device.c
+++ b/hcid/device.c
@@ -99,6 +99,29 @@ void init_devices(void)
 		memset(devices + i, 0, sizeof(struct hci_dev));
 }
 
+static int device_read_bdaddr(uint16_t dev_id, bdaddr_t *bdaddr)
+{
+	int dd;
+
+	dd = hci_open_dev(dev_id);
+	if (dd < 0) {
+		error("Can't open device hci%d",
+		      dev_id, strerror(errno), errno);
+		return -errno;
+	}
+
+	if (hci_read_bd_addr(dd, bdaddr, 2000) < 0) {
+		error("Can't read address for hci%d: %s (%d)",
+		      dev_id, strerror(errno), errno);
+		hci_close_dev(dd);
+		return -errno;
+	}
+
+	hci_close_dev(dd);
+
+	return 0;
+}
+
 int add_device(uint16_t dev_id)
 {
 	struct hci_dev *dev;
@@ -118,7 +141,13 @@ int add_device(uint16_t dev_id)
 		dev->ignore = 1;
 	}
 
-	bacpy(&dev->bdaddr, &di.bdaddr);
+	if (bacmp(&di.bdaddr, BDADDR_ANY))
+		bacpy(&dev->bdaddr, &di.bdaddr);
+	else {
+		int ret = device_read_bdaddr(dev_id, &dev->bdaddr);
+		if (ret < 0)
+			return ret;
+	}
 	memcpy(dev->features, di.features, 8);
 
 	info("Device hci%d has been added", dev_id);
@@ -238,7 +267,6 @@ int stop_device(uint16_t dev_id)
 int get_device_address(uint16_t dev_id, char *address, size_t size)
 {
 	struct hci_dev *dev;
-	int dd;
 
 	ASSERT_DEV_ID;
 
@@ -247,24 +275,6 @@ int get_device_address(uint16_t dev_id, char *address, size_t size)
 
 	dev = &devices[dev_id];
 
-	if (bacmp(&dev->bdaddr, BDADDR_ANY))
-		return ba2str(&dev->bdaddr, address);
-
-	dd = hci_open_dev(dev_id);
-	if (dd < 0) {
-		error("Can't open device hci%d",
-					dev_id, strerror(errno), errno);
-		return -errno;
-	}
-
-	if (hci_read_bd_addr(dd, &dev->bdaddr, 2000) < 0) {
-		error("Can't read address for hci%d: %s (%d)",
-					dev_id, strerror(errno), errno);
-		return -errno;
-	}
-
-	hci_close_dev(dd);
-
 	return ba2str(&dev->bdaddr, address);
 }
 
diff --git a/hcid/main.c b/hcid/main.c
index be71c3ad..b15634fe 100644
--- a/hcid/main.c
+++ b/hcid/main.c
@@ -276,6 +276,8 @@ static void configure_device(int hdev)
 	if (hci_test_bit(HCI_RAW, &di.flags))
 		exit(0);
 
+	memset(&dr, 0, sizeof(dr));
+
 	dr.dev_id   = hdev;
 	device_opts = get_device_opts(s, hdev);
 
@@ -447,6 +449,24 @@ static void init_device(int hdev)
 	exit(0);
 }
 
+static void device_devreg_setup(int dev_id)
+{
+	if (hcid.auto_init)
+		init_device(dev_id);
+	hcid_dbus_register_device(dev_id);
+}
+
+static void device_devup_setup(int dev_id)
+{
+	add_device(dev_id);
+	if (hcid.auto_init)
+		configure_device(dev_id);
+	if (hcid.security)
+		start_security_manager(dev_id);
+	start_device(dev_id);
+	hcid_dbus_start_device(dev_id);
+}
+
 static void init_all_devices(int ctl)
 {
 	struct hci_dev_list_req *dl;
@@ -471,21 +491,12 @@ static void init_all_devices(int ctl)
 	}
 
 	for (i = 0; i < dl->dev_num; i++, dr++) {
-		if (hcid.auto_init)
-			init_device(dr->dev_id);
-
-		add_device(dr->dev_id);
-
-		if (hcid.auto_init && hci_test_bit(HCI_UP, &dr->dev_opt))
-			configure_device(dr->dev_id);
-
-		if (hcid.security && hci_test_bit(HCI_UP, &dr->dev_opt))
-			start_security_manager(dr->dev_id);
-
-		start_device(dr->dev_id);
-
-		hcid_dbus_register_device(dr->dev_id);
-		hcid_dbus_start_device(dr->dev_id);
+		info("HCI dev %d registered", dr->dev_id);
+		device_devreg_setup(dr->dev_id);
+		if (hci_test_bit(HCI_UP, &dr->dev_opt)) {
+			info("HCI dev %d already up", dr->dev_id);
+			device_devup_setup(dr->dev_id);
+		}
 	}
 
 	free(dl);
@@ -525,10 +536,7 @@ static inline void device_event(GIOChannel *chan, evt_stack_internal *si)
 	switch (sd->event) {
 	case HCI_DEV_REG:
 		info("HCI dev %d registered", sd->dev_id);
-		if (hcid.auto_init)
-			init_device(sd->dev_id);
-		add_device(sd->dev_id);
-		hcid_dbus_register_device(sd->dev_id);
+		device_devreg_setup(sd->dev_id);
 		break;
 
 	case HCI_DEV_UNREG:
@@ -539,12 +547,7 @@ static inline void device_event(GIOChannel *chan, evt_stack_internal *si)
 
 	case HCI_DEV_UP:
 		info("HCI dev %d up", sd->dev_id);
-		if (hcid.auto_init)
-			configure_device(sd->dev_id);
-		if (hcid.security)
-			start_security_manager(sd->dev_id);
-		start_device(sd->dev_id);
-		hcid_dbus_start_device(sd->dev_id);
+		device_devup_setup(sd->dev_id);
 		break;
 
 	case HCI_DEV_DOWN:
@@ -712,13 +715,13 @@ int main(int argc, char *argv[])
 	/* Create event loop */
 	event_loop = g_main_new(FALSE);
 
-	/* Initialize already connected devices */
-	init_all_devices(hcid.sock);
-
 	ctl_io = g_io_channel_unix_new(hcid.sock);
 
 	g_io_add_watch(ctl_io, G_IO_IN, io_stack_event, NULL);
 
+	/* Initialize already connected devices */
+	init_all_devices(hcid.sock);
+
 	if (sdp)
 		start_sdp_server();