core: riscv: Modify S-mode boot flow to compatible with M-mode firmware

In system of S-mode OP-TEE, M-mode firmware transfers control flow to
OP-TEE during boot. M-mode runs one hart, which called boot hart, to
executes system initialization and jumps into OP-TEE.

However, the ID of boot hart might not be always zero. That is, M-mode
firmware may choose a hart, whose ID is non-zero, to jump into OP-TEE
"_start" symbol to do OP-TEE initialization. Therefore, it is not
feasible to let all harts enter "_start" symbol, and judge whether
current hart is primary boot hart, by checking ID is zero or not. Fix
it by letting non-boot hart enter "reset_secondary", in S-mode OP-TEE.
So that all the harts can do their right things regardless of their
hart ID.

Only OP-TEE runs in M-mode will judge primary hart by checking whether
the hart ID is zero.

Signed-off-by: Alvin Chang <[email protected]>

core/arch/riscv/kernel/entry.S

@@ -148,7 +148,10 @@ FUNC _start , :
 #else
 	mv	s1, a1		/* Save device tree address into s1 */
 #endif
+
+#ifdef CFG_RISCV_M_MODE
 	bnez	a0, reset_secondary
+#endif
 	jal	reset_primary
 	j	.
 END_FUNC _start
@@ -170,7 +173,7 @@ UNWIND(	.cantunwind)
 	bne	t0, t1, 0b
 1:
 #ifdef CFG_RISCV_S_MODE
-	lla	t0, _start
+	lla	t0, reset_secondary
 	lla	t1, start_addr
 	STR	t0, (t1)
 #endif
@@ -231,13 +234,27 @@ END_FUNC reset_primary
 LOCAL_FUNC reset_secondary , : , .identity_map
 UNWIND(	.cantunwind)
 	wait_primary
+
+#ifdef CFG_RISCV_S_MODE
+.option push
+.option norelax
+	la	gp, __global_pointer$
+.option pop
+	csrw	CSR_XSCRATCH, a0
+#endif
+
 	csrw	CSR_SATP, zero
 	set_sp
 	set_tp
 	set_satp
 	cpu_is_ready
 
 	jal	boot_init_secondary
+#ifdef CFG_RISCV_S_MODE
+	/* Return to untrusted domain */
+	li	a0, TEEABI_OPTEED_RETURN_ON_DONE
+	j	thread_return_to_ree
+#endif
 	j	.
 END_FUNC reset_secondary