diff --git a/bsps/riscv/shared/mmu/mmu-setup.c b/bsps/riscv/shared/mmu/mmu-setup.c index 33400f1fac..1f798b6842 100644 --- a/bsps/riscv/shared/mmu/mmu-setup.c +++ b/bsps/riscv/shared/mmu/mmu-setup.c @@ -57,45 +57,48 @@ BSP_START_DATA_SECTION riscv_mmu_control riscv_mmu_instance = { .va_bits = RTEMS_XCONCAT(SV, RISCV_MMU_VIRTUAL_ADDRESS_BITS) }; +BSP_START_TEXT_SECTION static inline uintptr_t riscv_page_to_ppn( + uintptr_t page, + int level +) +{ + const uint32_t mask = ( 1 << RISCV_PGLEVEL_BITS ) - 1; + uintptr_t ppn = page; + + _Assert( level >= 0 ); + _Assert( level < 5 ); + + ppn >>= RISCV_PGLEVEL_BITS * level; + ppn &= mask; + ppn <<= PTE_PPN_SHIFT + RISCV_PGLEVEL_BITS * level; + + return ppn; +} + BSP_START_TEXT_SECTION static inline uintptr_t riscv_create_pte( riscv_mmu_control *control, uintptr_t address, uint32_t flags ) { - uintptr_t ppn0, ppn1; -#if __riscv_xlen == 64 - uintptr_t ppn2; -#endif - uintptr_t pte = 0; + uintptr_t pte = flags; uintptr_t page = address >> RISCV_PGSHIFT; - const uint32_t mask = (1 << RISCV_PGLEVEL_BITS) - 1; - ppn0 = page & mask; - ppn0 <<= PTE_PPN_SHIFT; + pte |= riscv_page_to_ppn( page, 0 ); + pte |= riscv_page_to_ppn( page, 1 ); - ppn1 = (page >> RISCV_PGLEVEL_BITS); -#if __riscv_xlen == 64 - ppn1 &= mask; -#else +#if __riscv_xlen == 32 || RISCV_MMU_VIRTUAL_ADDRESS_BITS == 32 if ( control->va_bits != SV32 ) { - bsp_fatal( RISCV_FATAL_MMU_CANNOT_MAP_BLOCK ); + bsp_fatal( RISCV_FATAL_MMU_CANNOT_MAP_BLOCK ); } -#endif - ppn1 <<= PTE_PPN_SHIFT + RISCV_PGLEVEL_BITS; - -#if __riscv_xlen == 64 - ppn2 = (page >> 2 * RISCV_PGLEVEL_BITS); - ppn2 <<= PTE_PPN_SHIFT + 2 * RISCV_PGLEVEL_BITS; +#else + pte |= riscv_page_to_ppn( page, 2 ); if ( control->va_bits != SV39 ) { - bsp_fatal( RISCV_FATAL_MMU_CANNOT_MAP_BLOCK ); + bsp_fatal( RISCV_FATAL_MMU_CANNOT_MAP_BLOCK ); } - pte |= ppn2; #endif - pte |= ppn1 | ppn0 | flags; - return pte; }