greybus: endo: Create modules after validating Endo ID

We already have code to parse Endo ID, lets use it to create modules at
run time instead of creating them from a static array.

Signed-off-by: Viresh Kumar <viresh.kumar@linaro.org>
Reviewed-by: Alex Elder <elder@linaro.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@google.com>

4 files changed, 66 insertions, 57 deletions

diff --git a/drivers/staging/greybus/endo.c b/drivers/staging/greybus/endo.c
index e47ec6963fa1..80d9397c565f 100644
--- a/drivers/staging/greybus/endo.c
+++ b/drivers/staging/greybus/endo.c
@@ -350,45 +350,43 @@ u8 endo_get_module_id(struct gb_endo *endo, u8 interface_id)
 }
 
 /*
- * Endo "types" have different module locations, these are tables based on those
- * types that list the module ids for the different locations.
+ * Creates all possible modules for the Endo.
  *
- * List must end with 0x00 in order to properly terminate the list.
+ * We try to create modules for all possible interface IDs. If a module is
+ * already created, we skip creating it again with the help of prev_module_id.
  */
-static u8 endo_4755[] = {
-	0x01,
-	0x03,
-	0x05,
-	0x06,
-	0x07,
-	0x08,
-	0x0a,
-	0x0c,
-	0x0d,
-	0x0e,
-	0x00,
-};
-
-
 static int create_modules(struct gb_endo *endo)
 {
 	struct gb_module *module;
-	u8 *endo_modules;
-	int i;
+	int prev_module_id = 0;
+	int interface_id;
+	int module_id;
+	int interfaces;
 
-	/* Depending on the endo id, create a bunch of different modules */
-	switch (endo->id) {
-	case 0x4755:
-		endo_modules = &endo_4755[0];
-		break;
-	default:
-		dev_err(&endo->dev, "Unknown endo id 0x%04x, aborting!",
-			endo->id);
-		return -EINVAL;
-	}
+	/*
+	 * Total number of interfaces:
+	 * - Front: 4
+	 * - Back:
+	 *   - Left: max_ribs + 1
+	 *   - Right: max_ribs + 1
+	 */
+	interfaces = 4 + (endo->layout.max_ribs + 1) * 2;
+
+	/* Find module corresponding to each interface */
+	for (interface_id = 1; interface_id <= interfaces; interface_id++) {
+		module_id = endo_get_module_id(endo, interface_id);
+
+		if (WARN_ON(!module_id))
+			continue;
+
+		/* Skip already created modules */
+		if (module_id == prev_module_id)
+			continue;
+
+		prev_module_id = module_id;
 
-	for (i = 0; endo_modules[i] != 0x00; ++i) {
-		module = gb_module_create(&endo->dev, endo_modules[i]);
+		/* New module, create it */
+		module = gb_module_create(&endo->dev, module_id);
 		if (!module)
 			return -EINVAL;
 	}
@@ -396,15 +394,11 @@ static int create_modules(struct gb_endo *endo)
 	return 0;
 }
 
-struct gb_endo *gb_endo_create(struct greybus_host_device *hd)
+static int gb_endo_register(struct greybus_host_device *hd,
+			    struct gb_endo *endo)
 {
-	struct gb_endo *endo;
 	int retval;
 
-	endo = kzalloc(sizeof(*endo), GFP_KERNEL);
-	if (!endo)
-		return NULL;
-
 	endo->dev.parent = hd->parent;
 	endo->dev.bus = &greybus_bus_type;
 	endo->dev.type = &greybus_endo_type;
@@ -412,12 +406,11 @@ struct gb_endo *gb_endo_create(struct greybus_host_device *hd)
 	endo->dev.dma_mask = hd->parent->dma_mask;
 	device_initialize(&endo->dev);
 
-	// FIXME - determine endo "id" from the SVC
-	// Also get the version and serial number from the SVC, right now we are
+	// FIXME
+	// Get the version and serial number from the SVC, right now we are
 	// using "fake" numbers.
 	strcpy(&endo->svc.serial_number[0], "042");
 	strcpy(&endo->svc.version[0], "0.0");
-	endo->id = 0x4755;
 
 	dev_set_name(&endo->dev, "endo-0x%04x", endo->id);
 	retval = device_add(&endo->dev);
@@ -425,10 +418,32 @@ struct gb_endo *gb_endo_create(struct greybus_host_device *hd)
 		dev_err(hd->parent, "failed to add endo device of id 0x%04x\n",
 			endo->id);
 		put_device(&endo->dev);
-		kfree(endo);
-		return NULL;
 	}
 
+	return retval;
+}
+
+struct gb_endo *gb_endo_create(struct greybus_host_device *hd)
+{
+	struct gb_endo *endo;
+	int retval;
+	u16 endo_id = 0x4755; // FIXME - get endo "ID" from the SVC
+
+	endo = kzalloc(sizeof(*endo), GFP_KERNEL);
+	if (!endo)
+		return NULL;
+
+	/* First check if the value supplied is a valid endo id */
+	if (gb_validate_endo_id(hd, &endo->layout, endo_id))
+		goto free_endo;
+
+	endo->id = endo_id;
+
+	/* Register Endo device */
+	if (gb_endo_register(hd, endo))
+		goto free_endo;
+
+	/* Create modules/interfaces */
 	retval = create_modules(endo);
 	if (retval) {
 		gb_endo_remove(endo);
@@ -436,6 +451,10 @@ struct gb_endo *gb_endo_create(struct greybus_host_device *hd)
 	}
 
 	return endo;
+
+free_endo:
+	kfree(endo);
+	return NULL;
 }
 
 void gb_endo_remove(struct gb_endo *endo)
