iommu/io-pgtable-arm-v7s: Refactor split_blk_unmap

[linux.git] / drivers / iommu / io-pgtable-arm-v7s.c

diff --git a/drivers/iommu/io-pgtable-arm-v7s.c b/drivers/iommu/io-pgtable-arm-v7s.c
index 46da7aa7c7d0f100b188f520bcf4452e9b7e928b..dc74631322e4e09853aefc975118efa19608d263 100644 (file)
--- a/drivers/iommu/io-pgtable-arm-v7s.c
+++ b/drivers/iommu/io-pgtable-arm-v7s.c
@@ -281,6 +281,13 @@ static arm_v7s_iopte arm_v7s_prot_to_pte(int prot, int lvl,
        else if (prot & IOMMU_CACHE)
                pte |= ARM_V7S_ATTR_B | ARM_V7S_ATTR_C;
 
+       pte |= ARM_V7S_PTE_TYPE_PAGE;
+       if (lvl == 1 && (cfg->quirks & IO_PGTABLE_QUIRK_ARM_NS))
+               pte |= ARM_V7S_ATTR_NS_SECTION;
+
+       if (cfg->quirks & IO_PGTABLE_QUIRK_ARM_MTK_4GB)
+               pte |= ARM_V7S_ATTR_MTK_4GB;
+
        return pte;
 }
 
