13 files changed, 803 insertions, 57 deletions

diff --git a/drivers/iommu/Kconfig b/drivers/iommu/Kconfig
index e39f9dbf297b..8fc1c7fa20b5 100644
--- a/drivers/iommu/Kconfig
+++ b/drivers/iommu/Kconfig
@@ -158,7 +158,7 @@ config TEGRA_IOMMU_GART
 
 config TEGRA_IOMMU_SMMU
         bool "Tegra SMMU IOMMU Support"
-        depends on ARCH_TEGRA_3x_SOC && TEGRA_AHB
+        depends on ARCH_TEGRA && TEGRA_AHB
         select IOMMU_API
         help
           Enables support for remapping discontiguous physical memory
@@ -187,4 +187,78 @@ config EXYNOS_IOMMU_DEBUG
 
           Say N unless you need kernel log message for IOMMU debugging
 
+config SHMOBILE_IPMMU
+        bool
+
+config SHMOBILE_IPMMU_TLB
+        bool
+
+config SHMOBILE_IOMMU
+        bool "IOMMU for Renesas IPMMU/IPMMUI"
+        default n
+        depends on (ARM && ARCH_SHMOBILE)
+        select IOMMU_API
+        select ARM_DMA_USE_IOMMU
+        select SHMOBILE_IPMMU
+        select SHMOBILE_IPMMU_TLB
+        help
+          Support for Renesas IPMMU/IPMMUI. This option enables
+          remapping of DMA memory accesses from all of the IP blocks
+          on the ICB.
+
+          Warning: Drivers (including userspace drivers of UIO
+          devices) of the IP blocks on the ICB *must* use addresses
+          allocated from the IPMMU (iova) for DMA with this option
+          enabled.
+
+          If unsure, say N.
+
+choice
+        prompt "IPMMU/IPMMUI address space size"
+        default SHMOBILE_IOMMU_ADDRSIZE_2048MB
+        depends on SHMOBILE_IOMMU
+        help
+          This option sets IPMMU/IPMMUI address space size by
+          adjusting the 1st level page table size. The page table size
+          is calculated as follows:
+
+              page table size = number of page table entries * 4 bytes
+              number of page table entries = address space size / 1 MiB
+
+          For example, when the address space size is 2048 MiB, the
+          1st level page table size is 8192 bytes.
+
+        config SHMOBILE_IOMMU_ADDRSIZE_2048MB
+                bool "2 GiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_1024MB
+                bool "1 GiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_512MB
+                bool "512 MiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_256MB
+                bool "256 MiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_128MB
+                bool "128 MiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_64MB
+                bool "64 MiB"
+
+        config SHMOBILE_IOMMU_ADDRSIZE_32MB
+                bool "32 MiB"
+
+endchoice
+
+config SHMOBILE_IOMMU_L1SIZE
+        int
+        default 8192 if SHMOBILE_IOMMU_ADDRSIZE_2048MB
+        default 4096 if SHMOBILE_IOMMU_ADDRSIZE_1024MB
+        default 2048 if SHMOBILE_IOMMU_ADDRSIZE_512MB
+        default 1024 if SHMOBILE_IOMMU_ADDRSIZE_256MB
+        default 512 if SHMOBILE_IOMMU_ADDRSIZE_128MB
+        default 256 if SHMOBILE_IOMMU_ADDRSIZE_64MB
+        default 128 if SHMOBILE_IOMMU_ADDRSIZE_32MB
+
 endif # IOMMU_SUPPORT
diff --git a/drivers/iommu/Makefile b/drivers/iommu/Makefile
index f66b816d455c..ef0e5207ad69 100644
--- a/drivers/iommu/Makefile
+++ b/drivers/iommu/Makefile
@@ -13,3 +13,5 @@ obj-$(CONFIG_OMAP_IOMMU_DEBUG) += omap-iommu-debug.o
 obj-$(CONFIG_TEGRA_IOMMU_GART) += tegra-gart.o
 obj-$(CONFIG_TEGRA_IOMMU_SMMU) += tegra-smmu.o
 obj-$(CONFIG_EXYNOS_IOMMU) += exynos-iommu.o
