+#ifdef MMU_SUPPORT
+ ! Set up the VM using the MMU and caches
+
+ ! .vm_ep is first instruction to execute
+ ! after VM initialization
+ pt/l .vm_ep, tr1
+
+ ! Configure instruction cache (ICCR)
+ movi 3, r2
+ movi 0, r3
+ LOAD_ADDR (ICCR_BASE, r1)
+ putcfg r1, 0, r2
+ putcfg r1, 1, r3
+
+ ! movi 7, r2 ! write through
+ ! Configure operand cache (OCCR)
+ LOAD_ADDR (OCCR_BASE, r1)
+ putcfg r1, 0, r2
+ putcfg r1, 1, r3
+
+ ! Disable all PTE translations
+ LOAD_ADDR (MMUIR_BASE, r1)
+ LOAD_ADDR (MMUDR_BASE, r2)
+ movi 64, r3
+ pt/l .disable_ptes_loop, tr0
+.disable_ptes_loop:
+ putcfg r1, 0, r63
+ putcfg r2, 0, r63
+ addi r1, 16, r1
+ addi r2, 16, r2
+ addi r3, -1, r3
+ bgt r3, r63, tr0
+
+ LOAD_ADDR (MMUIR_BASE, r1)
+
+ ! FEMI instruction mappings
+ ! Area 0 - 1Mb cacheable at 0x00000000
+ ! Area 1 - None
+ ! Area 2 - 1Mb cacheable at 0x05000000
+ ! - 1Mb cacheable at 0x05100000
+ ! Area 3 - None
+ ! Area 4 - None
+
+ ! Map a 1Mb page for instructions at 0x00000000
+ LOAD_PTEH_VAL (FEMI_AREA_0, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_AREA_0, ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRX | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1Mb page for instructions at 0x05000000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (FEMI_AREA_2, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_AREA_2, ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRX | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1Mb page for instructions at 0x05100000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((FEMI_AREA_2+0x100000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((FEMI_AREA_2+0x100000), ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRX | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 512M page for instructions at EMI base
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (EMI_BASE, ALIGN_512MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (EMI_BASE, ALIGN_512MB, PTE_CB_CACHEABLE_WB | PTE_SZ_512MB | PTE_PRX | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for instructions at DM_DB_DLINK_BASE
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DM_DB_DLINK_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DM_DB_DLINK_BASE, ALIGN_4KB, PTE_CB_CACHEABLE_WB | PTE_SZ_4KB | PTE_PRX | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ LOAD_ADDR (MMUDR_BASE, r1)
+
+ ! FEMI data mappings
+ ! Area 0 - 1Mb cacheable at 0x00000000
+ ! Area 1 - 1Mb device at 0x04000000
+ ! Area 2 - 1Mb cacheable at 0x05000000
+ ! - 1Mb cacheable at 0x05100000
+ ! Area 3 - None
+ ! Area 4 - None
+ ! CB - 1Mb device at 0x08000000
+
+ ! Map a 1Mb page for data at 0x00000000
+ LOAD_PTEH_VAL (FEMI_AREA_0, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_AREA_0, ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1Mb page for data at 0x04000000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (FEMI_AREA_1, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_AREA_1, ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1Mb page for data at 0x05000000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (FEMI_AREA_2, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_AREA_2, ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1Mb page for data at 0x05100000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((FEMI_AREA_2+0x100000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((FEMI_AREA_2+0x100000), ALIGN_1MB, PTE_CB_CACHEABLE_WB | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for registers at 0x08000000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (FEMI_CB, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (FEMI_CB, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 512M page for data at EMI
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (EMI_BASE, ALIGN_512MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (EMI_BASE, ALIGN_512MB, PTE_CB_CACHEABLE_WB | PTE_SZ_512MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for DYNACON at DYNACON_BASE
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DYNACON_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DYNACON_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for instructions at DM_DB_DLINK_BASE
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DM_DB_DLINK_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DM_DB_DLINK_BASE, ALIGN_4KB, PTE_CB_CACHEABLE_WB | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for data at DM_DB_DLINK_BASE+0x1000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_DB_DLINK_BASE+0x1000), ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_DB_DLINK_BASE+0x1000), ALIGN_4KB, PTE_CB_UNCACHEABLE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for stack DM_DB_DLINK_BASE+0x2000
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_DB_DLINK_BASE+0x2000), ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_DB_DLINK_BASE+0x2000), ALIGN_4KB, PTE_CB_CACHEABLE_WB | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1M page for DM_CB_BASE2 at DM_CB_DLINK
+ ! 0x0c000000 - 0x0c0fffff
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DM_CB_DLINK_BASE, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DM_CB_DLINK_BASE, ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1M page for DM_CB_BASE2 at DM_CB_DLINK
+ ! 0x0c100000 - 0x0c1fffff
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_CB_DLINK_BASE+0x100000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_CB_DLINK_BASE+0x100000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1M page for DM_CB_BASE2 at DM_CB_DLINK
+ ! 0x0c200000 - 0x0c2fffff
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_CB_DLINK_BASE+0x200000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_CB_DLINK_BASE+0x200000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1M page for DM_CB_BASE2 at DM_CB_DLINK
+ ! 0x0c400000 - 0x0c4fffff
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_CB_DLINK_BASE+0x400000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_CB_DLINK_BASE+0x400000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 1M page for DM_CB_BASE2 at DM_CB_DLINK
+ ! 0x0c800000 - 0x0c8fffff
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((DM_CB_DLINK_BASE+0x800000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((DM_CB_DLINK_BASE+0x800000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map a 4K page for DMA control registers
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DMA_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DMA_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map lots of 4K pages for peripherals
+
+ ! /* peripheral */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (PERIPH_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (PERIPH_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* dmac */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (DMAC_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (DMAC_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* intc */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (INTC_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (INTC_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* rtc */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (RTC_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (RTC_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* dmac */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (TMU_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (TMU_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* scif */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (SCIF_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (SCIF_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ ! /* cprc */
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (CPRC_BASE, ALIGN_4KB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (CPRC_BASE, ALIGN_4KB, PTE_CB_DEVICE | PTE_SZ_4KB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Map CPU WPC registers
+ addi r1, 16, r1
+ LOAD_PTEH_VAL (CPU_BASE, ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL (CPU_BASE, ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+ addi r1, 16, r1
+
+ LOAD_PTEH_VAL ((CPU_BASE+0x100000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((CPU_BASE+0x100000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((CPU_BASE+0x200000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((CPU_BASE+0x200000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ addi r1, 16, r1
+ LOAD_PTEH_VAL ((CPU_BASE+0x400000), ALIGN_1MB, PTE_ENABLED | PTE_NOT_SHARED, r25, r2)
+ LOAD_PTEL_VAL ((CPU_BASE+0x400000), ALIGN_1MB, PTE_CB_DEVICE | PTE_SZ_1MB | PTE_PRR | PTE_PRW | PTE_PRU, r25, r3)
+ SET_PTE (r1, r2, r3)
+
+ ! Switch over to virtual addressing and enabled cache
+ getcon sr, r1
+ movi 1, r2
+ shlli r2, SR_BL_BIT, r2
+ or r1, r2, r1
+ putcon r1, ssr
+ getcon sr, r1
+ movi 1, r2
+ shlli r2, SR_MMU_BIT, r2
+ or r1, r2, r1
+ putcon r1, ssr
+ gettr tr1, r1
+ putcon r1, spc
+ synco
+ rte
+
+ ! VM entry point. From now on, we are in VM mode.
+.vm_ep:
+
+ ! Install the trap handler, by seeding vbr with the
+ ! correct value, and by assigning sr.bl = 0.
+
+ LOAD_ADDR (vbr_start, r1)
+ putcon r1, vbr
+ movi ~(1<<28), r1
+ getcon sr, r2
+ and r1, r2, r2
+ putcon r2, sr
+#endif /* MMU_SUPPORT */
+