diff options
| -rw-r--r-- | src/arch/riscv/payload.c | 25 |
1 files changed, 20 insertions, 5 deletions
diff --git a/src/arch/riscv/payload.c b/src/arch/riscv/payload.c index f3ed5a44a6..903e8a6ab6 100644 --- a/src/arch/riscv/payload.c +++ b/src/arch/riscv/payload.c @@ -19,23 +19,38 @@ #include <arch/boot.h> #include <arch/encoding.h> #include <console/console.h> +#include <vm.h> void run_payload(struct prog *prog, void *fdt, int payload_mode) { void (*doit)(int hart_id, void *fdt) = prog_entry(prog); int hart_id = read_csr(mhartid); uintptr_t status = read_csr(mstatus); - status &= ~MSTATUS_MPIE; - status &= ~MSTATUS_MPP; + status = INSERT_FIELD(status, MSTATUS_MPIE, 0); switch (payload_mode) { case RISCV_PAYLOAD_MODE_U: + status = INSERT_FIELD(status, MSTATUS_MPP, PRV_U); + /* Trap vector base address point to the payload */ + write_csr(utvec, doit); + /* disable U-Mode interrupt */ + write_csr(uie, 0); break; case RISCV_PAYLOAD_MODE_S: - status |= MSTATUS_SPP; + status = INSERT_FIELD(status, MSTATUS_MPP, PRV_S); + /* Trap vector base address point to the payload */ + write_csr(stvec, doit); + /* disable S-Mode interrupt */ + write_csr(sie, 0); + /* disable MMU */ + write_csr(satp, 0); break; case RISCV_PAYLOAD_MODE_M: - doit(hart_id, fdt); - return; + status = INSERT_FIELD(status, MSTATUS_MPP, PRV_M); + /* Trap vector base address point to the payload */ + write_csr(mtvec, doit); + /* disable M-Mode interrupt */ + write_csr(mie, 0); + break; default: die("wrong privilege level for payload"); break; |