diff --git a/include/sbi/sbi_csr_detect.h b/include/sbi/sbi_csr_detect.h index 097c31c8..31e50db9 100644 --- a/include/sbi/sbi_csr_detect.h +++ b/include/sbi/sbi_csr_detect.h @@ -16,9 +16,9 @@ #define csr_read_allowed(csr_num, trap) \ ({ \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ register ulong tinfo asm("a3") = (ulong)trap; \ register ulong ttmp asm("a4"); \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ register ulong ret = 0; \ ((struct sbi_trap_info *)(trap))->cause = 0; \ asm volatile( \ @@ -35,9 +35,9 @@ #define csr_write_allowed(csr_num, trap, value) \ ({ \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ register ulong tinfo asm("a3") = (ulong)trap; \ register ulong ttmp asm("a4"); \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ ((struct sbi_trap_info *)(trap))->cause = 0; \ asm volatile( \ "add %[ttmp], %[tinfo], zero\n" \ diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h index 543393bb..a020cf03 100644 --- a/include/sbi/sbi_hart.h +++ b/include/sbi/sbi_hart.h @@ -10,8 +10,10 @@ #ifndef __SBI_HART_H__ #define __SBI_HART_H__ +#include #include #include +#include /** Possible privileged specification versions of a hart */ enum sbi_hart_priv_versions { @@ -125,6 +127,7 @@ struct sbi_hart_features { unsigned int pmp_log2gran; unsigned int mhpm_mask; unsigned int mhpm_bits; + unsigned long expected_trap; }; extern unsigned long hart_features_offset; @@ -135,7 +138,25 @@ struct sbi_scratch; int sbi_hart_reinit(struct sbi_scratch *scratch); int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot); -extern void (*sbi_hart_expected_trap)(void); +extern void __sbi_expected_trap(void); +extern void __sbi_expected_trap_hext(void); + +/* + * Keep the load free of function calls, including stack-protector code. + * Early CSR probes can use the non-H handler before hart features are ready. + */ +#define sbi_hart_expected_trap_addr() \ +({ \ + unsigned long __offset = __smp_load_acquire(&hart_features_offset); \ + unsigned long __addr = (unsigned long)&__sbi_expected_trap; \ + if (__offset) { \ + struct sbi_hart_features *__features = \ + sbi_scratch_thishart_offset_ptr(__offset); \ + if (__features->expected_trap) \ + __addr = __features->expected_trap; \ + } \ + __addr; \ +}) unsigned int sbi_hart_mhpm_mask(struct sbi_scratch *scratch); void sbi_hart_delegation_dump(struct sbi_scratch *scratch, diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c index 7fc49cf7..925cfe96 100644 --- a/lib/sbi/sbi_hart.c +++ b/lib/sbi/sbi_hart.c @@ -22,11 +22,6 @@ #include #include -extern void __sbi_expected_trap(void); -extern void __sbi_expected_trap_hext(void); - -void (*sbi_hart_expected_trap)(void) = &__sbi_expected_trap; - unsigned long hart_features_offset; static void mstatus_init(struct sbi_scratch *scratch) @@ -481,6 +476,10 @@ static int hart_detect_features(struct sbi_scratch *scratch, bool cold_boot) if (hfeatures->detected) return 0; + /* Set the expected trap handler based on H-extension availability */ + hfeatures->expected_trap = misa_extension('H') ? + (ulong)&__sbi_expected_trap_hext : (ulong)&__sbi_expected_trap; + /* * Parse device tree extensions early, before any trap-based checks. * Needed to detect Smrnmi and install NMI handlers before CSR probes @@ -706,6 +705,7 @@ int sbi_hart_reinit(struct sbi_scratch *scratch) int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot) { + unsigned long offset; int rc; /* @@ -715,13 +715,11 @@ int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot) csr_write(CSR_MIP, 0); if (cold_boot) { - if (misa_extension('H')) - sbi_hart_expected_trap = &__sbi_expected_trap_hext; - - hart_features_offset = sbi_scratch_alloc_offset( - sizeof(struct sbi_hart_features)); - if (!hart_features_offset) + offset = sbi_scratch_alloc_offset(sizeof(struct sbi_hart_features)); + if (!offset) return SBI_ENOMEM; + /* Publish only after every hart's feature storage is zeroed. */ + __smp_store_release(&hart_features_offset, offset); } rc = hart_detect_features(scratch, cold_boot); diff --git a/lib/sbi/sbi_illegal_atomic.c b/lib/sbi/sbi_illegal_atomic.c index 977a9ad0..30f5118e 100644 --- a/lib/sbi/sbi_illegal_atomic.c +++ b/lib/sbi/sbi_illegal_atomic.c @@ -30,7 +30,7 @@ int sbi_illegal_atomic(ulong insn, struct sbi_trap_regs *regs) { \ register ulong tinfo asm("a3"); \ register ulong mstatus = 0; \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ type ret = 0; \ trap->cause = 0; \ asm volatile( \ @@ -57,7 +57,7 @@ int sbi_illegal_atomic(ulong insn, struct sbi_trap_regs *regs) { \ register ulong tinfo asm("a3"); \ register ulong mstatus = 0; \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ type ret = 0; \ trap->cause = 0; \ asm volatile( \ diff --git a/lib/sbi/sbi_unpriv.c b/lib/sbi/sbi_unpriv.c index 60becedc..1550d111 100644 --- a/lib/sbi/sbi_unpriv.c +++ b/lib/sbi/sbi_unpriv.c @@ -33,9 +33,9 @@ union sbi_unpriv_data { type sbi_load_##type(const type *addr, \ struct sbi_trap_info *trap) \ { \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ register ulong tinfo asm("a3") = (ulong)trap; \ register ulong mstatus = 0; \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ type ret = 0; \ trap->cause = 0; \ asm volatile( \ @@ -58,9 +58,9 @@ union sbi_unpriv_data { void sbi_store_##type(type *addr, type val, \ struct sbi_trap_info *trap) \ { \ + register ulong mtvec = sbi_hart_expected_trap_addr(); \ register ulong tinfo asm("a3") = (ulong)trap; \ register ulong mstatus = 0; \ - register ulong mtvec = (ulong)sbi_hart_expected_trap; \ trap->cause = 0; \ asm volatile( \ "csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n" \ @@ -207,7 +207,7 @@ ulong sbi_get_insn(ulong mepc, struct sbi_trap_info *trap) register ulong tinfo asm("a3"); register ulong ttmp asm("a4"); register ulong mstatus = 0; - register ulong mtvec = (ulong)sbi_hart_expected_trap; + register ulong mtvec = sbi_hart_expected_trap_addr(); ulong insn = 0; trap->cause = 0;