Cleanup Cache an MMU operations.

[mirror_edk2.git] / BeagleBoardPkg / Sec / Arm / ModuleEntryPoint.S

diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
old mode 100755 (executable)
new mode 100644 (file)
index 0ae8da6..950ba43
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
@@ -35,12 +35,22 @@ ASM_PFX(_ModuleEntryPoint):
   orr     r0, r0, #0x00000002     /* set bit 1 (A) Align */\r
   orr     r0, r0, #0x00001000     /* set bit 12 (I) enable I-Cache */\r
   mcr     p15, 0, r0, c1, c0, 0\r
-\r
+ \r
   // Set CPU vectors to start of DRAM\r
-  mov     r0, #0x80000000\r
+  LoadConstantToReg (FixedPcdGet32(PcdCpuVectorBaseAddress) ,r0) // Get vector base\r
   mcr     p15, 0, r0, c12, c0, 0\r
-\r
-  /* before we call C code, lets setup the stack pointer */\r
+  isb                               // Sync changes to control registers\r
+\r
+  // Fill vector table with branchs to current pc (jmp $)\r
+  ldr     r1, ShouldNeverGetHere\r
+  movs    r2, #0\r
+FillVectors:\r
+  str     r1, [r0, r2]\r
+  adds    r2, r2, #4\r
+  cmp     r2, #32\r
+  bne     FillVectors\r
+   \r
+  /* before we call C code, lets setup the stack pointer in internal RAM */\r
 stack_pointer_setup:\r
 \r
   //\r
@@ -51,43 +61,17 @@ stack_pointer_setup:
   LoadConstantToReg (FixedPcdGet32(PcdPrePiStackSize) ,r3)    /* stack size arg3  */\r
   add     r4, r2, r3\r
 \r
-  //Enter IRQ mode and set up IRQ stack pointer\r
-  mov     r0,#0x12|0x80|0x40\r
-  msr     CPSR_c,r0\r
-  mov     r13,r4\r
-\r
-  //Enter Abort mode and set up Abort stack pointer\r
-  mov     r0,#0x17|0x80|0x40\r
-  msr     CPSR_c,r0\r
-  sub     r4, r4, #0x400\r
-  mov     r13,r4\r
-\r
-  //Enter Undefined mode and set up Undefined stack pointer\r
-  mov     r0,#0x1b|0x80|0x40\r
-  msr     CPSR_c,r0\r
-  sub     r4, r4, #0x400\r
-  mov     r13,r4\r
-\r
   //Enter SVC mode and set up SVC stack pointer\r
   mov     r0,#0x13|0x80|0x40\r
   msr     CPSR_c,r0\r
-  sub     r4, r4, #0x400\r
-  mov     r13,r4\r
-\r
-  //Enter System mode and set up System stack pointer\r
-  mov     r0,#0x1f|0x80|0x40\r
-  msr     CPSR_c,r0\r
-  sub     r4, r4, #0x400\r
   mov     r13,r4\r
 \r
   // Call C entry point\r
-  mov     r0, #0x80000000   /* memory base arg0          */\r
-  mov     r1, #0x10000000   /* memory size arg1          */\r
-\r
-  bl      ASM_PFX(CEntryPoint) /* Assume C code is ARM      */\r
+  LoadConstantToReg (FixedPcdGet32(PcdMemorySize) ,r1)    /* memory size arg1          */\r
+  LoadConstantToReg (FixedPcdGet32(PcdMemoryBase) ,r0)    /* memory size arg0         */\r
+  blx      ASM_PFX(CEntryPoint) /* Assume C code is thumb    */\r
 \r
 ShouldNeverGetHere:\r
   /* _CEntryPoint should never return */\r
   b       ShouldNeverGetHere\r
-\r
   \r