Implement HVC call to switch execution state to aarch64 in EL2

Unfortunately, the main CPU core is still booting the arm64 Linux
kernel in EL1... Looking at the LK code (aboot.c and scm.c) the
execution state switch to aarch64 happens using a SMC call to TZ
(scm_elexec_call). The switch does not seem to involve the HYP
firmware though, so we have no good way to intercept that to jump
to the aarch64 kernel directly in EL2.

Actually, there should be no need to involve TZ for the aarch32
-> aarch64 execution state switch, since we can just do it from EL2.
The only requirement is that LK asks the HYP firmware to do the switch
with a HVC call instead of a SMC call.

To keep the LK changes simple, I have re-implemented the exact same
SMC call as a HVC call. This means LK can reuse all the code
and just needs to do "hvc" instead of "smc".

Implementing the HVC call is fairly simple, we need to:

  1. Add an exception vector table
  2. Check for exceptions caused by HVC instructions
  3. Compare the command in w0 with the expected one (0x200010f)
  4. Do some parameter validation
  5. Load all the registers from the passed struct el1_system_param
  6. And finally, jump to the new aarch64 kernel directly in EL2!

This works for the main CPU, but for some weird reason SMP is broken
now and the device reboots during SMP bring-up. :(

Further debugging shows that TZ tells our HYP firmware to boot
all other CPU cores in aarch32 state instead of aarch64. I guess it
records if we ever did the SMC call to switch to aarch64 state... :/

For now I just hack the code to forcibly boot all secondary CPU
in aarch64 state. A better solution is needed for this but it works
quite well for now. All CPUs finally started in EL2!

  smp: Bringing up secondary CPUs ...
  CPU1: Booted secondary processor 0x0000000001 [0x410fd030]
  CPU2: Booted secondary processor 0x0000000002 [0x410fd030]
  CPU3: Booted secondary processor 0x0000000003 [0x410fd030]
  smp: Brought up 1 node, 4 CPUs
  SMP: Total of 4 processors activated.
  CPU: All CPU(s) started at EL2
   ...
  kvm [1]: IPA Size Limit: 40 bits
  kvm [1]: Hyp mode initialized successfully

Even KVM seems to be mostly working. Great! Mission accomplished :)

qhypstub.ld

@@ -7,6 +7,10 @@ OUTPUT_ARCH(aarch64)
 SECTIONS {
 	. = 0x86400000;
 
+	/* Keep .data before exception vector table to make use of the padding */
 	.text : { *(.text) }
 	.data : { *(.data) }
+
+	/* Exception vector table must be aligned to 0x800 */
+	.text.vectab : ALIGN(0x800) { *(.text.vectab) }
 }

qhypstub.s

@@ -25,6 +25,10 @@
 /* Architectural Feature Trap Register (EL2) */
 .equ	CPTR_EL2_RES1,		1 << 13 | 1 << 12 | 1 << 9 | 1 << 8 | 0xff
 
+/* SMC Calling Convention return codes */
+.equ	SMCCC_NOT_SUPPORTED,		-1
+.equ	SMCCC_INVALID_PARAMETER,	-3
+
 /*
  * HYP entry point. This is called by TZ to initialize the CPU EL2 states
  * on initial boot-up and whenever a CPU core is turned back on after a power
@@ -63,10 +67,12 @@ _start:
 	ldr	w3, [x0]
 	and	w3, w3, ~0b1	/* RPM_RESET_REMOVAL */
 	str	w3, [x0]
+	b	not_aarch64	/* FIXME */
 
 skip_init:
-	cmp	x1, STATE_AARCH64
-	bne	not_aarch64
+	/* FIXME: Why is this always aarch32 suddenly? */
+	/*cmp	x1, STATE_AARCH64
+	bne	not_aarch64*/
 
 	/* Jump to aarch64 directly in EL2! */
 	clrregs
@@ -91,6 +97,10 @@ not_aarch64:
 	msr	cptr_el2, x3
 	msr	hstr_el2, xzr
 
+	/* Set exception vector table for initial execution state switch */
+	adr	x0, el2_vector_table
+	msr	vbar_el2, x0
+
 	/* Configure EL1 return address and return! */
 	msr	elr_el2, lr
 	clrregs
@@ -99,6 +109,91 @@ not_aarch64:
 panic:
 	b	panic
 
+hvc32:
+	/*
+	 * Right now we only handle one SMC/HVC call here, which is used to
+	 * jump to a aarch64 kernel from a aarch32 bootloader. The difference
+	 * is that we will try entering the kernel in EL2, while TZ/SMC
+	 * would enter in EL1.
+	 */
+	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
+	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
+
+hvc_invalid:
+	mov	w0, SMCCC_INVALID_PARAMETER
+	eret
+
+/* EL2 exception vectors (written to VBAR_EL2) */
+.section .text.vectab
+.macro excvec label
+	/* Each exception vector is 32 instructions long, so 32*4 = 2^7 bytes */
+	.align 7
+\label:
+.endm
+
+el2_vector_table:
+	excvec	el2_sp0_sync
+	b	panic
+	excvec	el2_sp0_irq
+	b	panic
+	excvec	el2_sp0_fiq
+	b	panic
+	excvec	el2_sp0_serror
+	b	panic
+
+	excvec	el2_sp2_sync
+	b	panic
+	excvec	el2_sp2_irq
+	b	panic
+	excvec	el2_sp2_fiq
+	b	panic
+	excvec	el2_sp2_serror
+	b	panic
+
+	excvec	el1_aarch64_sync
+	b	panic
+	excvec	el1_aarch64_irq
+	b	panic
+	excvec	el1_aarch64_fiq
+	b	panic
+	excvec	el1_aarch64_serror
+	b	panic
+
+	excvec	el1_aarch32_sync
+	mrs	x15, esr_el2
+	lsr	x15, x15, 26	/* shift to exception class */
+	cmp	x15, 0b010010	/* HVC instruction? */
+	beq	hvc32
+	b	panic
+	excvec	el1_aarch32_irq
+	b	panic
+	excvec	el1_aarch32_fiq
+	b	panic
+	excvec	el1_aarch32_serror
+	b	panic
+
+	excvec	el2_vector_table_end
+
 .data
 execution_state:
 	.byte	0