diff --git a/lib/xlat_tables_v2/xlat_tables_core.c b/lib/xlat_tables_v2/xlat_tables_core.c index a39dfc93a..0e6a6fa80 100644 --- a/lib/xlat_tables_v2/xlat_tables_core.c +++ b/lib/xlat_tables_v2/xlat_tables_core.c @@ -231,6 +231,37 @@ typedef enum { } action_t; +/* + * Function that returns the first VA of the table affected by the specified + * mmap region. + */ +static uintptr_t xlat_tables_find_start_va(mmap_region_t *mm, + const uintptr_t table_base_va, + const unsigned int level) +{ + uintptr_t table_idx_va; + + if (mm->base_va > table_base_va) { + /* Find the first index of the table affected by the region. */ + table_idx_va = mm->base_va & ~XLAT_BLOCK_MASK(level); + } else { + /* Start from the beginning of the table. */ + table_idx_va = table_base_va; + } + + return table_idx_va; +} + +/* + * Function that returns table index for the given VA and level arguments. + */ +static inline unsigned int xlat_tables_va_to_index(const uintptr_t table_base_va, + const uintptr_t va, + const unsigned int level) +{ + return (unsigned int)((va - table_base_va) >> XLAT_ADDR_SHIFT(level)); +} + #if PLAT_XLAT_TABLES_DYNAMIC /* @@ -316,19 +347,8 @@ static void xlat_tables_unmap_region(xlat_ctx_t *ctx, mmap_region_t *mm, unsigned int table_idx; - if (mm->base_va > table_base_va) { - /* Find the first index of the table affected by the region. */ - table_idx_va = mm->base_va & ~XLAT_BLOCK_MASK(level); - - table_idx = (unsigned int)((table_idx_va - table_base_va) >> - XLAT_ADDR_SHIFT(level)); - - assert(table_idx < table_entries); - } else { - /* Start from the beginning of the table. */ - table_idx_va = table_base_va; - table_idx = 0; - } + table_idx_va = xlat_tables_find_start_va(mm, table_base_va, level); + table_idx = xlat_tables_va_to_index(table_base_va, table_idx_va, level); while (table_idx < table_entries) { @@ -540,19 +560,8 @@ static uintptr_t xlat_tables_map_region(xlat_ctx_t *ctx, mmap_region_t *mm, unsigned int table_idx; - if (mm->base_va > table_base_va) { - /* Find the first index of the table affected by the region. */ - table_idx_va = mm->base_va & ~XLAT_BLOCK_MASK(level); - - table_idx = (unsigned int)((table_idx_va - table_base_va) >> - XLAT_ADDR_SHIFT(level)); - - assert(table_idx < table_entries); - } else { - /* Start from the beginning of the table. */ - table_idx_va = table_base_va; - table_idx = 0U; - } + table_idx_va = xlat_tables_find_start_va(mm, table_base_va, level); + table_idx = xlat_tables_va_to_index(table_base_va, table_idx_va, level); #if PLAT_XLAT_TABLES_DYNAMIC if (level > ctx->base_level)