greybus: create host device cport id map

A Greybus host device has a pool of CPort Ids it can use.  When we
establish a connection with a CPort on another module we will need
to allocate one from those that are available.

This patch adds a bitmap to the greybus host device structure that
allows cport ids to be allocated and freed as needed.

Signed-off-by: Alex Elder <elder@linaro.org>
Signed-off-by: Greg Kroah-Hartman <greg@kroah.com>

diff --git a/drivers/staging/greybus/core.c b/drivers/staging/greybus/core.c
index d9bbc67..9669a34 100644 (file)
--- a/drivers/staging/greybus/core.c
+++ b/drivers/staging/greybus/core.c
@@ -30,6 +30,8 @@ int greybus_disabled(void)
 }
 EXPORT_SYMBOL_GPL(greybus_disabled);
 
+static spinlock_t cport_id_map_lock;
+
 static int greybus_module_match(struct device *dev, struct device_driver *drv)
 {
        struct greybus_driver *driver = to_greybus_driver(dev->driver);
@@ -261,6 +263,69 @@ void greybus_remove_device(struct gb_module *gmod)
 
 static DEFINE_MUTEX(hd_mutex);
 
+/*
+ * Allocate an available CPort Id for use on the given host device.
+ * Returns the CPort Id, or CPORT_ID_BAD of none remain.
+ *
+ * The lowest-available id is returned, so the first call is
+ * guaranteed to allocate CPort Id 0.
+ */
+u16 greybus_hd_cport_id_alloc(struct greybus_host_device *hd)
+{
+       unsigned long cport_id;
+
+       /* If none left, return BAD */
+       if (hd->cport_id_count == HOST_DEV_CPORT_ID_MAX)
+               return CPORT_ID_BAD;
+
+       spin_lock_irq(&cport_id_map_lock);
+       cport_id = find_next_zero_bit(hd->cport_id_map, hd->cport_id_count,
+                                               hd->cport_id_next_free);
+       if (cport_id < hd->cport_id_count) {
+               hd->cport_id_next_free = cport_id + 1;  /* Success */
+               hd->cport_id_count++;
+       } else {
+               /* Lost a race for the last one */
+               if (hd->cport_id_count != HOST_DEV_CPORT_ID_MAX) {
+                       pr_err("bad cport_id_count in alloc");
+                       hd->cport_id_count = HOST_DEV_CPORT_ID_MAX;
+               }
+               cport_id = CPORT_ID_BAD;
+       }
+       spin_unlock_irq(&cport_id_map_lock);
+
+       return cport_id;
+}
+
+/*
+ * Free a previously-allocated CPort Id on the given host device.
+ */
+void greybus_hd_cport_id_free(struct greybus_host_device *hd, u16 cport_id)
+{
+       if (cport_id >= HOST_DEV_CPORT_ID_MAX) {
+               pr_err("bad cport_id %hu\n", cport_id);
+               return;
+       }
+       if (!hd->cport_id_count) {
+               pr_err("too many cport_id frees\n");
+               return;
+       }
+
+       spin_lock_irq(&cport_id_map_lock);
+       if (test_and_clear_bit(cport_id, hd->cport_id_map)) {
+               if (hd->cport_id_count) {
+                       hd->cport_id_count--;
+                       if (cport_id < hd->cport_id_next_free)
+                               hd->cport_id_next_free = cport_id;
+               } else {
+                       pr_err("bad cport_id_count in free");
+               }
+       } else {
+               pr_err("duplicate cport_id %hu free\n", cport_id);
+       }
+       spin_unlock_irq(&cport_id_map_lock);
+}
+
 static void free_hd(struct kref *kref)
 {
        struct greybus_host_device *hd;
@@ -284,6 +349,13 @@ struct greybus_host_device *greybus_create_hd(struct greybus_host_driver *driver
        hd->driver = driver;
        INIT_LIST_HEAD(&hd->modules);
 
+       /* Pre-allocate CPort 0 for control stuff. XXX */
+       if (greybus_hd_cport_id_alloc(hd) != 0) {
+               pr_err("couldn't allocate cport 0\n");
+               kfree(hd);
+               return NULL;
+       }
+
        return hd;
 }
 EXPORT_SYMBOL_GPL(greybus_create_hd);
@@ -299,6 +371,9 @@ static int __init gb_init(void)
 {
        int retval;
 
+       BUILD_BUG_ON(HOST_DEV_CPORT_ID_MAX >= (long)CPORT_ID_BAD);
+       spin_lock_init(&cport_id_map_lock);
+
        retval = gb_debugfs_init();
        if (retval) {
                pr_err("debugfs failed\n");

diff --git a/drivers/staging/greybus/greybus.h b/drivers/staging/greybus/greybus.h
index fabd74e..077daf0 100644 (file)
--- a/drivers/staging/greybus/greybus.h
+++ b/drivers/staging/greybus/greybus.h
@@ -18,6 +18,7 @@
 #include <linux/device.h>
 #include <linux/module.h>
 
+#include "kernel_ver.h"
 #include "greybus_id.h"
 #include "greybus_manifest.h"
 #include "manifest.h"
@@ -44,6 +45,15 @@
        .match_flags    = GREYBUS_DEVICE_ID_MATCH_SERIAL,       \
        .serial_number  = (s),
 
+/* XXX I couldn't get my Kconfig file to be noticed for out-of-tree build */
+#ifndef CONFIG_HOST_DEV_CPORT_ID_MAX
+#define CONFIG_HOST_DEV_CPORT_ID_MAX 128
+#endif /* !CONFIG_HOST_DEV_CPORT_ID_MAX */
+
+/* Maximum number of CPorts usable by a host device */
+/* XXX This should really be determined by the AP module manifest */
+#define HOST_DEV_CPORT_ID_MAX  CONFIG_HOST_DEV_CPORT_ID_MAX
+#define CPORT_ID_BAD           U16_MAX         /* UniPro max id is 4095 */
 
 /*
   gbuf
@@ -185,10 +195,18 @@ struct greybus_host_device {
        struct list_head modules;
        struct list_head connections;
 
+       spinlock_t cport_id_map_lock;
+       DECLARE_BITMAP(cport_id_map, HOST_DEV_CPORT_ID_MAX);
+       u16 cport_id_count;     /* How many have been allocated */
+       u16 cport_id_next_free; /* Where to start checking anyway */
+
        /* Private data for the host driver */
        unsigned long hd_priv[0] __attribute__ ((aligned(sizeof(s64))));
 };
 
+u16 greybus_hd_cport_id_alloc(struct greybus_host_device *hd);
+void greybus_hd_cport_id_free(struct greybus_host_device *hd, u16 cport_id);
+
 struct greybus_host_device *greybus_create_hd(struct greybus_host_driver *host_driver,
                                              struct device *parent);
 void greybus_remove_hd(struct greybus_host_device *hd);