@@ -130,7 +130,7 @@ vectors:
.word 0 /* dabt */
b into_hyp_mode /* hvc */
.word 0 /* irq */
- .word 0 /* fix */
+ .word 0 /* fiq */
into_hyp_mode:
@ Check CPU nr again
@@ -25,7 +25,7 @@
@
1:
ldr sp, =_monitor_stack
- push {r10-r12,lr}
+ push {r10-r12}
cmp r7, #0xffffffff
beq _non_sec
@@ -36,7 +36,8 @@
movnes pc, lr
and r12, r7, #0xf
cmp r12, #0x0
- ldmgtfd sp!, {r10-r12,pc}^
+ popgt {r10-r12}
+ movgts pc, lr
@ Check the VMID is 0
mrc p15, 0, r10, c1, c1, 0 @ SCR
@@ -48,7 +49,8 @@
lsr r11, r11, #16
and r11, r11, #0xff
cmp r11, #0
- ldmnefd sp!, {r10-r12,pc}^
+ popne {r10-r12}
+ movnes pc, lr
@ Jump to the right function
and r12, r7, #0xf
@@ -71,7 +73,8 @@ _non_sec:
ldr r11, =0x131
orr r12, r12, r11
mcr p15, 0, r12, c1, c1, 0
- ldmfd sp!, {r10-r12,pc}^
+ pop {r10-r12}
+ movs pc, lr
@
@ Read/Write HVBAR
@@ -82,7 +85,8 @@ _write_hvbar:
isb
mcr p15, 4, r0, c12, c0, 0
mcr p15, 0, r10, c1, c1, 0 @ Restore SCR
- ldmfd sp!, {r10-r12,pc}^
+ pop {r10-r12}
+ movs pc, lr
.ltorg