[PATCH v2 38/39] xen/riscv: implement continue_new_vcpu()

continue_new_vcpu() is the arch hook invoked the first time a freshly
created vCPU is scheduled. Implement both cases it has to cover:
 - for the idle vCPU, switch to its own stack and jump to idle_loop();
 - for a guest vCPU, restore hstatus and enter the guest through the new
   return_to_new_vcpu() path in entry.S, which loads sepc, passes the
   hart id in a0 and the DTB address in a1 as expected by the RISC-V
   boot protocol, sets sstatus.SPP and executes sret.

Interrupts have to stay disabled across the restore. The trap entry
logic implicitly clears hstatus.SPV, so an interrupt taken between the
write of hstatus and sret would make sret return to HS-mode instead of
VS-mode, and restoring SPV afterwards is non-trivial. Instead interrupts
are simply kept off and sstatus.SPIE is set, so that SIE is restored from
SPIE once sret has been executed. Also, it follows what hardware will do
with real CPU which is also started with interrupts disabled.

Introduce get_cpu_info() and reset_stack_and_jump() in asm/current.h,
needed by the above. get_cpu_info() is a macro rather than a static
inline because asm/current.h is pulled in by <xen/percpu.h> before
this_cpu() is defined and before <xen/sched.h> completes struct vcpu.

idle_loop() is added as a stub on purpose; its real implementation will
come separately later.

Signed-off-by: Oleksii Kurochko <[email protected]>
---
Changes in v2:
 - New patch.
---
---
 xen/arch/riscv/domain.c              | 44 +++++++++++++++++++++++++++-
 xen/arch/riscv/entry.S               | 23 +++++++++++++++
 xen/arch/riscv/include/asm/current.h |  4 +++
 3 files changed, 70 insertions(+), 1 deletion(-)

diff --git a/xen/arch/riscv/domain.c b/xen/arch/riscv/domain.c
index 29181968224c..0782148b7207 100644
--- a/xen/arch/riscv/domain.c
+++ b/xen/arch/riscv/domain.c
@@ -8,10 +8,13 @@
 #include <xen/smp.h>
 #include <xen/vmap.h>
 
+#include <asm/aia.h>
+#include <asm/aplic.h>
 #include <asm/bitops.h>
 #include <asm/cpufeature.h>
 #include <asm/csr.h>
 #include <asm/current.h>
+#include <asm/imsic.h>
 #include <asm/intc.h>
 #include <asm/mmio.h>
 #include <asm/riscv_encoding.h>
@@ -140,9 +143,43 @@ static void vcpu_csr_init(struct vcpu *v)
     v->arch.hie = MIP_SGEIP;
 }
 
+static void schedule_tail(struct vcpu *prev);
+static void noreturn idle_loop(void);
+void noreturn return_to_new_vcpu(void);
+
 static void continue_new_vcpu(struct vcpu *prev)
 {
-    BUG_ON("unimplemented\n");
+    schedule_tail(prev);
+
+    if ( is_idle_vcpu(current) )
+        reset_stack_and_jump(idle_loop);
+    else
+    {
+        /*
+         * During a context switch to a new vCPU, interrupts must be disabled
+         * to guarantee that the vCPU's CSR state can be safely restored into
+         * the hart without being clobbered by an interrupt trap.
+         *
+         * For example, when return_to_new_vcpu() finishes, it executes sret.
+         * At that point, the hart checks hstatus.SPV=1 and sstatus.SPP=1 in
+         * order to return from HS-mode into VS-mode. If an interrupt were to
+         * arrive before sret, the trap entry logic would implicitly clear
+         * hstatus.SPV to 0. Correctly restoring it afterwards is non-trivial,
+         * and if left as 0, sret would incorrectly return to HS-mode instead
+         * of VS-mode.
+         *
+         * To avoid this, interrupts are kept disabled during the restore.
+         * Additionally, setting sstatus.SPIE=1 ensures that after sret is
+         * executed (as sstatus.SIE will be loaded from SPIE), HS-mode will
+         * continue to receive interrupts normally.
+         */
+        local_irq_disable();
+        csr_set(CSR_SSTATUS, SSTATUS_SPIE);
+
+        csr_write(CSR_HSTATUS, vcpu_guest_cpu_user_regs(current)->hstatus);
+
+        reset_stack_and_jump(return_to_new_vcpu);
+    }
 }
 
 int arch_vcpu_create(struct vcpu *v)
@@ -551,3 +588,8 @@ static void __init __maybe_unused build_assertions(void)
      */
     BUILD_BUG_ON(offsetof(struct cpu_info, guest_cpu_user_regs));
 }
+
+static void noreturn idle_loop(void)
+{
+    BUG_ON("unimplemented");
+}
diff --git a/xen/arch/riscv/entry.S b/xen/arch/riscv/entry.S
index 331446a238d3..bf1843dcea4f 100644
--- a/xen/arch/riscv/entry.S
+++ b/xen/arch/riscv/entry.S
@@ -143,3 +143,26 @@ FUNC(__context_switch)
 
         ret
 END(__context_switch)
+
+/* t0 is used as a temporary reg and is clobbered to oblivion */
+FUNC(return_to_new_vcpu)
+        /* Swap tp with sscratch */
+        csrrw   tp, CSR_SSCRATCH, tp
+
+        /* Set vCPU registers */
+        REG_L   t0, CPU_USER_REGS_SEPC(sp)
+        csrw    sepc, t0
+
+        /* Hartid goes to a0 */
+        REG_L   a0, CPU_USER_REGS_A0(sp)
+
+        /* DTB goes to a1 */
+        REG_L   a1, CPU_USER_REGS_A1(sp)
+
+        /* Set guest mode to supervisor */
+        li      t0, SSTATUS_SPP
+        csrs    CSR_SSTATUS, t0
+
+        /* Enter guest */
+        sret
+END(return_to_new_vcpu)
diff --git a/xen/arch/riscv/include/asm/current.h b/xen/arch/riscv/include/asm/current.h
index 78ec52fd8a35..f8babcc3d926 100644
--- a/xen/arch/riscv/include/asm/current.h
+++ b/xen/arch/riscv/include/asm/current.h
@@ -47,6 +47,8 @@ DECLARE_PER_CPU(struct vcpu *, curr_vcpu);
 #define set_current(vcpu)  do { current = (vcpu); } while (0)
 #define get_cpu_current(cpu)  per_cpu(curr_vcpu, cpu)
 
+#define get_cpu_info() (current->arch.cpu_info)
+
 #define guest_cpu_user_regs() ({ BUG_ON("unimplemented"); NULL; })
 #define vcpu_guest_cpu_user_regs(vcpu) \
     (&(vcpu)->arch.cpu_info->guest_cpu_user_regs)
@@ -58,6 +60,8 @@ DECLARE_PER_CPU(struct vcpu *, curr_vcpu);
     unreachable();                                          \
 } while ( false )
 
+#define reset_stack_and_jump(fn) switch_stack_and_jump(get_cpu_info(), fn)
+
 #define get_per_cpu_offset() __per_cpu_offset[smp_processor_id()]
 
 #endif /* __ASSEMBLER__ */
-- 
2.55.0