Added MMU and Cache support for Exception level 2. Solves performance issues.

Author: dgarske

IDE/XilinxSDK/README.md

@@ -149,12 +149,12 @@ Example boot.bif in workspace root:
 the_ROM_image:
 {
 	[bootloader, destination_cpu=a53-0] zcu102\zynqmp_fsbl\fsbl_a53.elf
-	[destination_cpu=a53-0, exception_level=el-1] wolfboot\Debug\wolfboot.elf
+	[destination_cpu=a53-0, exception_level=el-2] wolfboot\Debug\wolfboot.elf
 	[destination_cpu=a53-0, partition_owner=uboot, offset=0x800000] hello_world\Debug\hello_world_v1_signed.bin
 }
 ```
 
-Note: If using the wolfboot.elf compiled from our makefile use "exception_level=el-3".
+You can also use exception level 3 or 1 depending on your needs.
 
 From the workspace root:
 

src/boot_aarch64_start.S

@@ -161,7 +161,7 @@ InitEL3:
     orr    w1, w1, #(1 << 1)   /* Set IRQ bit (IRQs routed to EL3) */
     msr    SCR_EL3, x1
 
-    /*configure cpu auxiliary control register EL1 */
+    /* Configure cpu auxiliary control register EL1 */
     ldr    x0,=0x80CA000          /* L1 Data prefetch control - 5, Enable device split throttle, 2 independent data prefetch streams */
 #if CONFIG_ARM_ERRATA_855873
     /* Set ENDCCASCI bit in CPUACTLR_EL1 register, to execute data
@@ -171,11 +171,11 @@ InitEL3:
 #endif
     msr    S3_1_C15_C2_0, x0      /* CPUACTLR_EL1 */
 
-    /* program the counter frequency */
+    /* Program the counter frequency */
     ldr    x0,=counterfreq
     msr    CNTFRQ_EL0, x0
 
-    /*Enable hardware coherency between cores*/
+    /* Enable hardware coherency between cores */
     mrs    x0, S3_1_c15_c2_1      /* Read EL1 CPU Extended Control Register */
     orr    x0, x0, #(1 << 6)      /* Set the SMPEN bit */
     msr    S3_1_c15_c2_1, x0      /* Write EL1 CPU Extended Control Register */
@@ -203,11 +203,11 @@ InitEL3:
     msr    MAIR_EL3, x1
 
     /**********************************************
-     * Set up TCR_EL3
-     * Physical Address Size PS =  010 -> 40bits 1TB
-     * Granual Size TG0 = 00 -> 4KB
-     * size offset of the memory region T0SZ = 24 -> (region size 2^(64-24) = 2^40)
-     ***************************************************/
+    * Set up TCR_EL3
+    * Physical Address Size PS =  010 -> 40bits 1TB
+    * Granual Size TG0 = 00 -> 4KB
+    * size offset of the memory region T0SZ = 24 -> (region size 2^(64-24) = 2^40)
+    ***************************************************/
     ldr    x1,=0x80823518
 
     msr    TCR_EL3, x1
@@ -240,13 +240,59 @@ InitEL2:
     ldr    x1, =vector_base
     msr    VBAR_EL2, x1
 
-    mov	   x0, #0x33ff
-    msr	   CPTR_EL2, x0   /* Enable FP/SIMD */
-
     /* Define stack pointer for current exception level */
     ldr    x2,=EL2_stack
     mov    sp,x2
 
+    mov	   x0, #0x33ff
+    msr	   CPTR_EL2, x0   /* Enable FP/SIMD */
+
+    /* Invalidate TLB */
+    tlbi    alle2
+    /* Invalidate ICache */
+    ic     ialluis
+    isb    sy
+    /* Invalidate DCache */
+    bl     invalidate_dcaches
+    dsb     sy
+    isb
+
+    ldr    x1, =L0Table           /* Get address of level 0 for TTBR0_EL2 */
+    msr    TTBR0_EL2, x1          /* Set TTBR0_EL2 */
+
+    /**********************************************
+    * Set up memory attributes
+    * This equates to:
+    * 0 = b01000100 = Normal, Inner/Outer Non-Cacheable
+    * 1 = b11111111 = Normal, Inner/Outer WB/WA/RA
+    * 2 = b00000000 = Device-nGnRnE
+    * 3 = b00000100 = Device-nGnRE
+    * 4 = b10111011 = Normal, Inner/Outer WT/WA/RA
+    **********************************************/
+    ldr    x1, =0x000000BB0400FF44
+    msr    MAIR_EL2, x1
+
+    /**********************************************
+    * Set up TCR_EL2
+    * Physical Address Size PS =  010 -> 40bits 1TB
+    * Granual Size TG0 = 00 -> 4KB
+    * size offset of the memory region T0SZ = 24 -> (region size 2^(64-24) = 2^40)
+    ***************************************************/
+    ldr    x1,=0x80823518
+
+    msr    TCR_EL2, x1
+    isb
+
+    /* Enable ICache */
+    mrs    x1, SCTLR_EL2
+    orr    x1, x1, #(1 << 12)     /* Enable ICache */
+    orr    x1, x1, #(1 << 3)      /* Enable SP alignment check */
+    orr    x1, x1, #(1 << 2)      /* Enable DCaches */
+    orr    x1, x1, #(1 << 0)      /* Enable MMU */
+    msr    SCTLR_EL2, x1
+    dsb    sy
+    isb
+
     bl     boot_entry_C           /* jump to start */
 #else
     /* present exception level and selected exception level mismatch */
@@ -289,8 +335,7 @@ InitEL1:
     msr    SCTLR_EL1, x1
     isb
 
-    TLBI   VMALLE1
-
+    tlbi   VMALLE1
     ic     IALLU                  /* Invalidate I cache to PoU */
     bl     invalidate_dcaches
     dsb    sy
@@ -946,9 +991,7 @@ RestorePrevStatefiq:
 SErrorInterruptHandler:
 
     saveregister
-
     bl     SErrorInterrupt
-
     restoreregister
     exception_return