and r5, r0, r1\r
\r
_SetSVCMode\r
- // Enter SVC mode\r
+ // Enter SVC mode, Disable FIQ and IRQ\r
mov r1, #0x13|0x80|0x40\r
msr CPSR_c, r1\r
\r
\r
// Is it the Primary Core ?\r
LoadConstantToReg (FixedPcdGet32(PcdArmPrimaryCore), r4)\r
- cmp r0, r4\r
+ cmp r5, r4\r
beq _SetupPrimaryCoreStack\r
\r
_SetupSecondaryCoreStack\r
mov r3, sp\r
mov r2, #0x0\r
_InitGlobals\r
- str r2, [r3], #4\r
cmp r3, r1\r
- blt _InitGlobals\r
-\r
+ beq _PrepareArguments\r
+ str r2, [r3], #4\r
+ b _InitGlobals\r
\r
_PrepareArguments\r
mov r0, r5\r