4 files changed, 117 insertions, 26 deletions

diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index dd5112265cc9..6d13f962f156 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -15,7 +15,7 @@ if IOMMU_SUPPORT
 
 config OF_IOMMU
        def_bool y
-       depends on OF
+       depends on OF && IOMMU_API
 
 config FSL_PAMU
         bool "Freescale IOMMU support"
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index ed8b04867b1f..02f798b7e295 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -737,7 +737,7 @@ static int add_iommu_group(struct device *dev, void *data)
         const struct iommu_ops *ops = cb->ops;
 
         if (!ops->add_device)
-                return -ENODEV;
+                return 0;
 
         WARN_ON(dev->iommu_group);
 
diff --git a/drivers/iommu/of_iommu.c b/drivers/iommu/of_iommu.c
index e550ccb7634e..af1dc6a1c0a1 100644
--- a/drivers/iommu/of_iommu.c
+++ b/drivers/iommu/of_iommu.c
@@ -18,9 +18,14 @@
  */
 
 #include <linux/export.h>
+#include <linux/iommu.h>
 #include <linux/limits.h>
 #include <linux/of.h>
 #include <linux/of_iommu.h>
+#include <linux/slab.h>
+
+static const struct of_device_id __iommu_of_table_sentinel
+        __used __section(__iommu_of_table_end);
 
 /**
  * of_get_dma_window - Parse *dma-window property and returns 0 if found.
@@ -89,3 +94,87 @@ int of_get_dma_window(struct device_node *dn, const char *prefix, int index,
         return 0;
 }
 EXPORT_SYMBOL_GPL(of_get_dma_window);
+
+struct of_iommu_node {
+        struct list_head list;
+        struct device_node *np;
+        struct iommu_ops *ops;
+};
+static LIST_HEAD(of_iommu_list);
+static DEFINE_SPINLOCK(of_iommu_lock);
+
+void of_iommu_set_ops(struct device_node *np, struct iommu_ops *ops)
+{
+        struct of_iommu_node *iommu = kzalloc(sizeof(*iommu), GFP_KERNEL);
+
+        if (WARN_ON(!iommu))
+                return;
+
+        INIT_LIST_HEAD(&iommu->list);
+        iommu->np = np;
+        iommu->ops = ops;
+        spin_lock(&of_iommu_lock);
+        list_add_tail(&iommu->list, &of_iommu_list);
+        spin_unlock(&of_iommu_lock);
+}
+
+struct iommu_ops *of_iommu_get_ops(struct device_node *np)
+{
+        struct of_iommu_node *node;
+        struct iommu_ops *ops = NULL;
+
+        spin_lock(&of_iommu_lock);
+        list_for_each_entry(node, &of_iommu_list, list)
+                if (node->np == np) {
+                        ops = node->ops;
+                        break;
+                }
+        spin_unlock(&of_iommu_lock);
+        return ops;
+}
+
+struct iommu_ops *of_iommu_configure(struct device *dev)
+{
+        struct of_phandle_args iommu_spec;
+        struct device_node *np;
+        struct iommu_ops *ops = NULL;
+        int idx = 0;
+
+        /*
+         * We don't currently walk up the tree looking for a parent IOMMU.
+         * See the `Notes:' section of
+         * Documentation/devicetree/bindings/iommu/iommu.txt
+         */
+        while (!of_parse_phandle_with_args(dev->of_node, "iommus",
+                                           "#iommu-cells", idx,
+                                           &iommu_spec)) {
+                np = iommu_spec.np;
+                ops = of_iommu_get_ops(np);
+
+                if (!ops || !ops->of_xlate || ops->of_xlate(dev, &iommu_spec))
+                        goto err_put_node;
+
+                of_node_put(np);
+                idx++;
+        }
+
+        return ops;
+
+err_put_node:
+        of_node_put(np);
+        return NULL;
+}
+
+void __init of_iommu_init(void)
+{
+        struct device_node *np;
+        const struct of_device_id *match, *matches = &__iommu_of_table;
+
+        for_each_matching_node_and_match(np, matches, &match) {
+                const of_iommu_init_fn init_fn = match->data;
+
+                if (init_fn(np))
+                        pr_err("Failed to initialise IOMMU %s\n",
+                                of_node_full_name(np));
+        }
+}
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index 3b64d0bf5bba..b89caf8c7586 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -19,6 +19,7 @@
 #include <linux/slab.h>
 #include <linux/of_address.h>
 #include <linux/of_device.h>
+#include <linux/of_iommu.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
 #include <linux/platform_device.h>
@@ -164,6 +165,9 @@ static void of_dma_configure(struct device *dev)
 {
         u64 dma_addr, paddr, size;
         int ret;
+        bool coherent;
+        unsigned long offset;
+        struct iommu_ops *iommu;
 
         /*
          * Set default dma-mask to 32 bit. Drivers are expected to setup
@@ -178,28 +182,30 @@ static void of_dma_configure(struct device *dev)
         if (!dev->dma_mask)
                 dev->dma_mask = &dev->coherent_dma_mask;
 
-        /*
-         * if dma-coherent property exist, call arch hook to setup
-         * dma coherent operations.
-         */
-        if (of_dma_is_coherent(dev->of_node)) {
-                set_arch_dma_coherent_ops(dev);
-                dev_dbg(dev, "device is dma coherent\n");
-        }
-
-        /*
-         * if dma-ranges property doesn't exist - just return else
-         * setup the dma offset
-         */
         ret = of_dma_get_range(dev->of_node, &dma_addr, &paddr, &size);
         if (ret < 0) {
-                dev_dbg(dev, "no dma range information to setup\n");
-                return;
+                dma_addr = offset = 0;
+                size = dev->coherent_dma_mask;
+        } else {
+                offset = PFN_DOWN(paddr - dma_addr);
+                dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", dev->dma_pfn_offset);
         }
+        dev->dma_pfn_offset = offset;
+
+        coherent = of_dma_is_coherent(dev->of_node);
+        dev_dbg(dev, "device is%sdma coherent\n",
+                coherent ? " " : " not ");
+
+        iommu = of_iommu_configure(dev);
+        dev_dbg(dev, "device is%sbehind an iommu\n",
+                iommu ? " " : " not ");
 
-        /* DMA ranges found. Calculate and set dma_pfn_offset */
-        dev->dma_pfn_offset = PFN_DOWN(paddr - dma_addr);
-        dev_dbg(dev, "dma_pfn_offset(%#08lx)\n", dev->dma_pfn_offset);
+        arch_setup_dma_ops(dev, dma_addr, size, iommu, coherent);
+}
+
+static void of_dma_deconfigure(struct device *dev)
+{
+        arch_teardown_dma_ops(dev);
 }
 
 /**
@@ -228,16 +234,12 @@ static struct platform_device *of_platform_device_create_pdata(
         if (!dev)
                 goto err_clear_flag;
 
-        of_dma_configure(&dev->dev);
         dev->dev.bus = &platform_bus_type;
         dev->dev.platform_data = platform_data;
-
-        /* We do not fill the DMA ops for platform devices by default.
-         * This is currently the responsibility of the platform code
-         * to do such, possibly using a device notifier
-         */
+        of_dma_configure(&dev->dev);
 
         if (of_device_add(dev) != 0) {
+                of_dma_deconfigure(&dev->dev);
                 platform_device_put(dev);
                 goto err_clear_flag;
         }