X-Git-Url: https://git.proxmox.com/?p=mirror_edk2.git;a=blobdiff_plain;f=BeagleBoardPkg%2FSec%2FArm%2FModuleEntryPoint.S;h=07e054732f264957b4966d04bf27ce482e7c0e5d;hp=0ae8da6a24b5a1829d4f0de974a9f4a2fda30e34;hb=2ed3c9ccf8c056aa4df35fcd29f670ee1238fbd0;hpb=a495774f6982581e9591fe9995f2e0a4bf1854fa

diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
index 0ae8da6a24..07e054732f 100644
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
@@ -36,11 +36,29 @@ ASM_PFX(_ModuleEntryPoint):
   orr     r0, r0, #0x00001000     /* set bit 12 (I) enable I-Cache */
   mcr     p15, 0, r0, c1, c0, 0
 
+  // Enable NEON register in case folks want to use them for optimizations (CopyMem)
+  mrc     p15, 0, r0, c1, c0, 2
+  orr     r0, r0, #0x00f00000   // Enable VPF access (V* instructions)
+  mcr     p15, 0, r0, c1, c0, 2
+  mov     r0, #0x40000000       // Set EN bit in FPEXC
+  mcr     p10,#0x7,r0,c8,c0,#0  // msr     FPEXC,r0 in ARM assembly
+   
+ 
   // Set CPU vectors to start of DRAM
-  mov     r0, #0x80000000
+  LoadConstantToReg (FixedPcdGet32(PcdCpuVectorBaseAddress) ,r0) // Get vector base
   mcr     p15, 0, r0, c12, c0, 0
-
-  /* before we call C code, lets setup the stack pointer */
+  isb                               // Sync changes to control registers
+
+  // Fill vector table with branchs to current pc (jmp $)
+  ldr     r1, ShouldNeverGetHere
+  movs    r2, #0
+FillVectors:
+  str     r1, [r0, r2]
+  adds    r2, r2, #4
+  cmp     r2, #32
+  bne     FillVectors
+   
+  /* before we call C code, lets setup the stack pointer in internal RAM */
 stack_pointer_setup:
 
   //
@@ -51,43 +69,17 @@ stack_pointer_setup:
   LoadConstantToReg (FixedPcdGet32(PcdPrePiStackSize) ,r3)    /* stack size arg3  */
   add     r4, r2, r3
 
-  //Enter IRQ mode and set up IRQ stack pointer
-  mov     r0,#0x12|0x80|0x40
-  msr     CPSR_c,r0
-  mov     r13,r4
-
-  //Enter Abort mode and set up Abort stack pointer
-  mov     r0,#0x17|0x80|0x40
-  msr     CPSR_c,r0
-  sub     r4, r4, #0x400
-  mov     r13,r4
-
-  //Enter Undefined mode and set up Undefined stack pointer
-  mov     r0,#0x1b|0x80|0x40
-  msr     CPSR_c,r0
-  sub     r4, r4, #0x400
-  mov     r13,r4
-
   //Enter SVC mode and set up SVC stack pointer
   mov     r0,#0x13|0x80|0x40
   msr     CPSR_c,r0
-  sub     r4, r4, #0x400
-  mov     r13,r4
-
-  //Enter System mode and set up System stack pointer
-  mov     r0,#0x1f|0x80|0x40
-  msr     CPSR_c,r0
-  sub     r4, r4, #0x400
   mov     r13,r4
 
   // Call C entry point
-  mov     r0, #0x80000000   /* memory base arg0          */
-  mov     r1, #0x10000000   /* memory size arg1          */
-
-  bl      ASM_PFX(CEntryPoint) /* Assume C code is ARM      */
+  LoadConstantToReg (FixedPcdGet32(PcdMemorySize) ,r1)    /* memory size arg1          */
+  LoadConstantToReg (FixedPcdGet32(PcdMemoryBase) ,r0)    /* memory size arg0         */
+  blx      ASM_PFX(CEntryPoint) /* Assume C code is thumb    */
 
 ShouldNeverGetHere:
   /* _CEntryPoint should never return */
   b       ShouldNeverGetHere
-