1 files changed, 107 insertions, 0 deletions

diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 6a5b5e777dd2..6d3dd3988d0f 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -19,10 +19,12 @@
  */
 
 #include <linux/errno.h>
+#include <linux/list.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_irq.h>
 #include <linux/string.h>
+#include <linux/slab.h>
 
 /* For archs that don't support NO_IRQ (such as x86), provide a dummy value */
 #ifndef NO_IRQ
@@ -386,3 +388,108 @@ int of_irq_to_resource_table(struct device_node *dev, struct resource *res,
 
         return i;
 }
+
+struct intc_desc {
+        struct list_head        list;
+        struct device_node      *dev;
+        struct device_node      *interrupt_parent;
+};
+
+/**
+ * of_irq_init - Scan and init matching interrupt controllers in DT
+ * @matches: 0 terminated array of nodes to match and init function to call
+ *
+ * This function scans the device tree for matching interrupt controller nodes,
+ * and calls their initialization functions in order with parents first.
+ */
+void __init of_irq_init(const struct of_device_id *matches)
+{
+        struct device_node *np, *parent = NULL;
+        struct intc_desc *desc, *temp_desc;
+        struct list_head intc_desc_list, intc_parent_list;
+
+        INIT_LIST_HEAD(&intc_desc_list);
+        INIT_LIST_HEAD(&intc_parent_list);
+
+        for_each_matching_node(np, matches) {
+                if (!of_find_property(np, "interrupt-controller", NULL))
+                        continue;
+                /*
+                 * Here, we allocate and populate an intc_desc with the node
+                 * pointer, interrupt-parent device_node etc.
+                 */
+                desc = kzalloc(sizeof(*desc), GFP_KERNEL);
+                if (WARN_ON(!desc))
+                        goto err;
+
+                desc->dev = np;
+                desc->interrupt_parent = of_irq_find_parent(np);
+                list_add_tail(&desc->list, &intc_desc_list);
+        }
+
+        /*
+         * The root irq controller is the one without an interrupt-parent.
+         * That one goes first, followed by the controllers that reference it,
+         * followed by the ones that reference the 2nd level controllers, etc.
+         */
+        while (!list_empty(&intc_desc_list)) {
+                /*
+                 * Process all controllers with the current 'parent'.
+                 * First pass will be looking for NULL as the parent.
+                 * The assumption is that NULL parent means a root controller.
+                 */
+                list_for_each_entry_safe(desc, temp_desc, &intc_desc_list, list) {
+                        const struct of_device_id *match;
+                        int ret;
+                        of_irq_init_cb_t irq_init_cb;
+
+                        if (desc->interrupt_parent != parent)
+                                continue;
+
+                        list_del(&desc->list);
+                        match = of_match_node(matches, desc->dev);
+                        if (WARN(!match->data,
+                            "of_irq_init: no init function for %s\n",
+                            match->compatible)) {
+                                kfree(desc);
+                                continue;
+                        }
+
+                        pr_debug("of_irq_init: init %s @ %p, parent %p\n",
+                                 match->compatible,
+                                 desc->dev, desc->interrupt_parent);
+                        irq_init_cb = match->data;
+                        ret = irq_init_cb(desc->dev, desc->interrupt_parent);
+                        if (ret) {
+                                kfree(desc);
+                                continue;
+                        }
+
+                        /*
+                         * This one is now set up; add it to the parent list so
+                         * its children can get processed in a subsequent pass.
+                         */
+                        list_add_tail(&desc->list, &intc_parent_list);
+                }
+
+                /* Get the next pending parent that might have children */
+                desc = list_first_entry(&intc_parent_list, typeof(*desc), list);
+                if (list_empty(&intc_parent_list) || !desc) {
+                        pr_err("of_irq_init: children remain, but no parents\n");
+                        break;
+                }
+                list_del(&desc->list);
+                parent = desc->dev;
+                kfree(desc);
+        }
+
+        list_for_each_entry_safe(desc, temp_desc, &intc_parent_list, list) {
+                list_del(&desc->list);
+                kfree(desc);
+        }
+err:
+        list_for_each_entry_safe(desc, temp_desc, &intc_desc_list, list) {
+                list_del(&desc->list);
+                kfree(desc);
+        }
+}