Signed-off-by: Robin Murphy <robin.murphy@arm.com>
---
 arch/arm64/include/asm/device.h      |   3 +
 arch/arm64/include/asm/dma-mapping.h |  17 ++
 arch/arm64/mm/dma-mapping.c          | 320 +++++++++++++++++++++++++++++++++++
 3 files changed, 340 insertions(+)

diff --git a/arch/arm64/include/asm/device.h b/arch/arm64/include/asm/device.h
index 243ef25..510cee1 100644
--- a/arch/arm64/include/asm/device.h
+++ b/arch/arm64/include/asm/device.h

@@ -20,6 +20,9 @@ struct dev_archdata {
 	struct dma_map_ops *dma_ops;
 #ifdef CONFIG_IOMMU_API
 	void *iommu;			/* private IOMMU data */
+#ifdef CONFIG_IOMMU_DMA
+	struct iommu_dma_domain *dma_domain;
+#endif
 #endif
 	bool dma_coherent;
 };

diff --git a/arch/arm64/include/asm/dma-mapping.h b/arch/arm64/include/asm/dma-mapping.h
index 6932bb5..c1b271f 100644
--- a/arch/arm64/include/asm/dma-mapping.h
+++ b/arch/arm64/include/asm/dma-mapping.h

@@ -62,13 +62,30 @@ static inline bool is_device_dma_coherent(struct device *dev)
 
 #include <asm-generic/dma-mapping-common.h>
 
+#ifdef CONFIG_IOMMU_DMA
+static inline struct iommu_dma_domain *get_dma_domain(struct device *dev)
+{
+	return dev->archdata.dma_domain;
+}

+static inline void set_dma_domain(struct device *dev,
+				  struct iommu_dma_domain *dma_domain)
+{
+	dev->archdata.dma_domain = dma_domain;
+}
+#endif
+
 static inline dma_addr_t phys_to_dma(struct device *dev, phys_addr_t paddr)
 {
+	if (WARN_ON(dev && get_dma_domain(dev)))
+		return DMA_ERROR_CODE;
 	return (dma_addr_t)paddr;
 }
 
 static inline phys_addr_t dma_to_phys(struct device *dev, dma_addr_t dev_addr)
 {
+	if (WARN_ON(dev && get_dma_domain(dev)))
+		return 0;
 	return (phys_addr_t)dev_addr;
 }
 

diff --git a/arch/arm64/mm/dma-mapping.c b/arch/arm64/mm/dma-mapping.c
index 0a24b9b..28e771c 100644
--- a/arch/arm64/mm/dma-mapping.c
+++ b/arch/arm64/mm/dma-mapping.c

@@ -23,6 +23,7 @@
 #include <linux/genalloc.h>
 #include <linux/dma-mapping.h>
 #include <linux/dma-contiguous.h>
+#include <linux/dma-iommu.h>
 #include <linux/vmalloc.h>
 #include <linux/swiotlb.h>
 

@@ -426,6 +427,7 @@ static int __init arm64_dma_init(void)
 
 	ret |= swiotlb_late_init();
 	ret |= atomic_pool_init();
+	ret |= iommu_dma_init();
 
 	return ret;
 }

@@ -439,3 +441,321 @@ static int __init dma_debug_do_init(void)
 	return 0;
 }
 fs_initcall(dma_debug_do_init);
+
+
+#ifdef CONFIG_IOMMU_DMA
+
+static struct page **__atomic_get_pages(void *addr)
+{
+	struct page *page;
+	phys_addr_t phys;
+
+	phys = gen_pool_virt_to_phys(atomic_pool, (unsigned long)addr);
+	page = phys_to_page(phys);
+
+	return (struct page **)page;
+}
+
+static struct page **__iommu_get_pages(void *cpu_addr, struct dma_attrs *attrs)
+{
+	struct vm_struct *area;
+
+	if (__in_atomic_pool(cpu_addr, PAGE_SIZE))
+		return __atomic_get_pages(cpu_addr);
+
+	area = find_vm_area(cpu_addr);
+	if (!area)
+		return NULL;
+
+	return area->pages;
+}
+
+static void *__iommu_alloc_atomic(struct device *dev, size_t size,
+				  dma_addr_t *handle, bool coherent)
+{
+	struct page *page;
+	void *addr;
+
+	addr = __alloc_from_pool(size, &page);
+	if (!addr)
+		return NULL;
+
+	*handle = iommu_dma_create_iova_mapping(dev, &page, size, coherent);
+	if (*handle == DMA_ERROR_CODE) {
+		__free_from_pool(addr, size);
+		return NULL;
+	}
+	return addr;
+}
+
+static void __iommu_free_atomic(struct device *dev, void *cpu_addr,
+				dma_addr_t handle, size_t size)
+{
+	iommu_dma_release_iova_mapping(dev, handle, size);
+	__free_from_pool(cpu_addr, size);
+}