@@ -353,7 +360,7 @@ static int arm_v7s_init_pte(struct arm_v7s_io_pgtable *data,
                            int lvl, int num_entries, arm_v7s_iopte *ptep)
 {
        struct io_pgtable_cfg *cfg = &data->iop.cfg;
-       arm_v7s_iopte pte = arm_v7s_prot_to_pte(prot, lvl, cfg);
+       arm_v7s_iopte pte;
        int i;
 
        for (i = 0; i < num_entries; i++)
@@ -375,13 +382,7 @@ static int arm_v7s_init_pte(struct arm_v7s_io_pgtable *data,
                        return -EEXIST;
                }
 
-       pte |= ARM_V7S_PTE_TYPE_PAGE;
-       if (lvl == 1 && (cfg->quirks & IO_PGTABLE_QUIRK_ARM_NS))
-               pte |= ARM_V7S_ATTR_NS_SECTION;
-
-       if (cfg->quirks & IO_PGTABLE_QUIRK_ARM_MTK_4GB)
-               pte |= ARM_V7S_ATTR_MTK_4GB;
-
+       pte = arm_v7s_prot_to_pte(prot, lvl, cfg);
        if (num_entries > 1)
                pte = arm_v7s_pte_to_cont(pte, lvl);
 
@@ -391,6 +392,20 @@ static int arm_v7s_init_pte(struct arm_v7s_io_pgtable *data,
        return 0;
 }
 
+static arm_v7s_iopte arm_v7s_install_table(arm_v7s_iopte *table,
+                                          arm_v7s_iopte *ptep,
+                                          struct io_pgtable_cfg *cfg)
+{
+       arm_v7s_iopte new;
+
+       new = virt_to_phys(table) | ARM_V7S_PTE_TYPE_TABLE;
+       if (cfg->quirks & IO_PGTABLE_QUIRK_ARM_NS)
+               new |= ARM_V7S_ATTR_NS_TABLE;
+
+       __arm_v7s_set_pte(ptep, new, 1, cfg);
+       return new;
+}
+
 static int __arm_v7s_map(struct arm_v7s_io_pgtable *data, unsigned long iova,
                         phys_addr_t paddr, size_t size, int prot,
                         int lvl, arm_v7s_iopte *ptep)
@@ -418,11 +433,7 @@ static int __arm_v7s_map(struct arm_v7s_io_pgtable *data, unsigned long iova,
                if (!cptep)
                        return -ENOMEM;
 
-               pte = virt_to_phys(cptep) | ARM_V7S_PTE_TYPE_TABLE;
-               if (cfg->quirks & IO_PGTABLE_QUIRK_ARM_NS)
-                       pte |= ARM_V7S_ATTR_NS_TABLE;
-
-               __arm_v7s_set_pte(ptep, pte, 1, cfg);
+               arm_v7s_install_table(cptep, ptep, cfg);
        } else if (ARM_V7S_PTE_IS_TABLE(pte, lvl)) {
                cptep = iopte_deref(pte, lvl);
        } else {
@@ -503,41 +514,35 @@ static void arm_v7s_split_cont(struct arm_v7s_io_pgtable *data,
 
 static int arm_v7s_split_blk_unmap(struct arm_v7s_io_pgtable *data,
                                   unsigned long iova, size_t size,
-                                  arm_v7s_iopte *ptep)
+                                  arm_v7s_iopte blk_pte, arm_v7s_iopte *ptep)
 {
-       unsigned long blk_start, blk_end, blk_size;
-       phys_addr_t blk_paddr;
-       arm_v7s_iopte table = 0;
-       int prot = arm_v7s_pte_to_prot(*ptep, 1);
+       struct io_pgtable_cfg *cfg = &data->iop.cfg;
+       arm_v7s_iopte pte, *tablep;
+       int i, unmap_idx, num_entries, num_ptes;
+
+       tablep = __arm_v7s_alloc_table(2, GFP_ATOMIC, data);
+       if (!tablep)
+               return 0; /* Bytes unmapped */
 
-       blk_size = ARM_V7S_BLOCK_SIZE(1);
-       blk_start = iova & ARM_V7S_LVL_MASK(1);
-       blk_end = blk_start + ARM_V7S_BLOCK_SIZE(1);
-       blk_paddr = *ptep & ARM_V7S_LVL_MASK(1);
+       num_ptes = ARM_V7S_PTES_PER_LVL(2);
+       num_entries = size >> ARM_V7S_LVL_SHIFT(2);
+       unmap_idx = ARM_V7S_LVL_IDX(iova, 2);
 
-       for (; blk_start < blk_end; blk_start += size, blk_paddr += size) {
-               arm_v7s_iopte *tablep;
+       pte = arm_v7s_prot_to_pte(arm_v7s_pte_to_prot(blk_pte, 1), 2, cfg);
+       if (num_entries > 1)
+               pte = arm_v7s_pte_to_cont(pte, 2);
 
+       for (i = 0; i < num_ptes; i += num_entries, pte += size) {
                /* Unmap! */
-               if (blk_start == iova)
+               if (i == unmap_idx)
                        continue;
 
-               /* __arm_v7s_map expects a pointer to the start of the table */
-               tablep = &table - ARM_V7S_LVL_IDX(blk_start, 1);
-               if (__arm_v7s_map(data, blk_start, blk_paddr, size, prot, 1,
-                                 tablep) < 0) {
-                       if (table) {
-                               /* Free the table we allocated */
-                               tablep = iopte_deref(table, 1);
-                               __arm_v7s_free_table(tablep, 2, data);
-                       }
-                       return 0; /* Bytes unmapped */
-               }
+               __arm_v7s_set_pte(&tablep[i], pte, num_entries, cfg);
        }
 
-       __arm_v7s_set_pte(ptep, table, 1, &data->iop.cfg);
-       iova &= ~(blk_size - 1);
-       io_pgtable_tlb_add_flush(&data->iop, iova, blk_size, blk_size, true);
+       arm_v7s_install_table(tablep, ptep, cfg);
+
+       io_pgtable_tlb_add_flush(&data->iop, iova, size, size, true);
        return size;
 }
 
@@ -594,7 +599,7 @@ static int __arm_v7s_unmap(struct arm_v7s_io_pgtable *data,
                 * Insert a table at the next level to map the old region,
                 * minus the part we want to unmap
                 */
-               return arm_v7s_split_blk_unmap(data, iova, size, ptep);
+               return arm_v7s_split_blk_unmap(data, iova, size, pte[0], ptep);
        }
 
        /* Keep on walkin' */