diff --git a/BeagleBoardPkg/Sec/Arm/Macro.inc b/BeagleBoardPkg/Sec/Arm/Macro.inc
deleted file mode 100755
index cacfef976a..0000000000
--- a/BeagleBoardPkg/Sec/Arm/Macro.inc
+++ /dev/null
@@ -1,67 +0,0 @@
-//%HEADER%
-  MACRO
-  MmioWrite32Macro $Address, $Data
-  ldr  r1, = ($Address)               
-  ldr  r0, = ($Data)                
-  str  r0, [r1] 
-  MEND
-    
-  MACRO
-  MmioOr32Macro $Address, $OrData
-  ldr  r1, =($Address)              
-  ldr  r2, =($OrData)               
-  ldr  r0, [r1]                     
-  orr  r0, r0, r2                   
-  str  r0, [r1]
-  MEND
-
-  MACRO
-  MmioAnd32Macro $Address, $AndData
-  ldr  r1, =($Address)                
-  ldr  r2, =($AndData)                
-  ldr  r0, [r1]                       
-  and  r0, r0, r2                     
-  str  r0, [r1]
-  MEND
-
-  MACRO
-  MmioAndThenOr32Macro $Address, $AndData, $OrData
-  ldr  r1, =($Address)                        
-  ldr  r0, [r1]                                       
-  ldr  r2, =($AndData)                                
-  and  r0, r0, r2                                     
-  ldr  r2, =($OrData)                                 
-  orr  r0, r0, r2                                     
-  str  r0, [r1]         
-  MEND
-
-  MACRO
-  MmioWriteFromReg32Macro $Address, $Reg
-  ldr  r1, =($Address)  
-  str  $Reg, [r1]       
-  MEND
-
-  MACRO
-  MmioRead32Macro $Address  
-  ldr  r1, =($Address)        
-  ldr  r0, [r1]  
-  MEND
-
-  MACRO
-  MmioReadToReg32Macro $Address, $Reg
-  ldr  r1, =($Address)                  
-  ldr  $Reg, [r1]       
-  MEND
-
-  MACRO
-  LoadConstantMacro $Data
-  ldr  r0, =($Data)
-  MEND
-
-  MACRO
-  LoadConstantToRegMacro $Data, $Reg
-  ldr  $Reg, =($Data)
-  MEND
-  
-  END
-  
\ No newline at end of file
diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
index 882aed469c..d8cabeb25f 100644
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.S
@@ -39,6 +39,8 @@ ASM_PFX(_ModuleEntryPoint):
   // Set CPU vectors to start of DRAM
   mov     r0, #0x80000000
   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*/
 stack_pointer_setup:
 
diff --git a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
index 52171692b5..eef6c3dc6b 100644
--- a/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
+++ b/BeagleBoardPkg/Sec/Arm/ModuleEntryPoint.asm
@@ -40,8 +40,19 @@ _ModuleEntryPoint
   mcr     p15, 0, r0, c1, c0, 0
  
   // Set CPU vectors to start of DRAM
-  mov     r0, #0x80000000
+  LoadConstantToReg (FixedPcdGet32(PcdMemoryBase) ,r0) /* memory size arg0         */
   mcr     p15, 0, r0, c12, c0, 0
+  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
 
@@ -54,13 +65,13 @@ stack_pointer_setup
   add     r4, r2, r3
 
   //Enter SVC mode and set up SVC stack pointer
-  mov     r0,#0x13|0x80|0x40
-  msr     CPSR_c,r0
+  mov     r5,#0x13|0x80|0x40
+  msr     CPSR_c,r5
   mov     r13,r4
 
   // Call C entry point
   LoadConstantToReg (FixedPcdGet32(PcdMemorySize) ,r1)    /* memory size arg1          */
-  LoadConstantToReg (FixedPcdGet32(PcdMemoryBase) ,r0)    /* memory size arg0         */
+//  LoadConstantToReg (FixedPcdGet32(PcdMemoryBase) ,r0)  Done above  
   blx     CEntryPoint       /* Assume C code is thumb    */
 
 ShouldNeverGetHere