+obj-$(CONFIG_SHMOBILE_IOMMU) += shmobile-iommu.o
+obj-$(CONFIG_SHMOBILE_IPMMU) += shmobile-ipmmu.o
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index c1c74e030a58..f6f4a62ddf09 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -3187,8 +3187,7 @@ int __init amd_iommu_init_dma_ops(void)
 free_domains:
 
         for_each_iommu(iommu) {
-                if (iommu->default_dom)
-                        dma_ops_domain_free(iommu->default_dom);
+                dma_ops_domain_free(iommu->default_dom);
         }
 
         return ret;
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 81837b0710a9..b6ecddb63cd0 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -975,6 +975,38 @@ static void __init free_iommu_all(void)
 }
 
 /*
+ * Family15h Model 10h-1fh erratum 746 (IOMMU Logging May Stall Translations)
+ * Workaround:
+ *     BIOS should disable L2B micellaneous clock gating by setting
+ *     L2_L2B_CK_GATE_CONTROL[CKGateL2BMiscDisable](D0F2xF4_x90[2]) = 1b
+ */
+static void __init amd_iommu_erratum_746_workaround(struct amd_iommu *iommu)
+{
+        u32 value;
+
+        if ((boot_cpu_data.x86 != 0x15) ||
+            (boot_cpu_data.x86_model < 0x10) ||
+            (boot_cpu_data.x86_model > 0x1f))
+                return;
+
+        pci_write_config_dword(iommu->dev, 0xf0, 0x90);
+        pci_read_config_dword(iommu->dev, 0xf4, &value);
+
+        if (value & BIT(2))
+                return;
+
+        /* Select NB indirect register 0x90 and enable writing */
+        pci_write_config_dword(iommu->dev, 0xf0, 0x90 | (1 << 8));
+
+        pci_write_config_dword(iommu->dev, 0xf4, value | 0x4);
+        pr_info("AMD-Vi: Applying erratum 746 workaround for IOMMU at %s\n",
+                dev_name(&iommu->dev->dev));
+
+        /* Clear the enable writing bit */
+        pci_write_config_dword(iommu->dev, 0xf0, 0x90);
+}
+
+/*
  * This function clues the initialization function for one IOMMU
  * together and also allocates the command buffer and programs the
  * hardware. It does NOT enable the IOMMU. This is done afterwards.
@@ -1172,6 +1204,8 @@ static int iommu_init_pci(struct amd_iommu *iommu)
                         iommu->stored_l2[i] = iommu_read_l2(iommu, i);
         }
 
+        amd_iommu_erratum_746_workaround(iommu);
+
         return pci_enable_device(iommu->dev);
 }
 
@@ -1842,11 +1876,6 @@ static int amd_iommu_init_dma(void)
         struct amd_iommu *iommu;
         int ret;
 
-        init_device_table_dma();
-
-        for_each_iommu(iommu)
-                iommu_flush_all_caches(iommu);
-
         if (iommu_pass_through)
                 ret = amd_iommu_init_passthrough();
         else
@@ -1855,6 +1884,11 @@ static int amd_iommu_init_dma(void)
         if (ret)
                 return ret;
 
+        init_device_table_dma();
+
+        for_each_iommu(iommu)
+                iommu_flush_all_caches(iommu);
+
         amd_iommu_init_api();
 
         amd_iommu_init_notifier();
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 86e2f4a62b9a..2623a570ad2f 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -1040,7 +1040,7 @@ int dmar_enable_qi(struct intel_iommu *iommu)
 
         qi->desc = page_address(desc_page);
 
-        qi->desc_status = kmalloc(QI_LENGTH * sizeof(int), GFP_ATOMIC);
+        qi->desc_status = kzalloc(QI_LENGTH * sizeof(int), GFP_ATOMIC);
         if (!qi->desc_status) {
                 free_page((unsigned long) qi->desc);
                 kfree(qi);
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index 7fe44f83cc37..238a3caa949a 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -511,7 +511,7 @@ int exynos_sysmmu_enable(struct device *dev, unsigned long pgtable)
         return ret;
 }
 
-bool exynos_sysmmu_disable(struct device *dev)
+static bool exynos_sysmmu_disable(struct device *dev)
 {
         struct sysmmu_drvdata *data = dev_get_drvdata(dev->archdata.iommu);
         bool disabled;
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index b9d091157884..eca28014ef3e 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -4234,6 +4234,21 @@ static struct iommu_ops intel_iommu_ops = {
         .pgsize_bitmap  = INTEL_IOMMU_PGSIZES,
 };
 
+static void quirk_iommu_g4x_gfx(struct pci_dev *dev)
+{
+        /* G4x/GM45 integrated gfx dmar support is totally busted. */
+        printk(KERN_INFO "DMAR: Disabling IOMMU for graphics on this chipset\n");
+        dmar_map_gfx = 0;
+}
+
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2a40, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e00, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e10, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e20, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e30, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e40, quirk_iommu_g4x_gfx);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2e90, quirk_iommu_g4x_gfx);
+
 static void quirk_iommu_rwbf(struct pci_dev *dev)
 {
         /*
@@ -4242,12 +4257,6 @@ static void quirk_iommu_rwbf(struct pci_dev *dev)
          */
         printk(KERN_INFO "DMAR: Forcing write-buffer flush capability\n");
         rwbf_quirk = 1;
-
-        /* https://bugzilla.redhat.com/show_bug.cgi?id=538163 */
-        if (dev->revision == 0x07) {
-                printk(KERN_INFO "DMAR: Disabling IOMMU for graphics on this chipset\n");
-                dmar_map_gfx = 0;
-        }
 }
 
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_INTEL, 0x2a40, quirk_iommu_rwbf);
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index ddbdacad7768..b972d430d92b 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -734,7 +734,8 @@ int iommu_map(struct iommu_domain *domain, unsigned long iova,
         size_t orig_size = size;
         int ret = 0;
 
-        if (unlikely(domain->ops->map == NULL))
+        if (unlikely(domain->ops->unmap == NULL ||
+                     domain->ops->pgsize_bitmap == 0UL))
                 return -ENODEV;
 
         /* find out the minimum page size supported */
@@ -808,7 +809,8 @@ size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
         size_t unmapped_page, unmapped = 0;
         unsigned int min_pagesz;
 
-        if (unlikely(domain->ops->unmap == NULL))
+        if (unlikely(domain->ops->unmap == NULL ||
+                     domain->ops->pgsize_bitmap == 0UL))
                 return -ENODEV;
 
         /* find out the minimum page size supported */
@@ -850,6 +852,26 @@ size_t iommu_unmap(struct iommu_domain *domain, unsigned long iova, size_t size)
 }
 EXPORT_SYMBOL_GPL(iommu_unmap);
 
