[PATCH v3 3/4] swiotlb: make new {unmap,sync}_desc dma apis work with swiotlb

[Laurentiu Tudor]

From: Laurentiu Tudor <laurentiu.tudor@xxxxxxx>

Add a new swiotlb helper to retrieve the original physical
address given a swiotlb physical address and use it in the new
dma_unmap_single_attrs_desc(), dma_sync_single_for_cpu_desc() and
dma_unmap_page_attrs_desc() APIs to make them work with swiotlb.

Signed-off-by: Laurentiu Tudor <laurentiu.tudor@xxxxxxx>
---
 include/linux/swiotlb.h |  7 +++++++
 kernel/dma/mapping.c    | 43 ++++++++++++++++++++++++++++++++---------
 kernel/dma/swiotlb.c    |  8 ++++++++
 3 files changed, 49 insertions(+), 9 deletions(-)

diff --git a/include/linux/swiotlb.h b/include/linux/swiotlb.h
index cde3dc18e21a..7a6883a71649 100644
--- a/include/linux/swiotlb.h
+++ b/include/linux/swiotlb.h
@@ -73,6 +73,8 @@ static inline bool is_swiotlb_buffer(phys_addr_t paddr)
 	return paddr >= io_tlb_start && paddr < io_tlb_end;
 }
 
+phys_addr_t swiotlb_get_orig_phys(phys_addr_t tlb_addr);
+
 bool swiotlb_map(struct device *dev, phys_addr_t *phys, dma_addr_t *dma_addr,
 		size_t size, enum dma_data_direction dir, unsigned long attrs);
 void __init swiotlb_exit(void);
@@ -85,6 +87,11 @@ static inline bool is_swiotlb_buffer(phys_addr_t paddr)
 {
 	return false;
 }
+
+static inline phys_addr_t swiotlb_get_orig_phys(phys_addr_t tlb_addr)
+{
+	return PHYS_ADDR_MAX;
+}
 static inline bool swiotlb_map(struct device *dev, phys_addr_t *phys,
 		dma_addr_t *dma_addr, size_t size, enum dma_data_direction dir,
 		unsigned long attrs)
diff --git a/kernel/dma/mapping.c b/kernel/dma/mapping.c
index 2b6f245c9bb1..1a2d02727271 100644
--- a/kernel/dma/mapping.c
+++ b/kernel/dma/mapping.c
@@ -14,6 +14,7 @@
 #include <linux/of_device.h>
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
+#include <linux/swiotlb.h>
 
 /*
  * Managed DMA API
@@ -352,10 +353,18 @@ dma_unmap_page_attrs_desc(struct device *dev, dma_addr_t addr, size_t size,
 	const struct dma_map_ops *ops = get_dma_ops(dev);
 	void *ptr = NULL;
 
-	if (dma_is_direct(ops))
-		ptr = phys_to_virt(dma_to_phys(dev, addr));
-	else if (ops && ops->get_virt_addr)
+	if (dma_is_direct(ops)) {
+		phys_addr_t phys = dma_to_phys(dev, addr);
+
+		if (is_swiotlb_buffer(phys)) {
+			phys = swiotlb_get_orig_phys(phys);
+			ptr = phys == PHYS_ADDR_MAX ? NULL : phys_to_virt(phys);
+		} else {
+			ptr = phys_to_virt(phys);
+		}
+	} else if (ops && ops->get_virt_addr) {
 		ptr = ops->get_virt_addr(dev, addr);
+	}
 
 	dma_unmap_page_attrs(dev, addr, size, dir, attrs);
 
@@ -370,10 +379,18 @@ void *dma_unmap_single_attrs_desc(struct device *dev, dma_addr_t addr,
 	const struct dma_map_ops *ops = get_dma_ops(dev);
 	void *ptr = NULL;
 
-	if (dma_is_direct(ops))
-		ptr = phys_to_virt(dma_to_phys(dev, addr));
-	else if (ops && ops->get_virt_addr)
+	if (dma_is_direct(ops)) {
+		phys_addr_t phys = dma_to_phys(dev, addr);
+
+		if (is_swiotlb_buffer(phys)) {
+			phys = swiotlb_get_orig_phys(phys);
+			ptr = phys == PHYS_ADDR_MAX ? NULL : phys_to_virt(phys);
+		} else {
+			ptr = phys_to_virt(phys);
+		}
+	} else if (ops && ops->get_virt_addr) {
 		ptr = ops->get_virt_addr(dev, addr);
+	}
 
 	dma_unmap_single_attrs(dev, addr, size, dir, attrs);
 
@@ -387,10 +404,18 @@ void *dma_sync_single_for_cpu_desc(struct device *dev, dma_addr_t addr,
 	const struct dma_map_ops *ops = get_dma_ops(dev);
 	void *ptr = NULL;
 
-	if (dma_is_direct(ops))
-		ptr = phys_to_virt(dma_to_phys(dev, addr));
-	else if (ops && ops->get_virt_addr)
+	if (dma_is_direct(ops)) {
+		phys_addr_t phys = dma_to_phys(dev, addr);
+
+		if (is_swiotlb_buffer(phys)) {
+			phys = swiotlb_get_orig_phys(phys);
+			ptr = phys == PHYS_ADDR_MAX ? NULL : phys_to_virt(phys);
+		} else {
+			ptr = phys_to_virt(phys);
+		}
+	} else if (ops && ops->get_virt_addr) {
 		ptr = ops->get_virt_addr(dev, addr);
+	}
 
 	dma_sync_single_for_cpu(dev, addr, size, dir);
 
diff --git a/kernel/dma/swiotlb.c b/kernel/dma/swiotlb.c
index 673a2cdb2656..9b241cc0535b 100644
--- a/kernel/dma/swiotlb.c
+++ b/kernel/dma/swiotlb.c
@@ -701,6 +701,14 @@ bool is_swiotlb_active(void)
 	return io_tlb_end != 0;
 }
 
+phys_addr_t swiotlb_get_orig_phys(phys_addr_t tlb_addr)
+{
+	int index = (tlb_addr - io_tlb_start) >> IO_TLB_SHIFT;
+	phys_addr_t phys = io_tlb_orig_addr[index];
+
+	return phys == INVALID_PHYS_ADDR ? PHYS_ADDR_MAX : phys;
+}
+
 #ifdef CONFIG_DEBUG_FS
 
 static int __init swiotlb_create_debugfs(void)
-- 
2.17.1