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>

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");

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);