diff --git a/drivers/staging/greybus/interface.c b/drivers/staging/greybus/interface.c
index 63665a2d8015..28b3c4fccf24 100644
--- a/drivers/staging/greybus/interface.c
+++ b/drivers/staging/greybus/interface.c
@@ -92,7 +92,7 @@ static struct gb_interface *gb_interface_create(struct greybus_host_device *hd,
 		return NULL;
 	}
 
-	module = gb_module_find(hd, get_module_id(interface_id));
+	module = gb_module_find(hd, endo_get_module_id(hd->endo, interface_id));
 	if (!module)
 		return NULL;
 
diff --git a/drivers/staging/greybus/module.c b/drivers/staging/greybus/module.c
index 202f141c7fe3..8ed96a8ba26d 100644
--- a/drivers/staging/greybus/module.c
+++ b/drivers/staging/greybus/module.c
@@ -83,17 +83,6 @@ struct device_type greybus_module_type = {
 	.release =	greybus_module_release,
 };
 
-u8 get_module_id(u8 interface_id)
-{
-	/*
-	 * FIXME:
-	 *
-	 * We should be able to find it from Endo ID passed during greybus
-	 * control operation, while setting up AP.
-	 */
-	return interface_id;
-}
-
 struct module_find {
 	struct gb_endo *endo;
 	u8 module_id;
@@ -125,6 +114,9 @@ struct gb_module *gb_module_find(struct greybus_host_device *hd, u8 module_id)
 	struct gb_module *module = NULL;
 	struct module_find find;
 
+	if (!module_id)
+		return NULL;
+
 	find.module_id = module_id;
 	find.endo = hd->endo;
 
diff --git a/drivers/staging/greybus/module.h b/drivers/staging/greybus/module.h
index c23ac98fc1ba..3b3f421b244f 100644
--- a/drivers/staging/greybus/module.h
+++ b/drivers/staging/greybus/module.h
@@ -23,6 +23,4 @@ struct gb_module *gb_module_find(struct greybus_host_device *hd, u8 module_id);
 struct gb_module *gb_module_create(struct device *parent, u8 module_id);
 void gb_module_remove_all(struct gb_endo *endo);
 
-u8 get_module_id(u8 interface_id);
-
 #endif /* __MODULE_H */