[PATCH v4 12/14] swiotlb: Add restricted DMA alloc/free support.

[Claire Chang <tientzu@xxxxxxxxxxxx>]

Add the functions, dev_swiotlb_{alloc,free} to support the memory
allocation from restricted DMA pool.

Signed-off-by: Claire Chang <tientzu@xxxxxxxxxxxx>
---
 include/linux/swiotlb.h |  2 ++
 kernel/dma/direct.c     | 30 ++++++++++++++++++++++--------
 kernel/dma/swiotlb.c    | 34 ++++++++++++++++++++++++++++++++++
 3 files changed, 58 insertions(+), 8 deletions(-)

diff --git a/include/linux/swiotlb.h b/include/linux/swiotlb.h
index b9f2a250c8da..2cd39e102915 100644
--- a/include/linux/swiotlb.h
+++ b/include/linux/swiotlb.h
@@ -74,6 +74,8 @@ extern enum swiotlb_force swiotlb_force;
 #ifdef CONFIG_DMA_RESTRICTED_POOL
 bool is_swiotlb_force(struct device *dev);
 bool is_dev_swiotlb_force(struct device *dev);
+struct page *dev_swiotlb_alloc(struct device *dev, size_t size, gfp_t gfp);
+bool dev_swiotlb_free(struct device *dev, struct page *page, size_t size);
 #else
 static inline bool is_swiotlb_force(struct device *dev)
 {
diff --git a/kernel/dma/direct.c b/kernel/dma/direct.c
index a76a1a2f24da..f9a9321f7559 100644
--- a/kernel/dma/direct.c
+++ b/kernel/dma/direct.c
@@ -12,6 +12,7 @@
 #include <linux/pfn.h>
 #include <linux/vmalloc.h>
 #include <linux/set_memory.h>
+#include <linux/swiotlb.h>
 #include <linux/slab.h>
 #include "direct.h"
 
@@ -77,6 +78,10 @@ static bool dma_coherent_ok(struct device *dev, phys_addr_t 
phys, size_t size)
 
 static void __dma_direct_free_pages(struct device *dev, struct page *page, 
size_t size)
 {
+#ifdef CONFIG_DMA_RESTRICTED_POOL
+       if (dev_swiotlb_free(dev, page, size))
+               return;
+#endif
        dma_free_contiguous(dev, page, size);
 }
 
@@ -89,6 +94,12 @@ static struct page *__dma_direct_alloc_pages(struct device 
*dev, size_t size,
 
        WARN_ON_ONCE(!PAGE_ALIGNED(size));
 
+#ifdef CONFIG_DMA_RESTRICTED_POOL
+       page = dev_swiotlb_alloc(dev, size, gfp);
+       if (page)
+               return page;
+#endif
+
        gfp |= dma_direct_optimal_gfp_mask(dev, dev->coherent_dma_mask,
                                           &phys_limit);
        page = dma_alloc_contiguous(dev, size, gfp);
@@ -147,7 +158,7 @@ void *dma_direct_alloc(struct device *dev, size_t size,
                gfp |= __GFP_NOWARN;
 
        if ((attrs & DMA_ATTR_NO_KERNEL_MAPPING) &&
-           !force_dma_unencrypted(dev)) {
+           !force_dma_unencrypted(dev) && !is_dev_swiotlb_force(dev)) {
                page = __dma_direct_alloc_pages(dev, size, gfp & ~__GFP_ZERO);
                if (!page)
                        return NULL;
@@ -160,8 +171,8 @@ void *dma_direct_alloc(struct device *dev, size_t size,
        }
 
        if (!IS_ENABLED(CONFIG_ARCH_HAS_DMA_SET_UNCACHED) &&
-           !IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) &&
-           !dev_is_dma_coherent(dev))
+           !IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) && !dev_is_dma_coherent(dev) &&
+           !is_dev_swiotlb_force(dev))
                return arch_dma_alloc(dev, size, dma_handle, gfp, attrs);
 
        /*
@@ -171,7 +182,9 @@ void *dma_direct_alloc(struct device *dev, size_t size,
        if (IS_ENABLED(CONFIG_DMA_COHERENT_POOL) &&
            !gfpflags_allow_blocking(gfp) &&
            (force_dma_unencrypted(dev) ||
-            (IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) && 
!dev_is_dma_coherent(dev))))
+            (IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) &&
+             !dev_is_dma_coherent(dev))) &&
+           !is_dev_swiotlb_force(dev))
                return dma_direct_alloc_from_pool(dev, size, dma_handle, gfp);
 
        /* we always manually zero the memory once we are done */
@@ -252,15 +265,15 @@ void dma_direct_free(struct device *dev, size_t size,
        unsigned int page_order = get_order(size);
 
        if ((attrs & DMA_ATTR_NO_KERNEL_MAPPING) &&
-           !force_dma_unencrypted(dev)) {
+           !force_dma_unencrypted(dev) && !is_dev_swiotlb_force(dev)) {
                /* cpu_addr is a struct page cookie, not a kernel address */
                dma_free_contiguous(dev, cpu_addr, size);
                return;
        }
 
        if (!IS_ENABLED(CONFIG_ARCH_HAS_DMA_SET_UNCACHED) &&
-           !IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) &&
-           !dev_is_dma_coherent(dev)) {
+           !IS_ENABLED(CONFIG_DMA_DIRECT_REMAP) && !dev_is_dma_coherent(dev) &&
+           !is_dev_swiotlb_force(dev)) {
                arch_dma_free(dev, size, cpu_addr, dma_addr, attrs);
                return;
        }
@@ -288,7 +301,8 @@ struct page *dma_direct_alloc_pages(struct device *dev, 
size_t size,
        void *ret;
 
        if (IS_ENABLED(CONFIG_DMA_COHERENT_POOL) &&
-           force_dma_unencrypted(dev) && !gfpflags_allow_blocking(gfp))
+           force_dma_unencrypted(dev) && !gfpflags_allow_blocking(gfp) &&
+           !is_dev_swiotlb_force(dev))
                return dma_direct_alloc_from_pool(dev, size, dma_handle, gfp);
 
        page = __dma_direct_alloc_pages(dev, size, gfp);
diff --git a/kernel/dma/swiotlb.c b/kernel/dma/swiotlb.c
index fd9c1bd183ac..8b77fd64199e 100644
--- a/kernel/dma/swiotlb.c
+++ b/kernel/dma/swiotlb.c
@@ -836,6 +836,40 @@ late_initcall(swiotlb_create_default_debugfs);
 #endif
 
 #ifdef CONFIG_DMA_RESTRICTED_POOL
+struct page *dev_swiotlb_alloc(struct device *dev, size_t size, gfp_t gfp)
+{
+       struct swiotlb *swiotlb;
+       phys_addr_t tlb_addr;
+       unsigned int index;
+
+       /* dev_swiotlb_alloc can be used only in the context which permits 
sleeping. */
+       if (!dev->dev_swiotlb || !gfpflags_allow_blocking(gfp))
+               return NULL;
+
+       swiotlb = dev->dev_swiotlb;
+       index = swiotlb_tbl_find_free_region(dev, swiotlb->start, size, 0);
+       if (index < 0)
+               return NULL;
+
+       tlb_addr = swiotlb->start + (index << IO_TLB_SHIFT);
+
+       return pfn_to_page(PFN_DOWN(tlb_addr));
+}
+
+bool dev_swiotlb_free(struct device *dev, struct page *page, size_t size)
+{
+       unsigned int index;
+       phys_addr_t tlb_addr = page_to_phys(page);
+
+       if (!is_swiotlb_buffer(dev, tlb_addr))
+               return false;
+
+       index = (tlb_addr - dev->dev_swiotlb->start) >> IO_TLB_SHIFT;
+       swiotlb_tbl_release_region(dev, index, size);
+
+       return true;
+}
+
 bool is_swiotlb_force(struct device *dev)
 {
        return unlikely(swiotlb_force == SWIOTLB_FORCE) || dev->dev_swiotlb;
-- 
2.30.0.478.g8a0d178c01-goog