[PATCH 2/3] of/slimbus: OF helper for SLIMbus

[Sagar Dharia <sdharia@codeaurora.org>]

OF helper routine scans the SLIMbus DeviceTree, allocates resources,
and creates slim_devices according to the hierarchy.

Signed-off-by: Sagar Dharia <sdharia@codeaurora.org>
---
 drivers/slimbus/slimbus.c | 77 +++++++++++++++++++++++++++++++++++++++++++++++
 include/linux/slimbus.h   | 19 ++++++++++++
 2 files changed, 96 insertions(+)

diff --git a/drivers/slimbus/slimbus.c b/drivers/slimbus/slimbus.c
index be4f2c7..f51b503 100644
--- a/drivers/slimbus/slimbus.c
+++ b/drivers/slimbus/slimbus.c
@@ -19,6 +19,7 @@
 #include <linux/idr.h>
 #include <linux/pm_runtime.h>
 #include <linux/slimbus.h>
+#include <linux/of.h>
 
 static DEFINE_MUTEX(slim_lock);
 static DEFINE_IDR(ctrl_idr);
@@ -761,6 +762,82 @@ int slim_get_logical_addr(struct slim_device *sb, struct slim_eaddr *e_addr,
 }
 EXPORT_SYMBOL(slim_get_logical_addr);
 
+#if IS_ENABLED(CONFIG_OF)
+/* OF helpers for SLIMbus */
+int of_register_slim_devices(struct slim_controller *ctrl)
+{
+	struct device_node *node;
+	struct slim_boardinfo *temp, *binfo = NULL;
+	int n = 0;
+	int ret = 0;
+
+	if (!ctrl->dev.of_node)
+		return -EINVAL;
+
+	for_each_child_of_node(ctrl->dev.of_node, node) {
+		struct property *prop;
+		u8 *ea;
+		struct slim_device *slim;
+		char *name;
+
+		prop = of_find_property(node, "enumeration-addr", NULL);
+		if (!prop || prop->length != 6) {
+			dev_err(&ctrl->dev, "of_slim: invalid E-addr");
+			continue;
+		}
+		ea = (u8 *)prop->value;
+		name = kcalloc(SLIMBUS_NAME_SIZE, sizeof(char), GFP_KERNEL);
+		if (!name) {
+			ret = -ENOMEM;
+			goto of_slim_err;
+		}
+		ret = of_modalias_node(node, name, SLIMBUS_NAME_SIZE);
+		if (ret < 0) {
+			dev_err(&ctrl->dev, "of_slim: modalias fail:%d on %s\n",
+				ret, node->full_name);
+			kfree(name);
+			continue;
+		}
+		slim = kcalloc(1, sizeof(struct slim_device), GFP_KERNEL);
+		if (!slim) {
+			ret = -ENOMEM;
+			kfree(name);
+			goto of_slim_err;
+		}
+		slim->e_addr.manf_id = (u16)(ea[5] << 8) | ea[4];
+		slim->e_addr.prod_code = (u16)(ea[3] << 8) | ea[2];
+		slim->e_addr.dev_index = ea[1];
+		slim->e_addr.instance = ea[0];
+
+
+		temp = krealloc(binfo, (n + 1) * sizeof(struct slim_boardinfo),
+					GFP_KERNEL);
+		if (!temp) {
+			kfree(name);
+			kfree(slim);
+			goto of_slim_err;
+		}
+		binfo = temp;
+		slim->dev.of_node = of_node_get(node);
+		slim->name = name;
+		binfo[n].bus_num = ctrl->nr;
+		binfo[n].slim_slave = slim;
+		n++;
+	}
+	return slim_register_board_info(binfo, n);
+
+of_slim_err:
+	n--;
+	while (n >= 0) {
+		kfree(binfo[n].slim_slave->name);
+		kfree(binfo[n].slim_slave);
+	}
+	kfree(binfo);
+	return ret;
+}
+EXPORT_SYMBOL(of_register_slim_devices);
+#endif
+
 MODULE_LICENSE("GPL v2");
 MODULE_VERSION("0.1");
 MODULE_DESCRIPTION("Slimbus module");
diff --git a/include/linux/slimbus.h b/include/linux/slimbus.h
index 05b7594..61b7c74 100644
--- a/include/linux/slimbus.h
+++ b/include/linux/slimbus.h
@@ -370,6 +370,25 @@ static inline int slim_register_board_info(struct slim_boardinfo const *info,
 }
 #endif
 
+#if IS_ENABLED(CONFIG_OF)
+/*
+ * of_slim_register_devices() - Register devices in the SLIMbus Device Tree
+ * @ctrl: slim_controller which devices should be registered to.
+ *
+ * This routine scans the SLIMbus Device Tree, allocating resources and
+ * creating slim_devices according to the SLIMbus Device Tree
+ * hierarchy.
+ * This routine is normally called from the probe routine of the driver
+ * registering as a slim_controller.
+ */
+extern int of_register_slim_devices(struct slim_controller *ctrl);
+#else
+static int of_register_slim_devices(struct slim_controller *ctrl)
+{
+	return 0;
+}
+#endif
+
 static inline void *slim_get_ctrldata(const struct slim_controller *dev)
 {
 	return dev_get_drvdata(&dev->dev);
-- 
1.8.2.1