diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
index d8cabeb25f..950ba43c69 100644
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
@@ -37,11 +37,20 @@ ASM_PFX(_ModuleEntryPoint):
   mcr     p15, 0, r0, c1, c0, 0
  
   // Set CPU vectors to start of DRAM
-  mov     r0, #0x80000000
+  LoadConstantToReg (FixedPcdGet32(PcdCpuVectorBaseAddress) ,r0) // Get vector base
   mcr     p15, 0, r0, c12, c0, 0
   isb                               // Sync changes to control registers
 
-  /* before we call C code, lets setup the stack pointer in internal RAM*/
+  // 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:
 
   //
diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
index 414adcb266..033f03c684 100644
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
@@ -53,7 +53,7 @@ FillVectors
   cmp     r2, #32
   bne     FillVectors
    
-  /* before we call C code, lets setup the stack pointer in internal RAM*/
+  /* before we call C code, lets setup the stack pointer in internal RAM */
 stack_pointer_setup
 
   //