Use TZ SMC call to do aarch32 -> aarch64 execution state switch

As mentioned in the previous commit, the (proprietary) PSCI implementation
in Qualcomm's TZ firmware seems to have a bug which complicates booting
aarch64 kernels, unless we invoke its SMC call to switch execution state
from aarch32 to aarch64. If we bypass it, the secondary CPU cores will
be brought up in aarch32 state instead of aarch64 later.

Right now we work around that in the HYP firmware by ignoring if TZ tells
us to boot the secondary CPU cores in aarch32 state instead of aarch64.
We could probably adjust this further to record ourselves if EL1 should
be running in aarch64 or aarch32, and use that to drop the remaining FIXMEs.

But overall, I would argue that's it's better to avoid the PSCI bug
in the first place. Who knows what other problems might be caused by
not making TZ aware of the correct execution state of EL1?

To avoid the bug, we need to invoke TZ's SMC call at least once to make it
aware that EL1 will be running in aarch64 execution state from now on.
Unfortunately, TZ does not involve the hypervisor when switching states...
It seems to modify our HCR_EL2 register to enable aarch64 and always
returns to EL1 even if we do the SMC call from EL2.

So, somehow we need to jump back to EL2 immediately after TZ returns
to EL1. We could change the EL1 entry point to some custom code and
do a HVC call back into HYP from there. But then we also need to save
registers etc etc. As a "hypervisor" in EL2, there must be some way
to prevent EL1 from running, right?

Looking at the available hypervisor configuration options available
in HCR_EL2 I found "HCR_EL2.TGE" (Trap General Exceptions).
It does much more than we need but the promising part was

  "An exception return to EL1 is treated as an illegal exception return."

Unfortunately, setting it before doing the SMC call just results in a hang.
Reading a bit further, the illegal exception return is sent to the exception
handlers in EL3, not to EL2, because the exception happens during execution
of the "eret" instruction in EL3. Oh well.

What we need is a way to cause an exception to EL2 immediately *after*
the "eret" has completed, so once the CPU attempts to execute the first
instruction. After thinking about it for a while this is actually quite
simple.

Since EL2 is used to implement hypervisors, it has the "stage 2 address
translation" mechanism to provide each virtual machine with its own view
of memory. This isn't used in qhypstub because it does not implement
a hypervisor, and we don't prevent EL1 from accessing EL2 memory.

But actually, a simple way to cause an Instruction Abort from EL1 into EL2
is to enable stage 2 address translation without setting up any (valid)
translation tables. This means that there is effectively no physical memory
mapped, so once TZ returns to EL1 it is immediately forced back to EL2.

We can then handle that in our exception handler, read the faulting
instruction address, and jump to it from EL2. It turns out that overall
this actually requires less assembly instructions than the previous approach. :D

qhypstub.s

@@ -11,6 +11,9 @@
 .equ	STATE_AARCH32,	1
 .equ	STATE_AARCH64,	2
 
+/* Hypervisor Configuration Register (EL2) */
+.equ	HCR_EL2_VM,	1 << 0		/* enable stage 2 address translation */
+
 /* Saved Program Status Register (EL2) */
 .equ	SPSR_EL2_A,		1 << 8	/* SError interrupt mask */
 .equ	SPSR_EL2_I,		1 << 7	/* IRQ interrupt mask */
@@ -67,12 +70,10 @@ _start:
 	ldr	w3, [x0]
 	and	w3, w3, ~0b1	/* RPM_RESET_REMOVAL */
 	str	w3, [x0]
-	b	not_aarch64	/* FIXME */
 
 skip_init:
-	/* FIXME: Why is this always aarch32 suddenly? */
-	/*cmp	x1, STATE_AARCH64
-	bne	not_aarch64*/
+	cmp	x1, STATE_AARCH64
+	bne	not_aarch64
 
 	/* Jump to aarch64 directly in EL2! */
 	clrregs
@@ -119,30 +120,53 @@ hvc32:
 	mov	w15, 0x2000000	/* SMC32/HVC32 SiP Service Call */
 	movk	w15, 0x10f	/* something like "jump to kernel in aarch64" */
 	cmp	w0, w15
-	beq	hvc32_jump_aarch64
+	beq	smc_switch_aarch64
 	mov	w0, SMCCC_NOT_SUPPORTED
 	eret
 
-hvc32_jump_aarch64:
-	/* Jump to aarch64 in EL2 based on struct el1_system_param in LK scm.h */
-	cmp	w1, 0x12	/* MAKE_SCM_ARGS(0x2, SMC_PARAM_TYPE_BUFFER_READ) */
-	bne	hvc_invalid
-	cmp	w3, 10*8	/* size of struct, x0-x7 + lr * uint64_t */
-	bne	hvc_invalid
-
-	/* Load all registers and jump here directly in EL2! */
-	mov	w8, w2
-	ldp	x0, x1, [x8]
-	ldp	x2, x3, [x8, 1*2*8]
-	ldp	x4, x5, [x8, 2*2*8]
-	ldp	x6, x7, [x8, 3*2*8]
-	ldp	x8, lr, [x8, 4*2*8]
-	ret
+smc_switch_aarch64:
+	/*
+	 * Theoretically we could just jump to the entry point directly here in
+	 * EL2. However, in practice this does not work correctly. It seems like
+	 * TZ/PSCI records if we ever did the SMC call to switch to aarch64 state.
+	 * If we bypass it when booting aarch64 kernels, the other CPU cores
+	 * will be brought up in aarch32 state instead of aarch64 later.
+	 *
+	 * So, we do need to use the SMC call to switch to aarch64.
+	 * Unfortunately, TZ does not involve the hypervisor when switching states.
+	 * It modifies our HCR_EL2 register to enable aarch64, and returns in EL1
+	 * even if we do the SMC call here from EL2.
+	 *
+	 * So, somehow we need to jump back to EL2 immediately after the state
+	 * switch. The way we do this here is by temporarily activating stage 2
+	 * address translation (i.e. the way to protect hypervisor memory).
+	 * We don't bother setting up a valid translation table - the only goal
+	 * is to cause an Instruction Abort immediately after the state switch.
+	 */
+
+	/* Enable stage 2 address translation */
+	mov	x15, HCR_EL2_VM
+	msr	hcr_el2, x15
+
+	/* Let TZ switch to aarch64 and return to EL1 */
+	smc	0
 
-hvc_invalid:
-	mov	w0, SMCCC_INVALID_PARAMETER
+	/*
+	 * Something went wrong. Maybe parameter validation?
+	 * Disable stage 2 address translation again and return to EL1.
+	 */
+	msr	hcr_el2, xzr
 	eret
 
+finish_smc_switch_aarch64:
+	/*
+	 * We get here once TZ has switched EL1 to aarch64 execution state
+	 * and EL1 ran into the Instruction Abort.
+	 * Now, simply jump to the entry point directly in EL2!
+	 */
+	mrs	lr, elr_el2
+	ret
+
 /* EL2 exception vectors (written to VBAR_EL2) */
 .section .text.vectab
 .macro excvec label
@@ -171,6 +195,10 @@ el2_vector_table:
 	b	panic
 
 	excvec	el1_aarch64_sync
+	mrs	x30, esr_el2
+	lsr	x30, x30, 26	/* shift to exception class */
+	cmp	x30, 0b100000	/* Instruction Abort from lower EL? */
+	beq	finish_smc_switch_aarch64
 	b	panic
 	excvec	el1_aarch64_irq
 	b	panic