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>

diff --git a/drivers/staging/greybus/endo.c b/drivers/staging/greybus/endo.c
index e47ec6963fa1bfc2e5eb07ee74dbc02199b4ff95..80d9397c565f16e5efe873faabb372346e9fe660 100644 (file)
--- 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 63665a2d80153a7d709ad43ea08e7f6d28b0ebfd..28b3c4fccf24a25b8d904f7dca78cba134719029 100644 (file)
--- 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 202f141c7fe33cd436d70d21e089528fe2d2e044..8ed96a8ba26d2a237cab2ad15a2313a04057abae 100644 (file)
--- 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 c23ac98fc1ba2847db51f76d1109e480cba82807..3b3f421b244f4739011a4b29731393acd44f4d2c 100644 (file)
--- 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 */