+
+int iommu_domain_window_enable(struct iommu_domain *domain, u32 wnd_nr,
+                               phys_addr_t paddr, u64 size)
+{
+        if (unlikely(domain->ops->domain_window_enable == NULL))
+                return -ENODEV;
+
+        return domain->ops->domain_window_enable(domain, wnd_nr, paddr, size);
+}
+EXPORT_SYMBOL_GPL(iommu_domain_window_enable);
+
+void iommu_domain_window_disable(struct iommu_domain *domain, u32 wnd_nr)
+{
+        if (unlikely(domain->ops->domain_window_disable == NULL))
+                return;
+
+        return domain->ops->domain_window_disable(domain, wnd_nr);
+}
+EXPORT_SYMBOL_GPL(iommu_domain_window_disable);
+
 static int __init iommu_init(void)
 {
         iommu_group_kset = kset_create_and_add("iommu_groups",
@@ -861,13 +883,15 @@ static int __init iommu_init(void)
 
         return 0;
 }
-subsys_initcall(iommu_init);
+arch_initcall(iommu_init);
 
 int iommu_domain_get_attr(struct iommu_domain *domain,
                           enum iommu_attr attr, void *data)
 {
         struct iommu_domain_geometry *geometry;
+        bool *paging;
         int ret = 0;
+        u32 *count;
 
         switch (attr) {
         case DOMAIN_ATTR_GEOMETRY:
@@ -875,6 +899,19 @@ int iommu_domain_get_attr(struct iommu_domain *domain,
                 *geometry = domain->geometry;
 
                 break;
+        case DOMAIN_ATTR_PAGING:
+                paging  = data;
+                *paging = (domain->ops->pgsize_bitmap != 0UL);
+                break;
+        case DOMAIN_ATTR_WINDOWS:
+                count = data;
+
+                if (domain->ops->domain_get_windows != NULL)
+                        *count = domain->ops->domain_get_windows(domain);
+                else
+                        ret = -ENODEV;
+
+                break;
         default:
                 if (!domain->ops->domain_get_attr)
                         return -EINVAL;
@@ -889,9 +926,26 @@ EXPORT_SYMBOL_GPL(iommu_domain_get_attr);
 int iommu_domain_set_attr(struct iommu_domain *domain,
                           enum iommu_attr attr, void *data)
 {
-        if (!domain->ops->domain_set_attr)
-                return -EINVAL;
+        int ret = 0;
+        u32 *count;
 
-        return domain->ops->domain_set_attr(domain, attr, data);
+        switch (attr) {
+        case DOMAIN_ATTR_WINDOWS:
+                count = data;
+
+                if (domain->ops->domain_set_windows != NULL)
+                        ret = domain->ops->domain_set_windows(domain, *count);
+                else
+                        ret = -ENODEV;
+
+                break;
+        default:
+                if (domain->ops->domain_set_attr == NULL)
+                        return -EINVAL;
+
+                ret = domain->ops->domain_set_attr(domain, attr, data);
+        }
+
+        return ret;
 }
 EXPORT_SYMBOL_GPL(iommu_domain_set_attr);
diff --git a/drivers/iommu/shmobile-iommu.c b/drivers/iommu/shmobile-iommu.c
new file mode 100644
index 000000000000..b6e8b57cf0a8
--- /dev/null
+++ b/drivers/iommu/shmobile-iommu.c
@@ -0,0 +1,395 @@
+/*
+ * IOMMU for IPMMU/IPMMUI
+ * Copyright (C) 2012  Hideki EIRAKU
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/dma-mapping.h>
+#include <linux/io.h>
+#include <linux/iommu.h>
+#include <linux/platform_device.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
+#include <asm/dma-iommu.h>
+#include "shmobile-ipmmu.h"
+
+#define L1_SIZE CONFIG_SHMOBILE_IOMMU_L1SIZE
+#define L1_LEN (L1_SIZE / 4)
+#define L1_ALIGN L1_SIZE
+#define L2_SIZE SZ_1K
+#define L2_LEN (L2_SIZE / 4)
+#define L2_ALIGN L2_SIZE
+
+struct shmobile_iommu_domain_pgtable {
+        uint32_t *pgtable;
+        dma_addr_t handle;
+};
+
+struct shmobile_iommu_archdata {
+        struct list_head attached_list;
+        struct dma_iommu_mapping *iommu_mapping;
+        spinlock_t attach_lock;
+        struct shmobile_iommu_domain *attached;
+        int num_attached_devices;
+        struct shmobile_ipmmu *ipmmu;
+};
+
+struct shmobile_iommu_domain {
+        struct shmobile_iommu_domain_pgtable l1, l2[L1_LEN];
+        spinlock_t map_lock;
+        spinlock_t attached_list_lock;
+        struct list_head attached_list;
+};
+
+static struct shmobile_iommu_archdata *ipmmu_archdata;
+static struct kmem_cache *l1cache, *l2cache;
+
+static int pgtable_alloc(struct shmobile_iommu_domain_pgtable *pgtable,
+                         struct kmem_cache *cache, size_t size)
+{
+        pgtable->pgtable = kmem_cache_zalloc(cache, GFP_ATOMIC);
+        if (!pgtable->pgtable)
+                return -ENOMEM;
+        pgtable->handle = dma_map_single(NULL, pgtable->pgtable, size,
+                                         DMA_TO_DEVICE);
+        return 0;
+}
+
+static void pgtable_free(struct shmobile_iommu_domain_pgtable *pgtable,
+                         struct kmem_cache *cache, size_t size)
+{
+        dma_unmap_single(NULL, pgtable->handle, size, DMA_TO_DEVICE);
+        kmem_cache_free(cache, pgtable->pgtable);
+}
+
+static uint32_t pgtable_read(struct shmobile_iommu_domain_pgtable *pgtable,
+                             unsigned int index)
+{
+        return pgtable->pgtable[index];
+}
+
+static void pgtable_write(struct shmobile_iommu_domain_pgtable *pgtable,
+                          unsigned int index, unsigned int count, uint32_t val)
+{
+        unsigned int i;
+
+        for (i = 0; i < count; i++)
+                pgtable->pgtable[index + i] = val;
+        dma_sync_single_for_device(NULL, pgtable->handle + index * sizeof(val),
+                                   sizeof(val) * count, DMA_TO_DEVICE);
+}
+
+static int shmobile_iommu_domain_init(struct iommu_domain *domain)
+{
+        struct shmobile_iommu_domain *sh_domain;
+        int i, ret;
+
+        sh_domain = kmalloc(sizeof(*sh_domain), GFP_KERNEL);
+        if (!sh_domain)
+                return -ENOMEM;
+        ret = pgtable_alloc(&sh_domain->l1, l1cache, L1_SIZE);
+        if (ret < 0) {
+                kfree(sh_domain);
+                return ret;
+        }
+        for (i = 0; i < L1_LEN; i++)
+                sh_domain->l2[i].pgtable = NULL;
+        spin_lock_init(&sh_domain->map_lock);
+        spin_lock_init(&sh_domain->attached_list_lock);
+        INIT_LIST_HEAD(&sh_domain->attached_list);
+        domain->priv = sh_domain;
+        return 0;
+}
+
+static void shmobile_iommu_domain_destroy(struct iommu_domain *domain)
+{
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+        int i;
+
+        for (i = 0; i < L1_LEN; i++) {
+                if (sh_domain->l2[i].pgtable)
+                        pgtable_free(&sh_domain->l2[i], l2cache, L2_SIZE);
+        }
+        pgtable_free(&sh_domain->l1, l1cache, L1_SIZE);
+        kfree(sh_domain);
+        domain->priv = NULL;
+}
+
+static int shmobile_iommu_attach_device(struct iommu_domain *domain,
+                                        struct device *dev)
+{
+        struct shmobile_iommu_archdata *archdata = dev->archdata.iommu;
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+        int ret = -EBUSY;
+
+        if (!archdata)
+                return -ENODEV;
+        spin_lock(&sh_domain->attached_list_lock);
+        spin_lock(&archdata->attach_lock);
+        if (archdata->attached != sh_domain) {
+                if (archdata->attached)
+                        goto err;
+                ipmmu_tlb_set(archdata->ipmmu, sh_domain->l1.handle, L1_SIZE,
+                              0);
+                ipmmu_tlb_flush(archdata->ipmmu);
+                archdata->attached = sh_domain;
+                archdata->num_attached_devices = 0;
+                list_add(&archdata->attached_list, &sh_domain->attached_list);
+        }
+        archdata->num_attached_devices++;
+        ret = 0;
+err:
+        spin_unlock(&archdata->attach_lock);
+        spin_unlock(&sh_domain->attached_list_lock);
+        return ret;
+}
+
+static void shmobile_iommu_detach_device(struct iommu_domain *domain,
+                                         struct device *dev)
+{
+        struct shmobile_iommu_archdata *archdata = dev->archdata.iommu;
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+
+        if (!archdata)
+                return;
+        spin_lock(&sh_domain->attached_list_lock);
+        spin_lock(&archdata->attach_lock);
+        archdata->num_attached_devices--;
+        if (!archdata->num_attached_devices) {
+                ipmmu_tlb_set(archdata->ipmmu, 0, 0, 0);
+                ipmmu_tlb_flush(archdata->ipmmu);
+                archdata->attached = NULL;
+                list_del(&archdata->attached_list);
+        }
+        spin_unlock(&archdata->attach_lock);
+        spin_unlock(&sh_domain->attached_list_lock);
+}
+
+static void domain_tlb_flush(struct shmobile_iommu_domain *sh_domain)
+{
+        struct shmobile_iommu_archdata *archdata;
+
+        spin_lock(&sh_domain->attached_list_lock);
+        list_for_each_entry(archdata, &sh_domain->attached_list, attached_list)
+                ipmmu_tlb_flush(archdata->ipmmu);
+        spin_unlock(&sh_domain->attached_list_lock);
+}
+
+static int l2alloc(struct shmobile_iommu_domain *sh_domain,
+                   unsigned int l1index)
+{
+        int ret;
+
+        if (!sh_domain->l2[l1index].pgtable) {
+                ret = pgtable_alloc(&sh_domain->l2[l1index], l2cache, L2_SIZE);
+                if (ret < 0)
+                        return ret;
+        }
+        pgtable_write(&sh_domain->l1, l1index, 1,
+                      sh_domain->l2[l1index].handle | 0x1);
+        return 0;
+}
+
+static void l2realfree(struct shmobile_iommu_domain_pgtable *l2)
+{
+        if (l2->pgtable)
+                pgtable_free(l2, l2cache, L2_SIZE);
+}
+
+static void l2free(struct shmobile_iommu_domain *sh_domain,
+                   unsigned int l1index,
+                   struct shmobile_iommu_domain_pgtable *l2)
+{
+        pgtable_write(&sh_domain->l1, l1index, 1, 0);
+        if (sh_domain->l2[l1index].pgtable) {
+                *l2 = sh_domain->l2[l1index];
+                sh_domain->l2[l1index].pgtable = NULL;
+        }
+}
+
+static int shmobile_iommu_map(struct iommu_domain *domain, unsigned long iova,
+                              phys_addr_t paddr, size_t size, int prot)
+{
+        struct shmobile_iommu_domain_pgtable l2 = { .pgtable = NULL };
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+        unsigned int l1index, l2index;
+        int ret;
+
+        l1index = iova >> 20;
+        switch (size) {
+        case SZ_4K:
+                l2index = (iova >> 12) & 0xff;
+                spin_lock(&sh_domain->map_lock);
+                ret = l2alloc(sh_domain, l1index);
+                if (!ret)
+                        pgtable_write(&sh_domain->l2[l1index], l2index, 1,
+                                      paddr | 0xff2);
+                spin_unlock(&sh_domain->map_lock);
+                break;
+        case SZ_64K:
+                l2index = (iova >> 12) & 0xf0;
+                spin_lock(&sh_domain->map_lock);
+                ret = l2alloc(sh_domain, l1index);
+                if (!ret)
+                        pgtable_write(&sh_domain->l2[l1index], l2index, 0x10,
+                                      paddr | 0xff1);
+                spin_unlock(&sh_domain->map_lock);
+                break;
+        case SZ_1M:
+                spin_lock(&sh_domain->map_lock);
+                l2free(sh_domain, l1index, &l2);
+                pgtable_write(&sh_domain->l1, l1index, 1, paddr | 0xc02);
+                spin_unlock(&sh_domain->map_lock);
+                ret = 0;
+                break;
+        default:
+                ret = -EINVAL;
+        }
+        if (!ret)
+                domain_tlb_flush(sh_domain);
+        l2realfree(&l2);
+        return ret;
+}
+
+static size_t shmobile_iommu_unmap(struct iommu_domain *domain,
+                                   unsigned long iova, size_t size)
+{
+        struct shmobile_iommu_domain_pgtable l2 = { .pgtable = NULL };
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+        unsigned int l1index, l2index;
+        uint32_t l2entry = 0;
+        size_t ret = 0;
+
+        l1index = iova >> 20;
+        if (!(iova & 0xfffff) && size >= SZ_1M) {
+                spin_lock(&sh_domain->map_lock);
+                l2free(sh_domain, l1index, &l2);
+                spin_unlock(&sh_domain->map_lock);
+                ret = SZ_1M;
+                goto done;
+        }
+        l2index = (iova >> 12) & 0xff;
+        spin_lock(&sh_domain->map_lock);
+        if (sh_domain->l2[l1index].pgtable)
+                l2entry = pgtable_read(&sh_domain->l2[l1index], l2index);
+        switch (l2entry & 3) {
+        case 1:
+                if (l2index & 0xf)
+                        break;
+                pgtable_write(&sh_domain->l2[l1index], l2index, 0x10, 0);
+                ret = SZ_64K;
+                break;
+        case 2:
+                pgtable_write(&sh_domain->l2[l1index], l2index, 1, 0);
+                ret = SZ_4K;
+                break;
+        }
+        spin_unlock(&sh_domain->map_lock);
+done:
+        if (ret)
+                domain_tlb_flush(sh_domain);
+        l2realfree(&l2);
+        return ret;
+}
+
+static phys_addr_t shmobile_iommu_iova_to_phys(struct iommu_domain *domain,
+                                               unsigned long iova)
+{
+        struct shmobile_iommu_domain *sh_domain = domain->priv;
+        uint32_t l1entry = 0, l2entry = 0;
+        unsigned int l1index, l2index;
+
+        l1index = iova >> 20;
+        l2index = (iova >> 12) & 0xff;
+        spin_lock(&sh_domain->map_lock);
+        if (sh_domain->l2[l1index].pgtable)
+                l2entry = pgtable_read(&sh_domain->l2[l1index], l2index);
+        else
+                l1entry = pgtable_read(&sh_domain->l1, l1index);
+        spin_unlock(&sh_domain->map_lock);
+        switch (l2entry & 3) {
+        case 1:
+                return (l2entry & ~0xffff) | (iova & 0xffff);
+        case 2:
+                return (l2entry & ~0xfff) | (iova & 0xfff);
+        default:
+                if ((l1entry & 3) == 2)
+                        return (l1entry & ~0xfffff) | (iova & 0xfffff);
+                return 0;
+        }
+}
+
+static int find_dev_name(struct shmobile_ipmmu *ipmmu, const char *dev_name)
+{
+        unsigned int i, n = ipmmu->num_dev_names;
+
+        for (i = 0; i < n; i++) {
+                if (strcmp(ipmmu->dev_names[i], dev_name) == 0)
+                        return 1;
+        }
+        return 0;
+}
+
+static int shmobile_iommu_add_device(struct device *dev)
+{
+        struct shmobile_iommu_archdata *archdata = ipmmu_archdata;
+        struct dma_iommu_mapping *mapping;
+
+        if (!find_dev_name(archdata->ipmmu, dev_name(dev)))
+                return 0;
+        mapping = archdata->iommu_mapping;
+        if (!mapping) {
+                mapping = arm_iommu_create_mapping(&platform_bus_type, 0,
+                                                   L1_LEN << 20, 0);
+                if (IS_ERR(mapping))
+                        return PTR_ERR(mapping);
+                archdata->iommu_mapping = mapping;
+        }
+        dev->archdata.iommu = archdata;
+        if (arm_iommu_attach_device(dev, mapping))
+                pr_err("arm_iommu_attach_device failed\n");
+        return 0;
+}
+
+static struct iommu_ops shmobile_iommu_ops = {
+        .domain_init = shmobile_iommu_domain_init,
+        .domain_destroy = shmobile_iommu_domain_destroy,
+        .attach_dev = shmobile_iommu_attach_device,
+        .detach_dev = shmobile_iommu_detach_device,
+        .map = shmobile_iommu_map,
+        .unmap = shmobile_iommu_unmap,
+        .iova_to_phys = shmobile_iommu_iova_to_phys,
+        .add_device = shmobile_iommu_add_device,
+        .pgsize_bitmap = SZ_1M | SZ_64K | SZ_4K,
+};
+
+int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu)
+{
+        static struct shmobile_iommu_archdata *archdata;
+
+        l1cache = kmem_cache_create("shmobile-iommu-pgtable1", L1_SIZE,
+                                    L1_ALIGN, SLAB_HWCACHE_ALIGN, NULL);
+        if (!l1cache)
+                return -ENOMEM;
+        l2cache = kmem_cache_create("shmobile-iommu-pgtable2", L2_SIZE,
+                                    L2_ALIGN, SLAB_HWCACHE_ALIGN, NULL);
+        if (!l2cache) {
+                kmem_cache_destroy(l1cache);
+                return -ENOMEM;
+        }
+        archdata = kmalloc(sizeof(*archdata), GFP_KERNEL);
+        if (!archdata) {
+                kmem_cache_destroy(l1cache);
+                kmem_cache_destroy(l2cache);
+                return -ENOMEM;
+        }
+        spin_lock_init(&archdata->attach_lock);
+        archdata->attached = NULL;
+        archdata->ipmmu = ipmmu;
+        ipmmu_archdata = archdata;
+        bus_set_iommu(&platform_bus_type, &shmobile_iommu_ops);
+        return 0;
+}
diff --git a/drivers/iommu/shmobile-ipmmu.c b/drivers/iommu/shmobile-ipmmu.c
new file mode 100644
index 000000000000..8321f89596c4
--- /dev/null
+++ b/drivers/iommu/shmobile-ipmmu.c
@@ -0,0 +1,136 @@
+/*
+ * IPMMU/IPMMUI
+ * Copyright (C) 2012  Hideki EIRAKU
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#include <linux/err.h>
+#include <linux/export.h>
+#include <linux/io.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/platform_data/sh_ipmmu.h>
+#include "shmobile-ipmmu.h"
+
+#define IMCTR1 0x000
+#define IMCTR2 0x004
+#define IMASID 0x010
+#define IMTTBR 0x014
+#define IMTTBCR 0x018
+
+#define IMCTR1_TLBEN (1 << 0)
+#define IMCTR1_FLUSH (1 << 1)
+
+static void ipmmu_reg_write(struct shmobile_ipmmu *ipmmu, unsigned long reg_off,
+                            unsigned long data)
+{
+        iowrite32(data, ipmmu->ipmmu_base + reg_off);
+}
+
+void ipmmu_tlb_flush(struct shmobile_ipmmu *ipmmu)
+{
+        if (!ipmmu)
+                return;
+
+        mutex_lock(&ipmmu->flush_lock);
+        if (ipmmu->tlb_enabled)
+                ipmmu_reg_write(ipmmu, IMCTR1, IMCTR1_FLUSH | IMCTR1_TLBEN);
+        else
+                ipmmu_reg_write(ipmmu, IMCTR1, IMCTR1_FLUSH);
+        mutex_unlock(&ipmmu->flush_lock);
+}
+
+void ipmmu_tlb_set(struct shmobile_ipmmu *ipmmu, unsigned long phys, int size,
+                   int asid)
+{
+        if (!ipmmu)
+                return;
+
+        mutex_lock(&ipmmu->flush_lock);
+        switch (size) {
+        default:
+                ipmmu->tlb_enabled = 0;
+                break;
+        case 0x2000:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 1);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x1000:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 2);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x800:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 3);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x400:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 4);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x200:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 5);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x100:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 6);
+                ipmmu->tlb_enabled = 1;
+                break;
+        case 0x80:
+                ipmmu_reg_write(ipmmu, IMTTBCR, 7);
+                ipmmu->tlb_enabled = 1;
+                break;
+        }
+        ipmmu_reg_write(ipmmu, IMTTBR, phys);
+        ipmmu_reg_write(ipmmu, IMASID, asid);
+        mutex_unlock(&ipmmu->flush_lock);
+}
+
+static int ipmmu_probe(struct platform_device *pdev)
+{
+        struct shmobile_ipmmu *ipmmu;
+        struct resource *res;
+        struct shmobile_ipmmu_platform_data *pdata = pdev->dev.platform_data;
+
+        res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "cannot get platform resources\n");
+                return -ENOENT;
+        }
+        ipmmu = devm_kzalloc(&pdev->dev, sizeof(*ipmmu), GFP_KERNEL);
+        if (!ipmmu) {
+                dev_err(&pdev->dev, "cannot allocate device data\n");
+                return -ENOMEM;
+        }
+        mutex_init(&ipmmu->flush_lock);
+        ipmmu->dev = &pdev->dev;
+        ipmmu->ipmmu_base = devm_ioremap_nocache(&pdev->dev, res->start,
+                                                resource_size(res));
+        if (!ipmmu->ipmmu_base) {
+                dev_err(&pdev->dev, "ioremap_nocache failed\n");
+                return -ENOMEM;
+        }
+        ipmmu->dev_names = pdata->dev_names;
+        ipmmu->num_dev_names = pdata->num_dev_names;
+        platform_set_drvdata(pdev, ipmmu);
+        ipmmu_reg_write(ipmmu, IMCTR1, 0x0); /* disable TLB */
+        ipmmu_reg_write(ipmmu, IMCTR2, 0x0); /* disable PMB */
+        ipmmu_iommu_init(ipmmu);
+        return 0;
+}
+
+static struct platform_driver ipmmu_driver = {
+        .probe = ipmmu_probe,
+        .driver = {
+                .owner = THIS_MODULE,
+                .name = "ipmmu",
+        },
+};
+
+static int __init ipmmu_init(void)
+{
+        return platform_driver_register(&ipmmu_driver);
+}
+subsys_initcall(ipmmu_init);
diff --git a/drivers/iommu/shmobile-ipmmu.h b/drivers/iommu/shmobile-ipmmu.h
new file mode 100644
index 000000000000..4d53684673e1
--- /dev/null
+++ b/drivers/iommu/shmobile-ipmmu.h
@@ -0,0 +1,34 @@
+/* shmobile-ipmmu.h
+ *
+ * Copyright (C) 2012  Hideki EIRAKU
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ */
+
+#ifndef __SHMOBILE_IPMMU_H__
+#define __SHMOBILE_IPMMU_H__
+
+struct shmobile_ipmmu {
+        struct device *dev;
+        void __iomem *ipmmu_base;
+        int tlb_enabled;
+        struct mutex flush_lock;
+        const char * const *dev_names;
+        unsigned int num_dev_names;
+};
+
+#ifdef CONFIG_SHMOBILE_IPMMU_TLB
+void ipmmu_tlb_flush(struct shmobile_ipmmu *ipmmu);
+void ipmmu_tlb_set(struct shmobile_ipmmu *ipmmu, unsigned long phys, int size,
+                   int asid);
+int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu);
+#else
+static inline int ipmmu_iommu_init(struct shmobile_ipmmu *ipmmu)
+{
+        return -EINVAL;
+}
+#endif
+
+#endif /* __SHMOBILE_IPMMU_H__ */
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
index 8219f1d596ee..86437575f94d 100644
--- a/drivers/iommu/tegra-gart.c
+++ b/drivers/iommu/tegra-gart.c
@@ -430,13 +430,11 @@ const struct dev_pm_ops tegra_gart_pm_ops = {
         .resume         = tegra_gart_resume,
 };
 
-#ifdef CONFIG_OF
 static struct of_device_id tegra_gart_of_match[] = {
         { .compatible = "nvidia,tegra20-gart", },
         { },
 };
 MODULE_DEVICE_TABLE(of, tegra_gart_of_match);
-#endif
 
 static struct platform_driver tegra_gart_driver = {
         .probe          = tegra_gart_probe,
@@ -445,7 +443,7 @@ static struct platform_driver tegra_gart_driver = {
                 .owner  = THIS_MODULE,
                 .name   = "tegra-gart",
                 .pm     = &tegra_gart_pm_ops,
-                .of_match_table = of_match_ptr(tegra_gart_of_match),
+                .of_match_table = tegra_gart_of_match,
         },
 };
 
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index fc178893789a..8b1d9f758076 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -1,7 +1,7 @@
 /*
  * IOMMU API for SMMU in Tegra30
  *
- * Copyright (c) 2011-2012, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2011-2013, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -293,7 +293,11 @@ struct smmu_debugfs_info {
  * Per SMMU device - IOMMU device
  */
 struct smmu_device {
-        void __iomem    *regs[NUM_SMMU_REG_BANKS];
+        void __iomem    *regbase;       /* register offset base */
+        void __iomem    **regs;         /* register block start address array */
+        void __iomem    **rege;         /* register block end address array */
+        int             nregs;          /* number of register blocks */
+
         unsigned long   iovmm_base;     /* remappable base address */
         unsigned long   page_count;     /* total remappable size */
         spinlock_t      lock;
@@ -323,38 +327,37 @@ static struct smmu_device *smmu_handle; /* unique for a system */
 /*
  *      SMMU register accessors
  */
+static bool inline smmu_valid_reg(struct smmu_device *smmu,
+                                  void __iomem *addr)
+{
+        int i;
+
+        for (i = 0; i < smmu->nregs; i++) {
+                if (addr < smmu->regs[i])
+                        break;
+                if (addr <= smmu->rege[i])
+                        return true;
+        }
+
+        return false;
+}
+
 static inline u32 smmu_read(struct smmu_device *smmu, size_t offs)
 {
-        BUG_ON(offs < 0x10);
-        if (offs < 0x3c)
-                return readl(smmu->regs[0] + offs - 0x10);
-        BUG_ON(offs < 0x1f0);
-        if (offs < 0x200)
-                return readl(smmu->regs[1] + offs - 0x1f0);
-        BUG_ON(offs < 0x228);
-        if (offs < 0x284)
-                return readl(smmu->regs[2] + offs - 0x228);
-        BUG();
+        void __iomem *addr = smmu->regbase + offs;
+
+        BUG_ON(!smmu_valid_reg(smmu, addr));
+
+        return readl(addr);
 }
 
 static inline void smmu_write(struct smmu_device *smmu, u32 val, size_t offs)
 {
-        BUG_ON(offs < 0x10);
-        if (offs < 0x3c) {
-                writel(val, smmu->regs[0] + offs - 0x10);
-                return;
-        }
-        BUG_ON(offs < 0x1f0);
-        if (offs < 0x200) {
-                writel(val, smmu->regs[1] + offs - 0x1f0);
-                return;
-        }
-        BUG_ON(offs < 0x228);
-        if (offs < 0x284) {
-                writel(val, smmu->regs[2] + offs - 0x228);
-                return;
-        }
-        BUG();
+        void __iomem *addr = smmu->regbase + offs;
+
+        BUG_ON(!smmu_valid_reg(smmu, addr));
+
+        writel(val, addr);
 }
 
 #define VA_PAGE_TO_PA(va, page) \
@@ -1170,7 +1173,13 @@ static int tegra_smmu_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
-        for (i = 0; i < ARRAY_SIZE(smmu->regs); i++) {
+        smmu->nregs = pdev->num_resources;
+        smmu->regs = devm_kzalloc(dev, 2 * smmu->nregs * sizeof(*smmu->regs),
+                                  GFP_KERNEL);
+        smmu->rege = smmu->regs + smmu->nregs;
+        if (!smmu->regs)
+                return -ENOMEM;
+        for (i = 0; i < smmu->nregs; i++) {
                 struct resource *res;
 
                 res = platform_get_resource(pdev, IORESOURCE_MEM, i);
@@ -1179,7 +1188,10 @@ static int tegra_smmu_probe(struct platform_device *pdev)
                 smmu->regs[i] = devm_request_and_ioremap(&pdev->dev, res);
                 if (!smmu->regs[i])
                         return -EBUSY;
+                smmu->rege[i] = smmu->regs[i] + resource_size(res) - 1;
         }
+        /* Same as "mc" 1st regiter block start address */
+        smmu->regbase = (void __iomem *)((u32)smmu->regs[0] & PAGE_MASK);
 
         err = of_get_dma_window(dev->of_node, NULL, 0, NULL, &base, &size);
         if (err)
@@ -1216,6 +1228,7 @@ static int tegra_smmu_probe(struct platform_device *pdev)
                 as->pte_attr = _PTE_ATTR;
 
                 spin_lock_init(&as->lock);
+                spin_lock_init(&as->client_lock);
                 INIT_LIST_HEAD(&as->client);
         }
         spin_lock_init(&smmu->lock);
@@ -1254,13 +1267,11 @@ const struct dev_pm_ops tegra_smmu_pm_ops = {
         .resume         = tegra_smmu_resume,
 };
 
-#ifdef CONFIG_OF
 static struct of_device_id tegra_smmu_of_match[] = {
         { .compatible = "nvidia,tegra30-smmu", },
         { },
 };
 MODULE_DEVICE_TABLE(of, tegra_smmu_of_match);
-#endif
 
 static struct platform_driver tegra_smmu_driver = {
         .probe          = tegra_smmu_probe,
@@ -1269,7 +1280,7 @@ static struct platform_driver tegra_smmu_driver = {
                 .owner  = THIS_MODULE,
                 .name   = "tegra-smmu",
                 .pm     = &tegra_smmu_pm_ops,
-                .of_match_table = of_match_ptr(tegra_smmu_of_match),
+                .of_match_table = tegra_smmu_of_match,
         },
 };