include: sbi: Ignore rs1 and rd fields in FENCE.TSO.

While FENCE.TSO is only specified with them set to zero, it is a special
case of FENCE, which needs to ignore these otherwise reserved fields, but
in some implementations, namely XuanTie C906 and C910, apparently does not.
See the RISCVuzz paper by Thomas et al. for details.

Signed-off-by: Benedikt Freisen <b.freisen@gmx.net>
Reviewed-by: Anup Patel <anup@brainfault.org>
Link: https://lore.kernel.org/r/20251114203842.13396-5-b.freisen@gmx.net
Signed-off-by: Anup Patel <anup@brainfault.org>

.editorconfig

@@ -1,4 +1,4 @@
-# SPDX-License-Identifier: GPL-2.0-only
+# SPDX-License-Identifier: BSD-2-Clause
 # See here for more information about the format and editor support:
 # https://editorconfig.org/
 

Makefile

@@ -94,20 +94,26 @@ OPENSBI_VERSION_MINOR=`grep "define OPENSBI_VERSION_MINOR" $(include_dir)/sbi/sb
 OPENSBI_VERSION_GIT=
 
 # Detect 'git' presence before issuing 'git' commands
-GIT_AVAIL=$(shell command -v git 2> /dev/null)
+GIT_AVAIL := $(shell command -v git 2> /dev/null)
 ifneq ($(GIT_AVAIL),)
-GIT_DIR=$(shell git rev-parse --git-dir 2> /dev/null)
+GIT_DIR := $(shell git rev-parse --git-dir 2> /dev/null)
 ifneq ($(GIT_DIR),)
-OPENSBI_VERSION_GIT=$(shell if [ -d $(GIT_DIR) ]; then git describe 2> /dev/null; fi)
+OPENSBI_VERSION_GIT := $(shell if [ -d $(GIT_DIR) ]; then git describe 2> /dev/null; fi)
 endif
 endif
 
 # Setup compilation commands
 ifneq ($(LLVM),)
-CC		=	clang
-AR		=	llvm-ar
-LD		=	ld.lld
-OBJCOPY		=	llvm-objcopy
+ifneq ($(filter %/,$(LLVM)),)
+LLVM_PREFIX := $(LLVM)
+else ifneq ($(filter -%,$(LLVM)),)
+LLVM_SUFFIX := $(LLVM)
+endif
+
+CC		=	$(LLVM_PREFIX)clang$(LLVM_SUFFIX)
+AR		=	$(LLVM_PREFIX)llvm-ar$(LLVM_SUFFIX)
+LD		=	$(LLVM_PREFIX)ld.lld$(LLVM_SUFFIX)
+OBJCOPY		=	$(LLVM_PREFIX)llvm-objcopy$(LLVM_SUFFIX)
 else
 ifdef CROSS_COMPILE
 CC		=	$(CROSS_COMPILE)gcc
@@ -145,6 +151,12 @@ endif
 
 # Guess the compiler's XLEN
 OPENSBI_CC_XLEN := $(shell TMP=`$(CC) $(CLANG_TARGET) -dumpmachine | sed 's/riscv\([0-9][0-9]\).*/\1/'`; echo $${TMP})
+# If guessing XLEN fails, default to 64
+ifneq ($(OPENSBI_CC_XLEN),32)
+  ifneq ($(OPENSBI_CC_XLEN),64)
+    OPENSBI_CC_XLEN = 64
+  endif
+endif
 
 # Guess the compiler's ABI and ISA
 ifneq ($(CC_IS_CLANG),y)
@@ -174,6 +186,11 @@ else
 USE_LD_FLAG	=	-fuse-ld=bfd
 endif
 
+REPRODUCIBLE ?= n
+ifeq ($(REPRODUCIBLE),y)
+REPRODUCIBLE_FLAGS		+=	-ffile-prefix-map=$(src_dir)=
+endif
+
 # Check whether the linker supports creating PIEs
 OPENSBI_LD_PIE := $(shell $(CC) $(CLANG_TARGET) $(RELAX_FLAG) $(USE_LD_FLAG) -fPIE -nostdlib -Wl,-pie -x c /dev/null -o /dev/null >/dev/null 2>&1 && echo y || echo n)
 
@@ -202,16 +219,18 @@ endif
 BUILD_INFO ?= n
 ifeq ($(BUILD_INFO),y)
 OPENSBI_BUILD_DATE_FMT = +%Y-%m-%d %H:%M:%S %z
+ifndef OPENSBI_BUILD_TIME_STAMP
 ifdef SOURCE_DATE_EPOCH
-	OPENSBI_BUILD_TIME_STAMP ?= $(shell date -u -d "@$(SOURCE_DATE_EPOCH)" \
+	OPENSBI_BUILD_TIME_STAMP := $(shell date -u -d "@$(SOURCE_DATE_EPOCH)" \
 		"$(OPENSBI_BUILD_DATE_FMT)" 2>/dev/null || \
 		date -u -r "$(SOURCE_DATE_EPOCH)" \
 		"$(OPENSBI_BUILD_DATE_FMT)" 2>/dev/null || \
 		date -u "$(OPENSBI_BUILD_DATE_FMT)")
 else
-	OPENSBI_BUILD_TIME_STAMP ?= $(shell date "$(OPENSBI_BUILD_DATE_FMT)")
+	OPENSBI_BUILD_TIME_STAMP := $(shell date "$(OPENSBI_BUILD_DATE_FMT)")
 endif
-OPENSBI_BUILD_COMPILER_VERSION=$(shell $(CC) -v 2>&1 | grep ' version ' | \
+endif
+OPENSBI_BUILD_COMPILER_VERSION := $(shell $(CC) -v 2>&1 | grep ' version ' | \
 	sed 's/[[:space:]]*$$//')
 endif
 
@@ -350,6 +369,7 @@ ifeq ($(BUILD_INFO),y)
 GENFLAGS	+=	-DOPENSBI_BUILD_TIME_STAMP="\"$(OPENSBI_BUILD_TIME_STAMP)\""
 GENFLAGS	+=	-DOPENSBI_BUILD_COMPILER_VERSION="\"$(OPENSBI_BUILD_COMPILER_VERSION)\""
 endif
+GENFLAGS	+=	-include $(include_dir)/sbi/sbi_visibility.h
 ifdef PLATFORM
 GENFLAGS	+=	-include $(KCONFIG_AUTOHEADER)
 endif
@@ -359,6 +379,9 @@ GENFLAGS	+=	$(firmware-genflags-y)
 
 CFLAGS		=	-g -Wall -Werror -ffreestanding -nostdlib -fno-stack-protector -fno-strict-aliasing -ffunction-sections -fdata-sections
 CFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls
+CFLAGS		+=	-fno-asynchronous-unwind-tables -fno-unwind-tables
+CFLAGS		+=	-std=gnu11
+CFLAGS		+=	$(REPRODUCIBLE_FLAGS)
 # Optionally supported flags
 ifeq ($(CC_SUPPORT_VECTOR),y)
 CFLAGS		+=	-DOPENSBI_CC_SUPPORT_VECTOR
@@ -384,6 +407,7 @@ CPPFLAGS	+=	$(firmware-cppflags-y)
 ASFLAGS		=	-g -Wall -nostdlib
 ASFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls
 ASFLAGS		+=	-fPIE
+ASFLAGS		+=	$(REPRODUCIBLE_FLAGS)
 # Optionally supported flags
 ifeq ($(CC_SUPPORT_SAVE_RESTORE),y)
 ASFLAGS		+=	-mno-save-restore
@@ -427,11 +451,14 @@ DTSCPPFLAGS	=	$(CPPFLAGS) -nostdinc -nostdlib -fno-builtin -D__DTS__ -x assemble
 
 ifneq ($(DEBUG),)
 CFLAGS		+=	-O0
-ELFFLAGS	+=	-Wl,--print-gc-sections
 else
 CFLAGS		+=	-O2
 endif
 
+ifeq ($(V), 1)
+ELFFLAGS	+=	-Wl,--print-gc-sections
+endif
+
 # Setup functions for compilation
 define dynamic_flags
 -I$(shell dirname $(2)) -D__OBJNAME__=$(subst -,_,$(shell basename $(1) .o))

README.md

@@ -99,7 +99,7 @@ capable enough to bring up all other non-booting harts using HSM extension.
 Required Toolchain and Packages
 -------------------------------
 
-OpenSBI can be compiled natively or cross-compiled on a x86 host. For
+OpenSBI can be compiled natively or cross-compiled on a host machine. For
 cross-compilation, you can build your own toolchain, download a prebuilt one
 from the [Bootlin toolchain repository] or install a distribution-provided
 toolchain; if you opt to use LLVM/Clang, most distribution toolchains will
@@ -108,16 +108,12 @@ LLVM/Clang toolchain due to LLVM's ability to support multiple backends in the
 same binary, so is often an easy way to obtain a working cross-compilation
 toolchain.
 
-Basically, we prefer toolchains with Position Independent Executable (PIE)
-support like *riscv64-linux-gnu-gcc*, *riscv64-unknown-freebsd-gcc*, or
-*Clang/LLVM* as they generate PIE firmware images that can run at arbitrary
-address with appropriate alignment. If a bare-metal GNU toolchain (e.g.
-*riscv64-unknown-elf-gcc*) is used, static linked firmware images are
-generated instead. *Clang/LLVM* can still generate PIE images if a bare-metal
-triple is used (e.g. *-target riscv64-unknown-elf*).
-
-Please note that only a 64-bit version of the toolchain is available in
-the Bootlin toolchain repository for now.
+Toolchains with Position Independent Executable (PIE) support like
+*riscv64-linux-gnu-gcc*, *riscv64-unknown-freebsd-gcc*, or *Clang/LLVM* are
+required in order to generate PIE firmware images that can run at arbitrary
+address with appropriate alignment. Bare-metal GNU toolchains (e.g.
+*riscv64-unknown-elf-gcc*) cannot be used. *Clang/LLVM* can still generate PIE
+images if a bare-metal triple is used (e.g. *-target riscv64-unknown-elf*).
 
 In addition to a toolchain, OpenSBI also requires the following packages on
 the host:
@@ -256,6 +252,18 @@ option with:
 make LLVM=1
 ```
 
+To build with a specific version of LLVM, a path to a directory containing the
+LLVM tools can be provided:
+```
+make LLVM=/path/to/llvm/
+```
+
+If you have versioned llvm tools you would like to use, such as `clang-17`, the LLVM variable can
+be set as:
+```
+make LLVM=-17
+```
+
 When using Clang, *CROSS_COMPILE* often does not need to be defined unless
 using GNU binutils with prefixed binary names. *PLATFORM_RISCV_XLEN* will be
 used to infer a default triple to pass to Clang, so if *PLATFORM_RISCV_XLEN*

docs/platform/fpga-ariane.md

@@ -13,7 +13,7 @@ The FPGA SoC currently contains the following peripherals:
 - Bootrom containing zero stage bootloader and device tree.
 
 To build platform specific library and firmwares, provide the
-*PLATFORM=fpga/ariane* parameter to the top level `make` command.
+*PLATFORM=generic* parameter to the top level `make` command.
 
 Platform Options
 ----------------
@@ -26,7 +26,7 @@ Building Ariane FPGA Platform
 **Linux Kernel Payload**
 
 ```
-make PLATFORM=fpga/ariane FW_PAYLOAD_PATH=<linux_build_directory>/arch/riscv/boot/Image
+make PLATFORM=generic FW_PAYLOAD_PATH=<linux_build_directory>/arch/riscv/boot/Image
 ```
 
 Booting Ariane FPGA Platform

docs/platform/fpga-openpiton.md

@@ -7,8 +7,8 @@ processor from ETH Zurich. To this end, Ariane has been equipped with a
 different L1 cache subsystem that follows a write-through protocol and that has
 support for cache invalidations and atomics.
 
-To build platform specific library and firmwares, provide the
-*PLATFORM=fpga/openpiton* parameter to the top level `make` command.
+To build platform specific library and firmwares, provide the *PLATFORM=generic*
+parameter to the top level `make` command.
 
 Platform Options
 ----------------
@@ -21,7 +21,7 @@ Building Ariane FPGA Platform
 **Linux Kernel Payload**
 
 ```
-make PLATFORM=fpga/openpiton FW_PAYLOAD_PATH=<linux_build_directory>/arch/riscv/boot/Image
+make PLATFORM=generic FW_PAYLOAD_PATH=<linux_build_directory>/arch/riscv/boot/Image
 ```
 
 Booting Ariane FPGA Platform

docs/platform/generic.md

@@ -47,6 +47,8 @@ RISC-V Platforms Using Generic Platform
 * **SiFive HiFive Unleashed** (*[sifive_fu540.md]*)
 * **Spike** (*[spike.md]*)
 * **T-HEAD C9xx series Processors** (*[thead-c9xx.md]*)
+* **OpenPiton FPGA SoC** (*[fpga-openpiton.md]*)
+* **Ariane FPGA SoC** (*[fpga-ariane.md]*)
 
 [andes-ae350.md]: andes-ae350.md
 [qemu_virt.md]: qemu_virt.md
@@ -55,3 +57,5 @@ RISC-V Platforms Using Generic Platform
 [sifive_fu540.md]: sifive_fu540.md
 [spike.md]: spike.md
 [thead-c9xx.md]: thead-c9xx.md
+[fpga-openpiton.md]: fpga-openpiton.md
+[fpga-ariane.md]: fpga-ariane.md

docs/platform/platform.md

@@ -21,20 +21,12 @@ OpenSBI currently supports the following virtual and hardware platforms:
 * **Kendryte K210 SoC**: Platform support for the Kendryte K210 SoC used on
   boards such as the Kendryte KD233 or the Sipeed MAIX Dock.
 
-* **Ariane FPGA SoC**: Platform support for the Ariane FPGA SoC used on
-  Genesys 2 board. More details on this platform can be found in the file
-  *[fpga-ariane.md]*.
-
 * **Andes AE350 SoC**: Platform support for the Andes's SoC (AE350). More
   details on this platform can be found in the file *[andes-ae350.md]*.
 
 * **Spike**: Platform support for the Spike emulator. More
   details on this platform can be found in the file *[spike.md]*.
 
-* **OpenPiton FPGA SoC**: Platform support OpenPiton research platform based
-  on ariane core. More details on this platform can be found in the file
-  *[fpga-openpiton.md]*.
-
 * **Shakti C-class SoC Platform**: Platform support for Shakti C-class
   processor based SOCs. More details on this platform can be found in the
   file *[shakti_cclass.md]*.
@@ -52,10 +44,8 @@ comments to facilitate the implementation.
 [generic.md]: generic.md
 [qemu_virt.md]: qemu_virt.md
 [sifive_fu540.md]: sifive_fu540.md
-[fpga-ariane.md]: fpga-ariane.md
 [andes-ae350.md]: andes-ae350.md
 [thead-c910.md]: thead-c910.md
 [spike.md]: spike.md
-[fpga-openpiton.md]: fpga-openpiton.md
 [shakti_cclass.md]: shakti_cclass.md
 [renesas-rzfive.md]: renesas-rzfive.md

docs/platform_requirements.md

@@ -19,6 +19,10 @@ Base Platform Requirements
 The base RISC-V platform requirements for OpenSBI are as follows:
 
 1. At least rv32ima_zicsr or rv64ima_zicsr required on all HARTs
+
+     * Users may restrict the usage of atomic instructions to lr/sc
+       via rv32im_zalrsc_zicsr or rv64im_zalrsc_zicsr if preferred
+
 2. At least one HART should have S-mode support because:
 
      * SBI calls are meant for RISC-V S-mode (Supervisor mode)

docs/pmu_support.md

@@ -74,10 +74,10 @@ pmu {
 						  <0x10000 0x10033 0x000ff000>;
 					/* For event ID 0x0002 */
 	riscv,raw-event-to-mhpmcounters = <0x0000 0x0002 0xffffffff 0xffffffff 0x00000f8>,
-					/* For event ID 0-4 */
+					/* For event ID 0-15 */
 					<0x0 0x0 0xffffffff 0xfffffff0 0x00000ff0>,
 					/* For event ID 0xffffffff0000000f - 0xffffffff000000ff */
-					<0xffffffff 0x0 0xffffffff 0xffffff0f 0x00000ff0>;
+					<0xffffffff 0xf 0xffffffff 0xffffff0f 0x00000ff0>;
 };
 ```
 

firmware/Kconfig

@@ -1 +1,28 @@
 # SPDX-License-Identifier: BSD-2-Clause
+
+menu "Stack Protector Support"
+
+config STACK_PROTECTOR
+	bool "Stack Protector buffer overflow detection"
+	default n
+	help
+	  This option turns on the "stack-protector" compiler feature.
+
+config STACK_PROTECTOR_STRONG
+	bool "Strong Stack Protector"
+	depends on STACK_PROTECTOR
+	default n
+	help
+	  Turn on the "stack-protector" with "-fstack-protector-strong" option.
+	  Like -fstack-protector but includes additional functions to be
+	  protected.
+
+config STACK_PROTECTOR_ALL
+	bool "Almighty Stack Protector"
+	depends on STACK_PROTECTOR
+	default n
+	help
+	  Turn on the "stack-protector" with "-fstack-protector-all" option.
+	  Like -fstack-protector except that all functions are protected.
+
+endmenu

firmware/fw_base.S

@@ -59,28 +59,38 @@ _try_lottery:
 	/* Jump to relocation wait loop if we don't get relocation lottery */
 	lla	a6, _boot_lottery
 	li	a7, BOOT_LOTTERY_ACQUIRED
+#ifdef __riscv_atomic
 	amoswap.w a6, a7, (a6)
 	bnez	a6, _wait_for_boot_hart
+#elif __riscv_zalrsc
+_sc_fail:
+	lr.w	t0, (a6)
+	sc.w	t1, a7, (a6)
+	bnez	t1, _sc_fail
+	bnez	t0, _wait_for_boot_hart
+#else
+#error "need a or zalrsc"
+#endif
 
 	/* relocate the global table content */
 	li	t0, FW_TEXT_START	/* link start */
 	lla	t1, _fw_start		/* load start */
 	sub	t2, t1, t0		/* load offset */
-	lla	t0, __rel_dyn_start
-	lla	t1, __rel_dyn_end
+	lla	t0, __rela_dyn_start
+	lla	t1, __rela_dyn_end
 	beq	t0, t1, _relocate_done
 2:
-	REG_L	t5, REGBYTES(t0)	/* t5 <-- relocation info:type */
+	REG_L	t5, __SIZEOF_LONG__(t0)	/* t5 <-- relocation info:type */
 	li	t3, R_RISCV_RELATIVE	/* reloc type R_RISCV_RELATIVE */
 	bne	t5, t3, 3f
 	REG_L	t3, 0(t0)
-	REG_L	t5, (REGBYTES * 2)(t0)	/* t5 <-- addend */
+	REG_L	t5, (__SIZEOF_LONG__ * 2)(t0)	/* t5 <-- addend */
 	add	t5, t5, t2
 	add	t3, t3, t2
 	REG_S	t5, 0(t3)		/* store runtime address to the GOT entry */
 
 3:
-	addi	t0, t0, (REGBYTES * 3)
+	addi	t0, t0, (__SIZEOF_LONG__ * 3)
 	blt	t0, t1, 2b
 _relocate_done:
 	/* At this point we are running from link address */
@@ -292,7 +302,7 @@ _fdt_reloc_done:
 	REG_S	t0, 0(t1)
 	j	_start_warm
 
-	/* waiting for boot hart to be done (_boot_status == 2) */
+	/* waiting for boot hart to be done (_boot_status == BOOT_STATUS_BOOT_HART_DONE) */
 _wait_for_boot_hart:
 	li	t0, BOOT_STATUS_BOOT_HART_DONE
 	lla	t1, _boot_status
@@ -726,6 +736,27 @@ _reset_regs:
 
 	ret
 
+	.section .rodata
+.Lstack_corrupt_msg:
+	.string "stack smashing detected\n"
+
+	/* This will be called when the stack corruption is detected */
+	.section .text
+	.align 3
+	.globl __stack_chk_fail
+	.type __stack_chk_fail, %function
+__stack_chk_fail:
+	la	a0, .Lstack_corrupt_msg
+	call	sbi_panic
+
+	/* Initial value of the stack guard variable */
+	.section .data
+	.align 3
+	.globl __stack_chk_guard
+	.type __stack_chk_guard, %object
+__stack_chk_guard:
+	RISCV_PTR	0x95B5FF5A
+
 #ifdef FW_FDT_PATH
 	.section .rodata
 	.align 4

firmware/fw_base.ldS

@@ -47,9 +47,9 @@
 	. = ALIGN(0x1000); /* Ensure next section is page aligned */
 
 	.rela.dyn : {
-		PROVIDE(__rel_dyn_start = .);
+		PROVIDE(__rela_dyn_start = .);
 		*(.rela*)
-		PROVIDE(__rel_dyn_end = .);
+		PROVIDE(__rela_dyn_end = .);
 	}
 
 	PROVIDE(_rodata_end = .);

firmware/objects.mk

@@ -66,3 +66,12 @@ endif
 ifdef FW_OPTIONS
 firmware-genflags-y += -DFW_OPTIONS=$(FW_OPTIONS)
 endif
+
+ifeq ($(CONFIG_STACK_PROTECTOR),y)
+stack-protector-cflags-$(CONFIG_STACK_PROTECTOR) := -fstack-protector
+stack-protector-cflags-$(CONFIG_STACK_PROTECTOR_STRONG) := -fstack-protector-strong
+stack-protector-cflags-$(CONFIG_STACK_PROTECTOR_ALL) := -fstack-protector-all
+else
+stack-protector-cflags-y := -fno-stack-protector
+endif
+firmware-cflags-y += $(stack-protector-cflags-y)

firmware/payloads/test_head.S

@@ -30,7 +30,18 @@ _start:
 	/* Pick one hart to run the main boot sequence */
 	lla	a3, _hart_lottery
 	li	a2, 1
+#ifdef __riscv_atomic
 	amoadd.w a3, a2, (a3)
+#elif __riscv_zalrsc
+_sc_fail:
+	lr.w	t0, (a3)
+	addw	t1, t0, a2
+	sc.w	t1, t1, (a3)
+	bnez	t1, _sc_fail
+	move	a3, t0
+#else
+#error "need a or zalrsc"
+#endif
 	bnez	a3, _start_hang
 
 	/* Save a0 and a1 */
@@ -86,3 +97,18 @@ _boot_a0:
 	RISCV_PTR	0
 _boot_a1:
 	RISCV_PTR	0
+
+	/* This will be called when the stack corruption is detected */
+	.section .text
+	.align 3
+	.globl __stack_chk_fail
+	.type __stack_chk_fail, %function
+	.equ  __stack_chk_fail, _start_hang
+
+	/* Initial value of the stack guard variable */
+	.section .data
+	.align 3
+	.globl __stack_chk_guard
+	.type __stack_chk_guard, %object
+__stack_chk_guard:
+	RISCV_PTR	0x95B5FF5A

firmware/payloads/test_main.c

@@ -46,6 +46,13 @@ static inline void sbi_ecall_console_puts(const char *str)
 		  sbi_strlen(str), (unsigned long)str, 0, 0, 0, 0);
 }
 
+static inline void sbi_ecall_shutdown(void)
+{
+	sbi_ecall(SBI_EXT_SRST, SBI_EXT_SRST_RESET,
+		  SBI_SRST_RESET_TYPE_SHUTDOWN, SBI_SRST_RESET_REASON_NONE,
+		  0, 0, 0, 0);
+}
+
 #define wfi()                                             \
 	do {                                              \
 		__asm__ __volatile__("wfi" ::: "memory"); \
@@ -54,7 +61,6 @@ static inline void sbi_ecall_console_puts(const char *str)
 void test_main(unsigned long a0, unsigned long a1)
 {
 	sbi_ecall_console_puts("\nTest payload running\n");
-
-	while (1)
-		wfi();
+	sbi_ecall_shutdown();
+	sbi_ecall_console_puts("sbi_ecall_shutdown failed to execute.\n");
 }

include/sbi/fw_dynamic.h

@@ -79,36 +79,12 @@ struct fw_dynamic_info {
  * Prevent modification of struct fw_dynamic_info from affecting
  * FW_DYNAMIC_INFO_xxx_OFFSET
  */
-_Static_assert(
-	offsetof(struct fw_dynamic_info, magic)
-		== FW_DYNAMIC_INFO_MAGIC_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_MAGIC_OFFSET");
-_Static_assert(
-	offsetof(struct fw_dynamic_info, version)
-		== FW_DYNAMIC_INFO_VERSION_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_VERSION_OFFSET");
-_Static_assert(
-	offsetof(struct fw_dynamic_info, next_addr)
-		== FW_DYNAMIC_INFO_NEXT_ADDR_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_NEXT_ADDR_OFFSET");
-_Static_assert(
-	offsetof(struct fw_dynamic_info, next_mode)
-		== FW_DYNAMIC_INFO_NEXT_MODE_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_NEXT_MODE_OFFSET");
-_Static_assert(
-	offsetof(struct fw_dynamic_info, options)
-		== FW_DYNAMIC_INFO_OPTIONS_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_OPTIONS_OFFSET");
-_Static_assert(
-	offsetof(struct fw_dynamic_info, boot_hart)
-		== FW_DYNAMIC_INFO_BOOT_HART_OFFSET,
-	"struct fw_dynamic_info definition has changed, please redefine "
-	"FW_DYNAMIC_INFO_BOOT_HART_OFFSET");
+assert_member_offset(struct fw_dynamic_info, magic, FW_DYNAMIC_INFO_MAGIC_OFFSET);
+assert_member_offset(struct fw_dynamic_info, version, FW_DYNAMIC_INFO_VERSION_OFFSET);
+assert_member_offset(struct fw_dynamic_info, next_addr, FW_DYNAMIC_INFO_NEXT_ADDR_OFFSET);
+assert_member_offset(struct fw_dynamic_info, next_mode, FW_DYNAMIC_INFO_NEXT_MODE_OFFSET);
+assert_member_offset(struct fw_dynamic_info, options, FW_DYNAMIC_INFO_OPTIONS_OFFSET);
+assert_member_offset(struct fw_dynamic_info, boot_hart, FW_DYNAMIC_INFO_BOOT_HART_OFFSET);
 
 #endif
 

include/sbi/riscv_asm.h

@@ -156,6 +156,26 @@
 				     : "memory");                  \
 	})
 
+#if __riscv_xlen == 64
+#define __csrrw64(op, csr, csrh, val) (true ? op(csr, val) : (uint64_t)csrh)
+#define __csrr64( op, csr, csrh)      (true ? op(csr)      : (uint64_t)csrh)
+#define __csrw64( op, csr, csrh, val) (true ? op(csr, val) : (uint64_t)csrh)
+#elif __riscv_xlen == 32
+#define __csrrw64(op, csr, csrh, val) (  op(csr, val) | (uint64_t)op(csrh, val >> 32) << 32)
+#define __csrr64( op, csr, csrh)      (  op(csr)      | (uint64_t)op(csrh)            << 32)
+#define __csrw64( op, csr, csrh, val) ({ op(csr, val);            op(csrh, val >> 32);    })
+#endif
+
+#define csr_swap64(        csr, val) __csrrw64(csr_swap,         csr, csr ## H, val)
+#define csr_read64(        csr)      __csrr64 (csr_read,         csr, csr ## H)
+#define csr_read_relaxed64(csr)      __csrr64 (csr_read_relaxed, csr, csr ## H)
+#define csr_write64(       csr, val) __csrw64 (csr_write,        csr, csr ## H, val)
+#define csr_read_set64(    csr, val) __csrrw64(csr_read_set,     csr, csr ## H, val)
+#define csr_set64(         csr, val) __csrw64 (csr_set,          csr, csr ## H, val)
+#define csr_clear64(       csr, val) __csrw64 (csr_clear,        csr, csr ## H, val)
+#define csr_read_clear64(  csr, val) __csrrw64(csr_read_clear,   csr, csr ## H, val)
+#define csr_clear64(       csr, val) __csrw64 (csr_clear,        csr, csr ## H, val)
+
 unsigned long csr_read_num(int csr_num);
 
 void csr_write_num(int csr_num, unsigned long val);

include/sbi/riscv_dbtr.h

@@ -122,6 +122,50 @@ enum {
 	RV_DBTR_DECLARE_BIT_MASK(MC, TYPE,    4),
 };
 
+
+/* ICOUNT - Match Control Type Register */
+enum {
+	RV_DBTR_DECLARE_BIT(ICOUNT, ACTION,   0),
+	RV_DBTR_DECLARE_BIT(ICOUNT, U,        6),
+	RV_DBTR_DECLARE_BIT(ICOUNT, S,        7),
+	RV_DBTR_DECLARE_BIT(ICOUNT, PENDING,  8),
+	RV_DBTR_DECLARE_BIT(ICOUNT, M,        9),
+	RV_DBTR_DECLARE_BIT(ICOUNT, COUNT,   10),
+	RV_DBTR_DECLARE_BIT(ICOUNT, HIT,     24),
+	RV_DBTR_DECLARE_BIT(ICOUNT, VU,      25),
+	RV_DBTR_DECLARE_BIT(ICOUNT, VS,      26),
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT(ICOUNT, DMODE,   59),
+	RV_DBTR_DECLARE_BIT(ICOUNT, TYPE,    60),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT(ICOUNT, DMODE,   27),
+	RV_DBTR_DECLARE_BIT(ICOUNT, TYPE,    28),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
+enum {
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, ACTION,   6),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, U,        1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, S,        1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, PENDING,  1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, M,        1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, COUNT,   14),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, HIT,      1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, VU,       1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, VS,       1),
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, DMODE,    1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, TYPE,     4),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, DMODE,    1),
+	RV_DBTR_DECLARE_BIT_MASK(ICOUNT, TYPE,     4),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
 /* MC6 - Match Control 6 Type Register */
 enum {
 	RV_DBTR_DECLARE_BIT(MC6, LOAD,   0),

include/sbi/riscv_encoding.h

@@ -32,7 +32,8 @@
 #define MSTATUS_TVM			_UL(0x00100000)
 #define MSTATUS_TW			_UL(0x00200000)
 #define MSTATUS_TSR			_UL(0x00400000)
-#define MSTATUS_SPELP		_UL(0x00800000)
+#define MSTATUS_SPELP			_UL(0x00800000)
+#define MSTATUS_SDT			_UL(0x01000000)
 #define MSTATUS32_SD			_UL(0x80000000)
 #if __riscv_xlen == 64
 #define MSTATUS_UXL			_ULL(0x0000000300000000)
@@ -85,6 +86,8 @@
 #define HSTATUS_GVA			_UL(0x00000040)
 #define HSTATUS_VSBE			_UL(0x00000020)
 
+#define MTVEC_MODE			_UL(0x00000003)
+
 #define MCAUSE_IRQ_MASK			(_UL(1) << (__riscv_xlen - 1))
 
 #define IRQ_S_SOFT			1
@@ -186,7 +189,7 @@
 
 #define TOPI_IID_SHIFT			16
 #define TOPI_IID_MASK			0xfff
-#define TOPI_IPRIO_MASK		0xff
+#define TOPI_IPRIO_MASK			0xff
 
 #if __riscv_xlen == 64
 #define MHPMEVENT_OF			(_UL(1) << 63)
@@ -375,6 +378,20 @@
 #define CSR_SSTATEEN2			0x10E
 #define CSR_SSTATEEN3			0x10F
 
+/* Supervisor Resource Management Configuration CSRs */
+#define CSR_SRMCFG			0x181
+
+/* Machine-Level Control transfer records CSRs */
+#define CSR_MCTRCTL                     0x34e
+
+/* Supervisor-Level Control transfer records CSRs */
+#define CSR_SCTRCTL                     0x14e
+#define CSR_SCTRSTATUS                  0x14f
+#define CSR_SCTRDEPTH                   0x15f
+
+/* VS-Level Control transfer records CSRs */
+#define CSR_VSCTRCTL                    0x24e
+
 /* ===== Hypervisor-level CSRs ===== */
 
 /* Hypervisor Trap Setup (H-extension) */
@@ -769,6 +786,40 @@
 #define CSR_VTYPE			0xc21
 #define CSR_VLENB			0xc22
 
+/* Custom CSR ranges */
+#define CSR_CUSTOM0_U_RW_BASE		0x800
+#define CSR_CUSTOM0_U_RW_COUNT		0x100
+
+#define CSR_CUSTOM1_U_RO_BASE		0xCC0
+#define CSR_CUSTOM1_U_RO_COUNT		0x040
+
+#define CSR_CUSTOM2_S_RW_BASE		0x5C0
+#define CSR_CUSTOM2_S_RW_COUNT		0x040
+
+#define CSR_CUSTOM3_S_RW_BASE		0x9C0
+#define CSR_CUSTOM3_S_RW_COUNT		0x040
+
+#define CSR_CUSTOM4_S_RO_BASE		0xDC0
+#define CSR_CUSTOM4_S_RO_COUNT		0x040
+
+#define CSR_CUSTOM5_HS_RW_BASE		0x6C0
+#define CSR_CUSTOM5_HS_RW_COUNT		0x040
+
+#define CSR_CUSTOM6_HS_RW_BASE		0xAC0
+#define CSR_CUSTOM6_HS_RW_COUNT		0x040
+
+#define CSR_CUSTOM7_HS_RO_BASE		0xEC0
+#define CSR_CUSTOM7_HS_RO_COUNT		0x040
+
+#define CSR_CUSTOM8_M_RW_BASE		0x7C0
+#define CSR_CUSTOM8_M_RW_COUNT		0x040
+
+#define CSR_CUSTOM9_M_RW_BASE		0xBC0
+#define CSR_CUSTOM9_M_RW_COUNT		0x040
+
+#define CSR_CUSTOM10_M_RO_BASE		0xFC0
+#define CSR_CUSTOM10_M_RO_COUNT		0x040
+
 /* ===== Trap/Exception Causes ===== */
 
 #define CAUSE_MISALIGNED_FETCH		0x0
@@ -799,6 +850,10 @@
 #define SMSTATEEN0_CS			(_ULL(1) << SMSTATEEN0_CS_SHIFT)
 #define SMSTATEEN0_FCSR_SHIFT		1
 #define SMSTATEEN0_FCSR			(_ULL(1) << SMSTATEEN0_FCSR_SHIFT)
+#define SMSTATEEN0_CTR_SHIFT		54
+#define SMSTATEEN0_CTR			(_ULL(1) << SMSTATEEN0_CTR_SHIFT)
+#define SMSTATEEN0_SRMCFG_SHIFT		55
+#define SMSTATEEN0_SRMCFG		(_ULL(1) << SMSTATEEN0_SRMCFG_SHIFT)
 #define SMSTATEEN0_CONTEXT_SHIFT	57
 #define SMSTATEEN0_CONTEXT		(_ULL(1) << SMSTATEEN0_CONTEXT_SHIFT)
 #define SMSTATEEN0_IMSIC_SHIFT		58
@@ -885,16 +940,16 @@
 #define INSN_MASK_C_FSWSP		0xe003
 
 #define INSN_MATCH_C_LHU		0x8400
-#define INSN_MASK_C_LHU		0xfc43
-#define INSN_MATCH_C_LH		0x8440
+#define INSN_MASK_C_LHU			0xfc43
+#define INSN_MATCH_C_LH			0x8440
 #define INSN_MASK_C_LH			0xfc43
-#define INSN_MATCH_C_SH		0x8c00
+#define INSN_MATCH_C_SH			0x8c00
 #define INSN_MASK_C_SH			0xfc43
 
 #define INSN_MASK_WFI			0xffffff00
 #define INSN_MATCH_WFI			0x10500000
 
-#define INSN_MASK_FENCE_TSO		0xffffffff
+#define INSN_MASK_FENCE_TSO		0xfff0707f
 #define INSN_MATCH_FENCE_TSO		0x8330000f
 
 #define INSN_MASK_VECTOR_UNIT_STRIDE		0xfdf0707f
@@ -970,13 +1025,14 @@
 #define INSN_MATCH_VS4RV		0x62800027
 #define INSN_MATCH_VS8RV		0xe2800027
 
-#define INSN_MASK_VECTOR_LOAD_STORE	0x7f
-#define INSN_MATCH_VECTOR_LOAD		0x07
-#define INSN_MATCH_VECTOR_STORE		0x27
+#define INSN_OPCODE_MASK		0x7f
+#define INSN_OPCODE_VECTOR_LOAD		0x07
+#define INSN_OPCODE_VECTOR_STORE	0x27
+#define INSN_OPCODE_AMO			0x2f
 
 #define IS_VECTOR_LOAD_STORE(insn) \
-	((((insn) & INSN_MASK_VECTOR_LOAD_STORE) == INSN_MATCH_VECTOR_LOAD) || \
-	(((insn) & INSN_MASK_VECTOR_LOAD_STORE) == INSN_MATCH_VECTOR_STORE))
+	((((insn) & INSN_OPCODE_MASK) == INSN_OPCODE_VECTOR_LOAD) || \
+	(((insn) & INSN_OPCODE_MASK) == INSN_OPCODE_VECTOR_STORE))
 
 #define IS_VECTOR_INSN_MATCH(insn, match, mask) \
 	(((insn) & (mask)) == ((match) & (mask)))
@@ -1256,7 +1312,7 @@
 /* 64-bit read for VS-stage address translation (RV64) */
 #define INSN_PSEUDO_VS_LOAD		0x00003000
 /* 64-bit write for VS-stage address translation (RV64) */
-#define INSN_PSEUDO_VS_STORE	0x00003020
+#define INSN_PSEUDO_VS_STORE		0x00003020
 
 #elif __riscv_xlen == 32
 
@@ -1264,18 +1320,31 @@
 #define INSN_PSEUDO_VS_LOAD		0x00002000
 
 /* 32-bit write for VS-stage address translation (RV32) */
-#define INSN_PSEUDO_VS_STORE	0x00002020
+#define INSN_PSEUDO_VS_STORE		0x00002020
 
 #else
 #error "Unexpected __riscv_xlen"
 #endif
 
+#define MASK_FUNCT3			0x7000
+#define SHIFT_FUNCT3			12
+
+#define MASK_RS1			0xf8000
+#define MASK_RS2			0x1f00000
+#define MASK_RD				0xf80
+
+#define MASK_CSR			0xfff00000
+#define SHIFT_CSR			20
+
+#define MASK_AQRL			0x06000000
+#define SHIFT_AQRL			25
+
 #define VM_MASK				0x1
-#define VIEW_MASK				0x3
-#define VSEW_MASK				0x3
-#define VLMUL_MASK				0x7
+#define VIEW_MASK			0x3
+#define VSEW_MASK			0x3
+#define VLMUL_MASK			0x7
 #define VD_MASK				0x1f
-#define VS2_MASK				0x1f
+#define VS2_MASK			0x1f
 #define INSN_16BIT_MASK			0x3
 #define INSN_32BIT_MASK			0x1c
 
@@ -1287,15 +1356,8 @@
 
 #define INSN_LEN(insn)			(INSN_IS_16BIT(insn) ? 2 : 4)
 
-#if __riscv_xlen == 64
-#define LOG_REGBYTES			3
-#else
-#define LOG_REGBYTES			2
-#endif
-#define REGBYTES			(1 << LOG_REGBYTES)
-
-#define SH_VSEW			3
-#define SH_VIEW			12
+#define SH_VSEW				3
+#define SH_VIEW				12
 #define SH_VD				7
 #define SH_VS2				20
 #define SH_VM				25
@@ -1328,49 +1390,32 @@
 #define SHIFT_RIGHT(x, y)		\
 	((y) < 0 ? ((x) << -(y)) : ((x) >> (y)))
 
-#define REG_MASK			\
-	((1 << (5 + LOG_REGBYTES)) - (1 << LOG_REGBYTES))
-
-#define REG_OFFSET(insn, pos)		\
-	(SHIFT_RIGHT((insn), (pos) - LOG_REGBYTES) & REG_MASK)
-
-#define REG_PTR(insn, pos, regs)	\
-	(ulong *)((ulong)(regs) + REG_OFFSET(insn, pos))
-
-#define GET_RM(insn)			((insn & MASK_FUNCT3) >> SHIFT_FUNCT3)
-
-#define GET_RS1_NUM(insn)		((insn & MASK_RS1) >> 15)
+#define GET_FUNC3(insn)			((insn & MASK_FUNCT3) >> SHIFT_FUNCT3)
+#define GET_RM(insn)			GET_FUNC3(insn)
+#define GET_RS1_NUM(insn)		((insn & MASK_RS1) >> SH_RS1)
+#define GET_RS2_NUM(insn)		((insn & MASK_RS2) >> SH_RS2)
+#define GET_RS1S_NUM(insn)		RVC_RS1S(insn)
+#define GET_RS2S_NUM(insn)		RVC_RS2S(insn)
+#define GET_RS2C_NUM(insn)		RVC_RS2(insn)
+#define GET_RD_NUM(insn)		((insn & MASK_RD) >> SH_RD)
 #define GET_CSR_NUM(insn)		((insn & MASK_CSR) >> SHIFT_CSR)
+#define GET_AQRL(insn)			((insn & MASK_AQRL) >> SHIFT_AQRL)
 
-#define GET_RS1(insn, regs)		(*REG_PTR(insn, SH_RS1, regs))
-#define GET_RS2(insn, regs)		(*REG_PTR(insn, SH_RS2, regs))
-#define GET_RS1S(insn, regs)		(*REG_PTR(RVC_RS1S(insn), 0, regs))
-#define GET_RS2S(insn, regs)		(*REG_PTR(RVC_RS2S(insn), 0, regs))
-#define GET_RS2C(insn, regs)		(*REG_PTR(insn, SH_RS2C, regs))
-#define GET_SP(regs)			(*REG_PTR(2, 0, regs))
-#define SET_RD(insn, regs, val)		(*REG_PTR(insn, SH_RD, regs) = (val))
 #define IMM_I(insn)			((s32)(insn) >> 20)
 #define IMM_S(insn)			(((s32)(insn) >> 25 << 5) | \
 					 (s32)(((insn) >> 7) & 0x1f))
 
-#define IS_MASKED(insn)		(((insn >> SH_VM) & VM_MASK) == 0)
+#define IS_MASKED(insn)			(((insn >> SH_VM) & VM_MASK) == 0)
 #define GET_VD(insn)			((insn >> SH_VD) & VD_MASK)
 #define GET_VS2(insn)			((insn >> SH_VS2) & VS2_MASK)
 #define GET_VIEW(insn)			(((insn) >> SH_VIEW) & VIEW_MASK)
 #define GET_MEW(insn)			(((insn) >> SH_MEW) & 1)
-#define GET_VSEW(vtype)		(((vtype) >> SH_VSEW) & VSEW_MASK)
+#define GET_VSEW(vtype)			(((vtype) >> SH_VSEW) & VSEW_MASK)
 #define GET_VLMUL(vtype)		((vtype) & VLMUL_MASK)
 #define GET_LEN(view)			(1UL << (view))
 #define GET_NF(insn)			(1 + ((insn >> 29) & 7))
 #define GET_VEMUL(vlmul, view, vsew)	((vlmul + view - vsew) & 7)
-#define GET_EMUL(vemul)		(1UL << ((vemul) >= 4 ? 0 : (vemul)))
-
-#define MASK_FUNCT3			0x7000
-#define MASK_RS1			0xf8000
-#define MASK_CSR			0xfff00000
-
-#define SHIFT_FUNCT3			12
-#define SHIFT_CSR			20
+#define GET_EMUL(vemul)			(1UL << ((vemul) >= 4 ? 0 : (vemul)))
 
 #define CSRRW 1
 #define CSRRS 2

include/sbi/sbi_bitmap.h

@@ -130,4 +130,17 @@ static inline void bitmap_xor(unsigned long *dst, const unsigned long *src1,
 		__bitmap_xor(dst, src1, src2, nbits);
 }
 
+static inline int bitmap_weight(const unsigned long *src, int nbits)
+{
+	int i, res = 0;
+
+	for (i = 0; i < nbits / BITS_PER_LONG; i++)
+		res += sbi_popcount(src[i]);
+
+	if (nbits % BITS_PER_LONG)
+		res += sbi_popcount(src[i] & BITMAP_LAST_WORD_MASK(nbits));
+
+	return res;
+}
+
 #endif

include/sbi/sbi_bitops.h

@@ -125,14 +125,22 @@ static inline unsigned long sbi_fls(unsigned long word)
  */
 static inline unsigned long sbi_popcount(unsigned long word)
 {
-	unsigned long count = 0;
+	unsigned long count;
 
-	while (word) {
-		word &= word - 1;
-		count++;
-	}
-
-	return count;
+#if BITS_PER_LONG == 64
+	count = word - ((word >> 1) & 0x5555555555555555ul);
+	count = (count & 0x3333333333333333ul) + ((count >> 2) & 0x3333333333333333ul);
+	count = (count + (count >> 4)) & 0x0F0F0F0F0F0F0F0Ful;
+	count = count + (count >> 8);
+	count = count + (count >> 16);
+	return (count + (count >> 32)) & 0x00000000000000FFul;
+#else
+	count = word - ((word >> 1) & 0x55555555);
+	count = (count & 0x33333333) + ((count >> 2) & 0x33333333);
+	count = (count + (count >> 4)) & 0x0F0F0F0F;
+	count = count + (count >> 8);
+	return (count + (count >> 16)) & 0x000000FF;
+#endif
 }
 
 #define for_each_set_bit(bit, addr, size) \

include/sbi/sbi_byteorder.h

@@ -14,13 +14,13 @@
 # define _conv_cast(type, val) ((type)(val))
 #endif
 
-#define BSWAP16(x)	((((x) & 0x00ff) << 8) | \
+#define __BSWAP16(x)	((((x) & 0x00ff) << 8) | \
 			 (((x) & 0xff00) >> 8))
-#define BSWAP32(x)	((((x) & 0x000000ff) << 24) | \
+#define __BSWAP32(x)	((((x) & 0x000000ff) << 24) | \
 			 (((x) & 0x0000ff00) << 8) | \
 			 (((x) & 0x00ff0000) >> 8) | \
 			 (((x) & 0xff000000) >> 24))
-#define BSWAP64(x)	((((x) & 0x00000000000000ffULL) << 56) | \
+#define __BSWAP64(x)	((((x) & 0x00000000000000ffULL) << 56) | \
 			 (((x) & 0x000000000000ff00ULL) << 40) | \
 			 (((x) & 0x0000000000ff0000ULL) << 24) | \
 			 (((x) & 0x00000000ff000000ULL) << 8) | \
@@ -29,6 +29,10 @@
 			 (((x) & 0x00ff000000000000ULL) >> 40) | \
 			 (((x) & 0xff00000000000000ULL) >> 56))
 
+#define BSWAP64(x) ({ uint64_t _sv = (x); __BSWAP64(_sv); })
+#define BSWAP32(x) ({ uint32_t _sv = (x); __BSWAP32(_sv); })
+#define BSWAP16(x) ({ uint16_t _sv = (x); __BSWAP16(_sv); })
+
 #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__  /* CPU(little-endian) */
 #define cpu_to_be16(x)		_conv_cast(uint16_t, BSWAP16(x))
 #define cpu_to_be32(x)		_conv_cast(uint32_t, BSWAP32(x))

include/sbi/sbi_csr_detect.h

@@ -18,7 +18,7 @@
 	({								\
 	register ulong tinfo asm("a3") = (ulong)trap;			\
 	register ulong ttmp asm("a4");					\
-	register ulong mtvec = sbi_hart_expected_trap_addr();		\
+	register ulong mtvec = (ulong)sbi_hart_expected_trap;		\
 	register ulong ret = 0;						\
 	((struct sbi_trap_info *)(trap))->cause = 0;			\
 	asm volatile(							\
@@ -37,7 +37,7 @@
 	({								\
 	register ulong tinfo asm("a3") = (ulong)trap;			\
 	register ulong ttmp asm("a4");					\
-	register ulong mtvec = sbi_hart_expected_trap_addr();		\
+	register ulong mtvec = (ulong)sbi_hart_expected_trap;		\
 	((struct sbi_trap_info *)(trap))->cause = 0;			\
 	asm volatile(							\
 		"add %[ttmp], %[tinfo], zero\n"				\

include/sbi/sbi_dbtr.h

@@ -90,7 +90,7 @@ struct sbi_dbtr_hart_triggers_state {
 	}while (0);
 
 /** SBI shared mem messages layout */
-struct sbi_dbtr_shmem_entry {
+union sbi_dbtr_shmem_entry {
 	struct sbi_dbtr_data_msg data;
 	struct sbi_dbtr_id_msg id;
 };
@@ -115,8 +115,7 @@ int sbi_dbtr_uninstall_trig(unsigned long trig_idx_base,
 int sbi_dbtr_enable_trig(unsigned long trig_idx_base,
 			 unsigned long trig_idx_mask);
 int sbi_dbtr_update_trig(unsigned long smode,
-			 unsigned long trig_idx_base,
-			 unsigned long trig_idx_mask);
+			 unsigned long trig_count);
 int sbi_dbtr_disable_trig(unsigned long trig_idx_base,
 			  unsigned long trig_idx_mask);
 

include/sbi/sbi_domain.h

@@ -121,6 +121,9 @@ struct sbi_domain_memregion {
 		((__flags & SBI_DOMAIN_MEMREGION_SU_ACCESS_MASK)  &&	\
 		 !(__flags & SBI_DOMAIN_MEMREGION_M_ACCESS_MASK))
 
+#define SBI_DOMAIN_MEMREGION_IS_FIRMWARE(__flags)			\
+		((__flags & SBI_DOMAIN_MEMREGION_FW) ? true : false)	\
+
 /** Bit to control if permissions are enforced on all modes */
 #define SBI_DOMAIN_MEMREGION_ENF_PERMISSIONS	(1UL << 6)
 
@@ -157,6 +160,7 @@ struct sbi_domain_memregion {
 				 SBI_DOMAIN_MEMREGION_M_EXECUTABLE)
 
 #define SBI_DOMAIN_MEMREGION_MMIO		(1UL << 31)
+#define SBI_DOMAIN_MEMREGION_FW			(1UL << 30)
 	unsigned long flags;
 };
 
@@ -249,6 +253,13 @@ void sbi_domain_memregion_init(unsigned long addr,
 				unsigned long flags,
 				struct sbi_domain_memregion *reg);
 
+/**
+ * Return the Smepmp pmpcfg LRWX encoding for the flags in @reg.
+ *
+ * @param reg pointer to memory region; its flags field encodes permissions.
+ */
+unsigned int sbi_domain_get_smepmp_flags(struct sbi_domain_memregion *reg);
+
 /**
  * Check whether we can access specified address for given mode and
  * memory region flags under a domain
@@ -307,8 +318,11 @@ int sbi_domain_register(struct sbi_domain *dom,
 int sbi_domain_root_add_memrange(unsigned long addr, unsigned long size,
 			   unsigned long align, unsigned long region_flags);
 
-/** Finalize domain tables and startup non-root domains */
-int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid);
+/** Startup non-root domains */
+int sbi_domain_startup(struct sbi_scratch *scratch, u32 cold_hartid);
+
+/** Finalize domain tables */
+int sbi_domain_finalize(struct sbi_scratch *scratch);
 
 /** Initialize domains */
 int sbi_domain_init(struct sbi_scratch *scratch, u32 cold_hartid);

include/sbi/sbi_double_trap.h

@@ -0,0 +1,20 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <cleger@rivosinc.com>
+ */
+
+#ifndef __SBI_DOUBLE_TRAP_H__
+#define __SBI_DOUBLE_TRAP_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_trap.h>
+
+int sbi_double_trap_handler(struct sbi_trap_context *tcntx);
+
+void sbi_double_trap_init(struct sbi_scratch *scratch);
+
+#endif

include/sbi/sbi_ecall.h

@@ -13,7 +13,7 @@
 #include <sbi/sbi_types.h>
 #include <sbi/sbi_list.h>
 
-#define SBI_ECALL_VERSION_MAJOR		2
+#define SBI_ECALL_VERSION_MAJOR		3
 #define SBI_ECALL_VERSION_MINOR		0
 #define SBI_OPENSBI_IMPID		1
 

include/sbi/sbi_ecall_interface.h

@@ -291,6 +291,15 @@ struct sbi_pmu_event_info {
 #define SBI_PMU_CFG_FLAG_SET_UINH	(1 << 5)
 #define SBI_PMU_CFG_FLAG_SET_SINH	(1 << 6)
 #define SBI_PMU_CFG_FLAG_SET_MINH	(1 << 7)
+/* Event configuration mask */
+#define SBI_PMU_CFG_EVENT_MASK		\
+	( \
+		SBI_PMU_CFG_FLAG_SET_VUINH | \
+		SBI_PMU_CFG_FLAG_SET_VSINH | \
+		SBI_PMU_CFG_FLAG_SET_UINH  | \
+		SBI_PMU_CFG_FLAG_SET_SINH  | \
+		SBI_PMU_CFG_FLAG_SET_MINH    \
+	)
 
 /* Flags defined for counter start function */
 #define SBI_PMU_START_FLAG_SET_INIT_VALUE (1 << 0)
@@ -380,10 +389,12 @@ enum sbi_sse_attr_id {
 
 #define SBI_SSE_ATTR_CONFIG_ONESHOT	(1 << 0)
 
-#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPP	BIT(0)
-#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPIE	BIT(1)
+#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPP	BIT(0)
+#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPIE	BIT(1)
 #define SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPV	BIT(2)
 #define SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPVP	BIT(3)
+#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPELP	BIT(4)
+#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SDT	BIT(5)
 
 enum sbi_sse_state {
 	SBI_SSE_STATE_UNUSED		= 0,
@@ -393,48 +404,77 @@ enum sbi_sse_state {
 };
 
 /* SBI SSE Event IDs. */
-#define SBI_SSE_EVENT_LOCAL_RAS			0x00000000
+/* Range 0x00000000 - 0x0000ffff */
+#define SBI_SSE_EVENT_LOCAL_HIGH_PRIO_RAS	0x00000000
 #define SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP		0x00000001
+#define SBI_SSE_EVENT_LOCAL_RESERVED_0_START	0x00000002
+#define SBI_SSE_EVENT_LOCAL_RESERVED_0_END	0x00003fff
 #define SBI_SSE_EVENT_LOCAL_PLAT_0_START	0x00004000
 #define SBI_SSE_EVENT_LOCAL_PLAT_0_END		0x00007fff
-#define SBI_SSE_EVENT_GLOBAL_RAS		0x00008000
+
+#define SBI_SSE_EVENT_GLOBAL_HIGH_PRIO_RAS	0x00008000
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_0_START	0x00008001
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_0_END	0x0000bfff
 #define SBI_SSE_EVENT_GLOBAL_PLAT_0_START	0x0000c000
 #define SBI_SSE_EVENT_GLOBAL_PLAT_0_END		0x0000ffff
 
-#define SBI_SSE_EVENT_LOCAL_PMU			0x00010000
+/* Range 0x00010000 - 0x0001ffff */
+#define SBI_SSE_EVENT_LOCAL_PMU_OVERFLOW	0x00010000
+#define SBI_SSE_EVENT_LOCAL_RESERVED_1_START	0x00010001
+#define SBI_SSE_EVENT_LOCAL_RESERVED_1_END	0x00013fff
 #define SBI_SSE_EVENT_LOCAL_PLAT_1_START	0x00014000
 #define SBI_SSE_EVENT_LOCAL_PLAT_1_END		0x00017fff
+
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_1_START	0x00018000
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_1_END	0x0001bfff
 #define SBI_SSE_EVENT_GLOBAL_PLAT_1_START	0x0001c000
 #define SBI_SSE_EVENT_GLOBAL_PLAT_1_END		0x0001ffff
 
-#define SBI_SSE_EVENT_LOCAL_PLAT_2_START	0x00024000
-#define SBI_SSE_EVENT_LOCAL_PLAT_2_END		0x00027fff
-#define SBI_SSE_EVENT_GLOBAL_PLAT_2_START	0x0002c000
-#define SBI_SSE_EVENT_GLOBAL_PLAT_2_END		0x0002ffff
+/* Range 0x00100000 - 0x0010ffff */
+#define SBI_SSE_EVENT_LOCAL_LOW_PRIO_RAS	0x00100000
+#define SBI_SSE_EVENT_LOCAL_RESERVED_2_START	0x00100001
+#define SBI_SSE_EVENT_LOCAL_RESERVED_2_END	0x00103fff
+#define SBI_SSE_EVENT_LOCAL_PLAT_2_START	0x00104000
+#define SBI_SSE_EVENT_LOCAL_PLAT_2_END		0x00107fff
 
+#define SBI_SSE_EVENT_GLOBAL_LOW_PRIO_RAS	0x00108000
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_2_START	0x00108001
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_2_END	0x0010bfff
+#define SBI_SSE_EVENT_GLOBAL_PLAT_2_START	0x0010c000
+#define SBI_SSE_EVENT_GLOBAL_PLAT_2_END		0x0010ffff
+
+/* Range 0xffff0000 - 0xffffffff */
 #define SBI_SSE_EVENT_LOCAL_SOFTWARE		0xffff0000
+#define SBI_SSE_EVENT_LOCAL_RESERVED_3_START	0xffff0001
+#define SBI_SSE_EVENT_LOCAL_RESERVED_3_END	0xffff3fff
 #define SBI_SSE_EVENT_LOCAL_PLAT_3_START	0xffff4000
 #define SBI_SSE_EVENT_LOCAL_PLAT_3_END		0xffff7fff
+
 #define SBI_SSE_EVENT_GLOBAL_SOFTWARE		0xffff8000
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_3_START	0xffff8001
+#define SBI_SSE_EVENT_GLOBAL_RESERVED_3_END	0xffffbfff
 #define SBI_SSE_EVENT_GLOBAL_PLAT_3_START	0xffffc000
 #define SBI_SSE_EVENT_GLOBAL_PLAT_3_END		0xffffffff
 
-#define SBI_SSE_EVENT_GLOBAL_BIT		(1 << 15)
-#define SBI_SSE_EVENT_PLATFORM_BIT		(1 << 14)
+#define SBI_SSE_EVENT_GLOBAL_BIT		BIT(15)
+#define SBI_SSE_EVENT_PLATFORM_BIT		BIT(14)
 
 /* SBI function IDs for MPXY extension */
-#define SBI_EXT_MPXY_SET_SHMEM			0x0
-#define SBI_EXT_MPXY_GET_CHANNEL_IDS		0x1
-#define SBI_EXT_MPXY_READ_ATTRS			0x2
-#define SBI_EXT_MPXY_WRITE_ATTRS		0x3
-#define SBI_EXT_MPXY_SEND_MSG_WITH_RESP		0x4
-#define SBI_EXT_MPXY_SEND_MSG_NO_RESP		0x5
-#define SBI_EXT_MPXY_GET_NOTIFICATION_EVENTS	0x6
+#define SBI_EXT_MPXY_GET_SHMEM_SIZE		0x0
+#define SBI_EXT_MPXY_SET_SHMEM			0x1
+#define SBI_EXT_MPXY_GET_CHANNEL_IDS		0x2
+#define SBI_EXT_MPXY_READ_ATTRS			0x3
+#define SBI_EXT_MPXY_WRITE_ATTRS		0x4
+#define SBI_EXT_MPXY_SEND_MSG_WITH_RESP		0x5
+#define SBI_EXT_MPXY_SEND_MSG_WITHOUT_RESP	0x6
+#define SBI_EXT_MPXY_GET_NOTIFICATION_EVENTS	0x7
 
 /* SBI base specification related macros */
 #define SBI_SPEC_VERSION_MAJOR_OFFSET		24
 #define SBI_SPEC_VERSION_MAJOR_MASK		0x7f
 #define SBI_SPEC_VERSION_MINOR_MASK		0xffffff
+#define SBI_EXT_EXPERIMENTAL_START		0x08000000
+#define SBI_EXT_EXPERIMENTAL_END		0x08FFFFFF
 #define SBI_EXT_VENDOR_START			0x09000000
 #define SBI_EXT_VENDOR_END			0x09FFFFFF
 #define SBI_EXT_FIRMWARE_START			0x0A000000
@@ -455,8 +495,9 @@ enum sbi_sse_state {
 #define SBI_ERR_BAD_RANGE			-11
 #define SBI_ERR_TIMEOUT				-12
 #define SBI_ERR_IO				-13
+#define SBI_ERR_DENIED_LOCKED			-14
 
-#define SBI_LAST_ERR				SBI_ERR_BAD_RANGE
+#define SBI_LAST_ERR				SBI_ERR_DENIED_LOCKED
 
 /* clang-format on */
 

include/sbi/sbi_error.h

@@ -29,6 +29,7 @@
 #define SBI_ETIMEOUT		SBI_ERR_TIMEOUT
 #define SBI_ETIMEDOUT		SBI_ERR_TIMEOUT
 #define SBI_EIO			SBI_ERR_IO
+#define SBI_EDENIED_LOCKED	SBI_ERR_DENIED_LOCKED
 
 #define SBI_ENODEV		-1000
 #define SBI_ENOSYS		-1001

include/sbi/sbi_hart.h

@@ -31,7 +31,7 @@ enum sbi_hart_extensions {
 	SBI_HART_EXT_SMAIA = 0,
 	/** HART has Smepmp */
 	SBI_HART_EXT_SMEPMP,
-	/** HART has Smstateen CSR **/
+	/** HART has Smstateen extension **/
 	SBI_HART_EXT_SMSTATEEN,
 	/** Hart has Sscofpmt extension */
 	SBI_HART_EXT_SSCOFPMF,
@@ -75,6 +75,18 @@ enum sbi_hart_extensions {
 	SBI_HART_EXT_ZICFISS,
 	/** Hart has Ssdbltrp extension */
 	SBI_HART_EXT_SSDBLTRP,
+	/** HART has CTR M-mode CSRs */
+	SBI_HART_EXT_SMCTR,
+	/** HART has CTR S-mode CSRs */
+	SBI_HART_EXT_SSCTR,
+	/** Hart has Ssqosid extension */
+	SBI_HART_EXT_SSQOSID,
+	/** HART has Ssstateen extension **/
+	SBI_HART_EXT_SSSTATEEN,
+	/** Hart has Xsfcflushdlone extension */
+	SBI_HART_EXT_XSIFIVE_CFLUSH_D_L1,
+	/** Hart has Xsfcease extension */
+	SBI_HART_EXT_XSIFIVE_CEASE,
 
 	/** Maximum index of Hart extension */
 	SBI_HART_EXT_MAX,
@@ -87,6 +99,14 @@ struct sbi_hart_ext_data {
 
 extern const struct sbi_hart_ext_data sbi_hart_ext[];
 
+/** CSRs should be detected by access and trapping */
+enum sbi_hart_csrs {
+	SBI_HART_CSR_CYCLE = 0,
+	SBI_HART_CSR_TIME,
+	SBI_HART_CSR_INSTRET,
+	SBI_HART_CSR_MAX,
+};
+
 /*
  * Smepmp enforces access boundaries between M-mode and
  * S/U-mode. When it is enabled, the PMPs are programmed
@@ -106,6 +126,7 @@ struct sbi_hart_features {
 	bool detected;
 	int priv_version;
 	unsigned long extensions[BITS_TO_LONGS(SBI_HART_EXT_MAX)];
+	unsigned long csrs[BITS_TO_LONGS(SBI_HART_CSR_MAX)];
 	unsigned int pmp_count;
 	unsigned int pmp_addr_bits;
 	unsigned int pmp_log2gran;
@@ -119,10 +140,6 @@ 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);
-static inline ulong sbi_hart_expected_trap_addr(void)
-{
-	return (ulong)sbi_hart_expected_trap;
-}
 
 unsigned int sbi_hart_mhpm_mask(struct sbi_scratch *scratch);
 void sbi_hart_delegation_dump(struct sbi_scratch *scratch,
@@ -131,6 +148,7 @@ unsigned int sbi_hart_pmp_count(struct sbi_scratch *scratch);
 unsigned int sbi_hart_pmp_log2gran(struct sbi_scratch *scratch);
 unsigned int sbi_hart_pmp_addrbits(struct sbi_scratch *scratch);
 unsigned int sbi_hart_mhpm_bits(struct sbi_scratch *scratch);
+bool sbi_hart_smepmp_is_fw_region(unsigned int pmp_idx);
 int sbi_hart_pmp_configure(struct sbi_scratch *scratch);
 int sbi_hart_map_saddr(unsigned long base, unsigned long size);
 int sbi_hart_unmap_saddr(void);
@@ -144,6 +162,7 @@ bool sbi_hart_has_extension(struct sbi_scratch *scratch,
 			    enum sbi_hart_extensions ext);
 void sbi_hart_get_extensions_str(struct sbi_scratch *scratch,
 				 char *extension_str, int nestr);
+bool sbi_hart_has_csr(struct sbi_scratch *scratch, enum sbi_hart_csrs csr);
 
 void __attribute__((noreturn)) sbi_hart_hang(void);
 

include/sbi/sbi_hartmask.h

@@ -181,6 +181,17 @@ static inline void sbi_hartmask_xor(struct sbi_hartmask *dstp,
 		   sbi_hartmask_bits(src2p), SBI_HARTMASK_MAX_BITS);
 }
 
+/**
+ * Count of bits in *srcp
+ * @param srcp the hartmask to count bits in
+ *
+ * Return: count of bits set in *srcp
+ */
+static inline int sbi_hartmask_weight(const struct sbi_hartmask *srcp)
+{
+	return bitmap_weight(sbi_hartmask_bits(srcp), SBI_HARTMASK_MAX_BITS);
+}
+
 /**
  * Iterate over each HART index in hartmask
  * __i hart index

include/sbi/sbi_illegal_atomic.h

@@ -0,0 +1,17 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 MIPS
+ *
+ */
+
+#ifndef __SBI_ILLEGAL_ATOMIC_H__
+#define __SBI_ILLEGAL_ATOMIC_H__
+
+#include <sbi/sbi_types.h>
+
+struct sbi_trap_regs;
+
+int sbi_illegal_atomic(ulong insn, struct sbi_trap_regs *regs);
+
+#endif

include/sbi/sbi_illegal_insn.h

@@ -14,6 +14,10 @@
 
 struct sbi_trap_context;
 
+typedef int (*illegal_insn_func)(ulong insn, struct sbi_trap_regs *regs);
+
+int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs);
+
 int sbi_illegal_insn_handler(struct sbi_trap_context *tcntx);
 
 #endif

include/sbi/sbi_init.h

@@ -16,6 +16,8 @@ struct sbi_scratch;
 
 void __noreturn sbi_init(struct sbi_scratch *scratch);
 
+void sbi_revert_entry_count(struct sbi_scratch *scratch);
+
 unsigned long sbi_entry_count(u32 hartindex);
 
 unsigned long sbi_init_count(u32 hartindex);

include/sbi/sbi_ipi.h

@@ -23,6 +23,9 @@ struct sbi_ipi_device {
 	/** Name of the IPI device */
 	char name[32];
 
+	/** Ratings of the IPI device (higher is better) */
+	unsigned long rating;
+
 	/** Send IPI to a target HART index */
 	void (*ipi_send)(u32 hart_index);
 
@@ -85,13 +88,13 @@ int sbi_ipi_send_halt(ulong hmask, ulong hbase);
 
 void sbi_ipi_process(void);
 
-int sbi_ipi_raw_send(u32 hartindex);
+int sbi_ipi_raw_send(u32 hartindex, bool all_devices);
 
-void sbi_ipi_raw_clear(void);
+void sbi_ipi_raw_clear(bool all_devices);
 
 const struct sbi_ipi_device *sbi_ipi_get_device(void);
 
-void sbi_ipi_set_device(const struct sbi_ipi_device *dev);
+void sbi_ipi_add_device(const struct sbi_ipi_device *dev);
 
 int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot);
 

include/sbi/sbi_list.h

@@ -160,4 +160,28 @@ static inline void sbi_list_del_init(struct sbi_dlist *entry)
 	     &pos->member != (head); 	\
 	     pos = sbi_list_entry(pos->member.next, typeof(*pos), member))
 
+/**
+ * Iterate over list of given type safe against removal of list entry
+ * @param pos the type * to use as a loop cursor.
+ * @param n another type * to use as temporary storage.
+ * @param head the head for your list.
+ * @param member the name of the list_struct within the struct.
+ */
+#define sbi_list_for_each_entry_safe(pos, n, head, member) \
+	for (pos = sbi_list_entry((head)->next, typeof(*pos), member),	\
+	     n = sbi_list_entry(pos->member.next, typeof(*pos), member);	\
+	     &pos->member != (head);	\
+	     pos = n, n = sbi_list_entry(pos->member.next, typeof(*pos), member))
+
+/**
+ * Iterate over list of given type in reverse order
+ * @param pos the type * to use as a loop cursor.
+ * @param head the head for your list.
+ * @param member the name of the list_struct within the struct.
+ */
+#define sbi_list_for_each_entry_reverse(pos, head, member) \
+	for (pos = sbi_list_entry((head)->prev, typeof(*pos), member); \
+	     &pos->member != (head); \
+	     pos = sbi_list_entry(pos->member.prev, typeof(*pos), member))
+
 #endif

include/sbi/sbi_mpxy.h

@@ -153,11 +153,13 @@ int sbi_mpxy_init(struct sbi_scratch *scratch);
 /** Check if some Message proxy channel is available */
 bool sbi_mpxy_channel_available(void);
 
-/** Set Message proxy shared memory on the calling HART */
-int sbi_mpxy_set_shmem(unsigned long shmem_size,
-			unsigned long shmem_phys_lo,
-			unsigned long shmem_phys_hi,
-			unsigned long flags);
+/** Get message proxy shared memory size */
+unsigned long sbi_mpxy_get_shmem_size(void);
+
+/** Set message proxy shared memory on the calling HART */
+int sbi_mpxy_set_shmem(unsigned long shmem_phys_lo,
+		       unsigned long shmem_phys_hi,
+		       unsigned long flags);
 
 /** Get channel IDs list */
 int sbi_mpxy_get_channel_ids(u32 start_index);

include/sbi/sbi_platform.h

@@ -39,6 +39,8 @@
 #define SBI_PLATFORM_FIRMWARE_CONTEXT_OFFSET (0x60 + __SIZEOF_POINTER__)
 /** Offset of hart_index2id in struct sbi_platform */
 #define SBI_PLATFORM_HART_INDEX2ID_OFFSET (0x60 + (__SIZEOF_POINTER__ * 2))
+/** Offset of cbom_block_size in struct sbi_platform */
+#define SBI_PLATFORM_CBOM_BLOCK_SIZE_OFFSET (0x60 + (__SIZEOF_POINTER__ * 3))
 
 #define SBI_PLATFORM_TLB_RANGE_FLUSH_LIMIT_DEFAULT		(1UL << 12)
 
@@ -114,9 +116,6 @@ struct sbi_platform_operations {
 	/** Initialize the platform interrupt controller during cold boot */
 	int (*irqchip_init)(void);
 
-	/** Initialize IPI during cold boot */
-	int (*ipi_init)(void);
-
 	/** Get tlb flush limit value **/
 	u64 (*get_tlbr_flush_limit)(void);
 
@@ -129,8 +128,6 @@ struct sbi_platform_operations {
 	/** Initialize the platform Message Proxy(MPXY) driver */
 	int (*mpxy_init)(void);
 
-	/** Check if SBI vendor extension is implemented or not */
-	bool (*vendor_ext_check)(void);
 	/** platform specific SBI extension implementation provider */
 	int (*vendor_ext_provider)(long funcid,
 				   struct sbi_trap_regs *regs,
@@ -142,6 +139,13 @@ struct sbi_platform_operations {
 	/** platform specific handler to fixup store fault */
 	int (*emulate_store)(int wlen, unsigned long addr,
 			     union sbi_ldst_data in_val);
+
+	/** platform specific pmp setup on current HART */
+	void (*pmp_set)(unsigned int n, unsigned long flags,
+			unsigned long prot, unsigned long addr,
+			unsigned long log2len);
+	/** platform specific pmp disable on current HART */
+	void (*pmp_disable)(unsigned int n);
 };
 
 /** Platform default per-HART stack size for exception/interrupt handling */
@@ -169,7 +173,7 @@ struct sbi_platform {
 	char name[64];
 	/** Supported features */
 	u64 features;
-	/** Total number of HARTs */
+	/** Total number of HARTs (at most SBI_HARTMASK_MAX_BITS) */
 	u32 hart_count;
 	/** Per-HART stack size for exception/interrupt handling */
 	u32 hart_stack_size;
@@ -184,70 +188,34 @@ struct sbi_platform {
 	/**
 	 * HART index to HART id table
 	 *
-	 * For used HART index <abc>:
+	 * If hart_index2id != NULL then the table must contain a mapping
+	 * for each HART index 0 <= <abc> < hart_count:
 	 *     hart_index2id[<abc>] = some HART id
-	 * For unused HART index <abc>:
-	 *     hart_index2id[<abc>] = -1U
 	 *
 	 * If hart_index2id == NULL then we assume identity mapping
 	 *     hart_index2id[<abc>] = <abc>
-	 *
-	 * We have only two restrictions:
-	 * 1. HART index < sbi_platform hart_count
-	 * 2. HART id < SBI_HARTMASK_MAX_BITS
 	 */
 	const u32 *hart_index2id;
+	/** Allocation alignment for Scratch */
+	unsigned long cbom_block_size;
 };
 
 /**
  * Prevent modification of struct sbi_platform from affecting
  * SBI_PLATFORM_xxx_OFFSET
  */
-_Static_assert(
-	offsetof(struct sbi_platform, opensbi_version)
-		== SBI_PLATFORM_OPENSBI_VERSION_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_OPENSBI_VERSION_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, platform_version)
-		== SBI_PLATFORM_VERSION_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_VERSION_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, name)
-		== SBI_PLATFORM_NAME_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_NAME_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, features)
-		== SBI_PLATFORM_FEATURES_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_FEATURES_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, hart_count)
-		== SBI_PLATFORM_HART_COUNT_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_HART_COUNT_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, hart_stack_size)
-		== SBI_PLATFORM_HART_STACK_SIZE_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_HART_STACK_SIZE_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, platform_ops_addr)
-		== SBI_PLATFORM_OPS_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_OPS_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, firmware_context)
-		== SBI_PLATFORM_FIRMWARE_CONTEXT_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_FIRMWARE_CONTEXT_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_platform, hart_index2id)
-		== SBI_PLATFORM_HART_INDEX2ID_OFFSET,
-	"struct sbi_platform definition has changed, please redefine "
-	"SBI_PLATFORM_HART_INDEX2ID_OFFSET");
+assert_member_offset(struct sbi_platform, opensbi_version, SBI_PLATFORM_OPENSBI_VERSION_OFFSET);
+assert_member_offset(struct sbi_platform, platform_version, SBI_PLATFORM_VERSION_OFFSET);
+assert_member_offset(struct sbi_platform, name, SBI_PLATFORM_NAME_OFFSET);
+assert_member_offset(struct sbi_platform, features, SBI_PLATFORM_FEATURES_OFFSET);
+assert_member_offset(struct sbi_platform, hart_count, SBI_PLATFORM_HART_COUNT_OFFSET);
+assert_member_offset(struct sbi_platform, hart_stack_size, SBI_PLATFORM_HART_STACK_SIZE_OFFSET);
+assert_member_offset(struct sbi_platform, heap_size, SBI_PLATFORM_HEAP_SIZE_OFFSET);
+assert_member_offset(struct sbi_platform, reserved, SBI_PLATFORM_RESERVED_OFFSET);
+assert_member_offset(struct sbi_platform, platform_ops_addr, SBI_PLATFORM_OPS_OFFSET);
+assert_member_offset(struct sbi_platform, firmware_context, SBI_PLATFORM_FIRMWARE_CONTEXT_OFFSET);
+assert_member_offset(struct sbi_platform, hart_index2id, SBI_PLATFORM_HART_INDEX2ID_OFFSET);
+assert_member_offset(struct sbi_platform, cbom_block_size, SBI_PLATFORM_CBOM_BLOCK_SIZE_OFFSET);
 
 /** Get pointer to sbi_platform for sbi_scratch pointer */
 #define sbi_platform_ptr(__s) \
@@ -331,7 +299,7 @@ static inline u32 sbi_platform_tlb_fifo_num_entries(const struct sbi_platform *p
 {
 	if (plat && sbi_platform_ops(plat)->get_tlb_num_entries)
 		return sbi_platform_ops(plat)->get_tlb_num_entries();
-	return sbi_scratch_last_hartindex() + 1;
+	return sbi_hart_count();
 }
 
 /**
@@ -557,20 +525,6 @@ static inline int sbi_platform_irqchip_init(const struct sbi_platform *plat)
 	return 0;
 }
 
-/**
- * Initialize the platform IPI support during cold boot
- *
- * @param plat pointer to struct sbi_platform
- *
- * @return 0 on success and negative error code on failure
- */
-static inline int sbi_platform_ipi_init(const struct sbi_platform *plat)
-{
-	if (plat && sbi_platform_ops(plat)->ipi_init)
-		return sbi_platform_ops(plat)->ipi_init();
-	return 0;
-}
-
 /**
  * Initialize the platform timer during cold boot
  *
@@ -609,10 +563,7 @@ static inline int sbi_platform_mpxy_init(const struct sbi_platform *plat)
 static inline bool sbi_platform_vendor_ext_check(
 					const struct sbi_platform *plat)
 {
-	if (plat && sbi_platform_ops(plat)->vendor_ext_check)
-		return sbi_platform_ops(plat)->vendor_ext_check();
-
-	return false;
+	return plat && sbi_platform_ops(plat)->vendor_ext_provider;
 }
 
 /**
@@ -683,6 +634,38 @@ static inline int sbi_platform_emulate_store(const struct sbi_platform *plat,
 	return SBI_ENOTSUPP;
 }
 
+/**
+ * Platform specific PMP setup on current HART
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param n index of the pmp entry
+ * @param flags domain memregion flags
+ * @param prot attribute of the pmp entry
+ * @param addr address of the pmp entry
+ * @param log2len size of the pmp entry as power-of-2
+ */
+static inline void sbi_platform_pmp_set(const struct sbi_platform *plat,
+					unsigned int n, unsigned long flags,
+					unsigned long prot, unsigned long addr,
+					unsigned long log2len)
+{
+	if (plat && sbi_platform_ops(plat)->pmp_set)
+		sbi_platform_ops(plat)->pmp_set(n, flags, prot, addr, log2len);
+}
+
+/**
+ * Platform specific PMP disable on current HART
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param n index of the pmp entry
+ */
+static inline void sbi_platform_pmp_disable(const struct sbi_platform *plat,
+					    unsigned int n)
+{
+	if (plat && sbi_platform_ops(plat)->pmp_disable)
+		sbi_platform_ops(plat)->pmp_disable(n);
+}
+
 #endif
 
 #endif

include/sbi/sbi_pmu.h

@@ -114,6 +114,9 @@ void sbi_pmu_exit(struct sbi_scratch *scratch);
 /** Return the pmu irq bit depending on extension existence */
 int sbi_pmu_irq_bit(void);
 
+/** Return the pmu irq mask or 0 if the pmu overflow irq is not supported */
+unsigned long sbi_pmu_irq_mask(void);
+
 /**
  * Add the hardware event to counter mapping information. This should be called
  * from the platform code to update the mapping table.

include/sbi/sbi_scratch.h

@@ -93,61 +93,21 @@ struct sbi_scratch {
  * Prevent modification of struct sbi_scratch from affecting
  * SBI_SCRATCH_xxx_OFFSET
  */
-_Static_assert(
-	offsetof(struct sbi_scratch, fw_start)
-		== SBI_SCRATCH_FW_START_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_FW_START_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, fw_size)
-		== SBI_SCRATCH_FW_SIZE_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_FW_SIZE_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, next_arg1)
-		== SBI_SCRATCH_NEXT_ARG1_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_NEXT_ARG1_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, next_addr)
-		== SBI_SCRATCH_NEXT_ADDR_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_NEXT_ADDR_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, next_mode)
-		== SBI_SCRATCH_NEXT_MODE_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_NEXT_MODE_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, warmboot_addr)
-		== SBI_SCRATCH_WARMBOOT_ADDR_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_WARMBOOT_ADDR_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, platform_addr)
-		== SBI_SCRATCH_PLATFORM_ADDR_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_PLATFORM_ADDR_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, hartid_to_scratch)
-		== SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, trap_context)
-		== SBI_SCRATCH_TRAP_CONTEXT_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_TRAP_CONTEXT_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, tmp0)
-		== SBI_SCRATCH_TMP0_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_TMP0_OFFSET");
-_Static_assert(
-	offsetof(struct sbi_scratch, options)
-		== SBI_SCRATCH_OPTIONS_OFFSET,
-	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_OPTIONS_OFFSET");
+assert_member_offset(struct sbi_scratch, fw_start, SBI_SCRATCH_FW_START_OFFSET);
+assert_member_offset(struct sbi_scratch, fw_size, SBI_SCRATCH_FW_SIZE_OFFSET);
+assert_member_offset(struct sbi_scratch, fw_rw_offset, SBI_SCRATCH_FW_RW_OFFSET);
+assert_member_offset(struct sbi_scratch, fw_heap_offset, SBI_SCRATCH_FW_HEAP_OFFSET);
+assert_member_offset(struct sbi_scratch, fw_heap_size, SBI_SCRATCH_FW_HEAP_SIZE_OFFSET);
+assert_member_offset(struct sbi_scratch, next_arg1, SBI_SCRATCH_NEXT_ARG1_OFFSET);
+assert_member_offset(struct sbi_scratch, next_addr, SBI_SCRATCH_NEXT_ADDR_OFFSET);
+assert_member_offset(struct sbi_scratch, next_mode, SBI_SCRATCH_NEXT_MODE_OFFSET);
+assert_member_offset(struct sbi_scratch, warmboot_addr, SBI_SCRATCH_WARMBOOT_ADDR_OFFSET);
+assert_member_offset(struct sbi_scratch, platform_addr, SBI_SCRATCH_PLATFORM_ADDR_OFFSET);
+assert_member_offset(struct sbi_scratch, hartid_to_scratch, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET);
+assert_member_offset(struct sbi_scratch, trap_context, SBI_SCRATCH_TRAP_CONTEXT_OFFSET);
+assert_member_offset(struct sbi_scratch, tmp0, SBI_SCRATCH_TMP0_OFFSET);
+assert_member_offset(struct sbi_scratch, options, SBI_SCRATCH_OPTIONS_OFFSET);
+assert_member_offset(struct sbi_scratch, hartindex, SBI_SCRATCH_HARTINDEX_OFFSET);
 
 /** Possible options for OpenSBI library */
 enum sbi_scratch_options {
@@ -210,15 +170,18 @@ do {									\
 #define current_hartindex() \
 	(sbi_scratch_thishart_ptr()->hartindex)
 
-/** Last HART index having a sbi_scratch pointer */
-extern u32 last_hartindex_having_scratch;
+/** Number of harts managed by this OpenSBI instance */
+extern u32 sbi_scratch_hart_count;
 
-/** Get last HART index having a sbi_scratch pointer */
-#define sbi_scratch_last_hartindex()	last_hartindex_having_scratch
+/** Get the number of harts managed by this OpenSBI instance */
+#define sbi_hart_count() sbi_scratch_hart_count
+
+/** Iterate over the harts managed by this OpenSBI instance */
+#define sbi_for_each_hartindex(__var) \
+	for (u32 __var = 0; __var < sbi_hart_count(); ++__var)
 
 /** Check whether a particular HART index is valid or not */
-#define sbi_hartindex_valid(__hartindex) \
-(((__hartindex) <= sbi_scratch_last_hartindex()) ? true : false)
+#define sbi_hartindex_valid(__hartindex) ((__hartindex) < sbi_hart_count())
 
 /** HART index to HART id table */
 extern u32 hartindex_to_hartid_table[];
@@ -226,7 +189,7 @@ extern u32 hartindex_to_hartid_table[];
 /** Get sbi_scratch from HART index */
 #define sbi_hartindex_to_hartid(__hartindex)		\
 ({							\
-	((__hartindex) <= sbi_scratch_last_hartindex()) ?\
+	((__hartindex) < SBI_HARTMASK_MAX_BITS) ?	\
 	hartindex_to_hartid_table[__hartindex] : -1U;	\
 })
 
@@ -236,8 +199,8 @@ extern struct sbi_scratch *hartindex_to_scratch_table[];
 /** Get sbi_scratch from HART index */
 #define sbi_hartindex_to_scratch(__hartindex)		\
 ({							\
-	((__hartindex) <= sbi_scratch_last_hartindex()) ?\
-	hartindex_to_scratch_table[__hartindex] : NULL;\
+	((__hartindex) < SBI_HARTMASK_MAX_BITS) ?	\
+	hartindex_to_scratch_table[__hartindex] : NULL;	\
 })
 
 /**

include/sbi/sbi_slist.h

@@ -0,0 +1,33 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Simple simply-linked list library.
+ *
+ * Copyright (c) 2025 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <cleger@rivosinc.com>
+ */
+
+#ifndef __SBI_SLIST_H__
+#define __SBI_SLIST_H__
+
+#include <sbi/sbi_types.h>
+
+#define SBI_SLIST_HEAD_INIT(_ptr)	(_ptr)
+#define SBI_SLIST_HEAD(_lname, _stype) struct _stype *_lname
+#define SBI_SLIST_NODE(_stype) SBI_SLIST_HEAD(next, _stype)
+#define SBI_SLIST_NODE_INIT(_ptr) .next = _ptr
+
+#define SBI_INIT_SLIST_HEAD(_head) (_head) = NULL
+
+#define SBI_SLIST_ADD(_ptr, _head) \
+do { \
+	(_ptr)->next = _head; \
+	(_head) = _ptr; \
+} while (0)
+
+#define SBI_SLIST_FOR_EACH_ENTRY(_ptr, _head) \
+	for (_ptr = _head; _ptr; _ptr = _ptr->next)
+
+#endif

include/sbi/sbi_sse.h

@@ -54,12 +54,12 @@ struct sbi_sse_cb_ops {
 	void (*disable_cb)(uint32_t event_id);
 };
 
-/* Set the callback operations for an event
- * @param event_id Event identifier (SBI_SSE_EVENT_*)
- * @param cb_ops Callback operations
+/* Add a supported event with associated callback operations
+ * @param event_id Event identifier (SBI_SSE_EVENT_* or a custom platform one)
+ * @param cb_ops Callback operations (Can be NULL if any)
  * @return 0 on success, error otherwise
  */
-int sbi_sse_set_cb_ops(uint32_t event_id, const struct sbi_sse_cb_ops *cb_ops);
+int sbi_sse_add_event(uint32_t event_id, const struct sbi_sse_cb_ops *cb_ops);
 
 /* Inject an event to the current hard
  * @param event_id Event identifier (SBI_SSE_EVENT_*)

include/sbi/sbi_system.h

@@ -69,11 +69,18 @@ struct sbi_system_suspend_device {
 	 *     return from system_suspend() may ignore this parameter.
 	 */
 	int (*system_suspend)(u32 sleep_type, unsigned long mmode_resume_addr);
+
+	/**
+	 * Resume the system from system suspend
+	 */
+	void (*system_resume)(void);
 };
 
 const struct sbi_system_suspend_device *sbi_system_suspend_get_device(void);
 void sbi_system_suspend_set_device(struct sbi_system_suspend_device *dev);
 void sbi_system_suspend_test_enable(void);
+void sbi_system_resume(void);
+bool sbi_system_is_suspended(void);
 bool sbi_system_suspend_supported(u32 sleep_type);
 int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque);
 

include/sbi/sbi_trap.h

@@ -112,10 +112,13 @@
 /** Size (in bytes) of sbi_trap_info */
 #define SBI_TRAP_INFO_SIZE SBI_TRAP_INFO_OFFSET(last)
 
+#define STACK_BOUNDARY 16
+#define ALIGN_TO_BOUNDARY(x, a) (((x) + (a) - 1) & ~((a) - 1))
+
 /** Size (in bytes) of sbi_trap_context */
-#define SBI_TRAP_CONTEXT_SIZE (SBI_TRAP_REGS_SIZE + \
+#define SBI_TRAP_CONTEXT_SIZE ALIGN_TO_BOUNDARY((SBI_TRAP_REGS_SIZE + \
 			       SBI_TRAP_INFO_SIZE + \
-			       __SIZEOF_POINTER__)
+			       __SIZEOF_POINTER__), STACK_BOUNDARY)
 
 #ifndef __ASSEMBLER__
 
@@ -124,70 +127,75 @@
 
 /** Representation of register state at time of trap/interrupt */
 struct sbi_trap_regs {
-	/** zero register state */
-	unsigned long zero;
-	/** ra register state */
-	unsigned long ra;
-	/** sp register state */
-	unsigned long sp;
-	/** gp register state */
-	unsigned long gp;
-	/** tp register state */
-	unsigned long tp;
-	/** t0 register state */
-	unsigned long t0;
-	/** t1 register state */
-	unsigned long t1;
-	/** t2 register state */
-	unsigned long t2;
-	/** s0 register state */
-	unsigned long s0;
-	/** s1 register state */
-	unsigned long s1;
-	/** a0 register state */
-	unsigned long a0;
-	/** a1 register state */
-	unsigned long a1;
-	/** a2 register state */
-	unsigned long a2;
-	/** a3 register state */
-	unsigned long a3;
-	/** a4 register state */
-	unsigned long a4;
-	/** a5 register state */
-	unsigned long a5;
-	/** a6 register state */
-	unsigned long a6;
-	/** a7 register state */
-	unsigned long a7;
-	/** s2 register state */
-	unsigned long s2;
-	/** s3 register state */
-	unsigned long s3;
-	/** s4 register state */
-	unsigned long s4;
-	/** s5 register state */
-	unsigned long s5;
-	/** s6 register state */
-	unsigned long s6;
-	/** s7 register state */
-	unsigned long s7;
-	/** s8 register state */
-	unsigned long s8;
-	/** s9 register state */
-	unsigned long s9;
-	/** s10 register state */
-	unsigned long s10;
-	/** s11 register state */
-	unsigned long s11;
-	/** t3 register state */
-	unsigned long t3;
-	/** t4 register state */
-	unsigned long t4;
-	/** t5 register state */
-	unsigned long t5;
-	/** t6 register state */
-	unsigned long t6;
+	union {
+		unsigned long gprs[32];
+		struct {
+			/** zero register state */
+			unsigned long zero;
+			/** ra register state */
+			unsigned long ra;
+			/** sp register state */
+			unsigned long sp;
+			/** gp register state */
+			unsigned long gp;
+			/** tp register state */
+			unsigned long tp;
+			/** t0 register state */
+			unsigned long t0;
+			/** t1 register state */
+			unsigned long t1;
+			/** t2 register state */
+			unsigned long t2;
+			/** s0 register state */
+			unsigned long s0;
+			/** s1 register state */
+			unsigned long s1;
+			/** a0 register state */
+			unsigned long a0;
+			/** a1 register state */
+			unsigned long a1;
+			/** a2 register state */
+			unsigned long a2;
+			/** a3 register state */
+			unsigned long a3;
+			/** a4 register state */
+			unsigned long a4;
+			/** a5 register state */
+			unsigned long a5;
+			/** a6 register state */
+			unsigned long a6;
+			/** a7 register state */
+			unsigned long a7;
+			/** s2 register state */
+			unsigned long s2;
+			/** s3 register state */
+			unsigned long s3;
+			/** s4 register state */
+			unsigned long s4;
+			/** s5 register state */
+			unsigned long s5;
+			/** s6 register state */
+			unsigned long s6;
+			/** s7 register state */
+			unsigned long s7;
+			/** s8 register state */
+			unsigned long s8;
+			/** s9 register state */
+			unsigned long s9;
+			/** s10 register state */
+			unsigned long s10;
+			/** s11 register state */
+			unsigned long s11;
+			/** t3 register state */
+			unsigned long t3;
+			/** t4 register state */
+			unsigned long t4;
+			/** t5 register state */
+			unsigned long t5;
+			/** t6 register state */
+			unsigned long t6;
+		};
+	};
 	/** mepc register state */
 	unsigned long mepc;
 	/** mstatus register state */
@@ -196,6 +204,21 @@ struct sbi_trap_regs {
 	unsigned long mstatusH;
 };
 
+_Static_assert(
+	sizeof(((struct sbi_trap_regs *)0)->gprs) ==
+	offsetof(struct sbi_trap_regs, t6) +
+	sizeof(((struct sbi_trap_regs *)0)->t6),
+	"struct sbi_trap_regs's layout differs between gprs and named members");
+
+#define REG_VAL(idx, regs)		((regs)->gprs[(idx)])
+
+#define GET_RS1(insn, regs)		REG_VAL(GET_RS1_NUM(insn), regs)
+#define GET_RS2(insn, regs)		REG_VAL(GET_RS2_NUM(insn), regs)
+#define GET_RS1S(insn, regs)		REG_VAL(GET_RS1S_NUM(insn), regs)
+#define GET_RS2S(insn, regs)		REG_VAL(GET_RS2S_NUM(insn), regs)
+#define GET_RS2C(insn, regs)		REG_VAL(GET_RS2C_NUM(insn), regs)
+#define SET_RD(insn, regs, val)		(REG_VAL(GET_RD_NUM(insn), regs) = (val))
+
 /** Representation of trap details */
 struct sbi_trap_info {
 	/** cause Trap exception cause */

include/sbi/sbi_trap_ldst.h

@@ -28,8 +28,6 @@ int sbi_load_access_handler(struct sbi_trap_context *tcntx);
 
 int sbi_store_access_handler(struct sbi_trap_context *tcntx);
 
-int sbi_double_trap_handler(struct sbi_trap_context *tcntx);
-
 ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
 				 ulong addr_offset);
 

include/sbi/sbi_types.h

@@ -14,7 +14,7 @@
 
 /* clang-format off */
 
-typedef char			s8;
+typedef signed char		s8;
 typedef unsigned char		u8;
 typedef unsigned char		uint8_t;
 
@@ -96,6 +96,13 @@ typedef uint64_t		be64_t;
 	const typeof(((type *)0)->member) * __mptr = (ptr);	\
 	(type *)((char *)__mptr - offsetof(type, member)); })
 
+
+#define assert_member_offset(type, member, offset)			\
+	_Static_assert(							\
+		(offsetof(type, member)) == (offset ),			\
+		"The offset " #offset " of " #member " in " #type	\
+		"is not correct, please redefine it.")
+
 #define array_size(x) 	(sizeof(x) / sizeof((x)[0]))
 
 #define MAX(a, b) ((a) > (b) ? (a) : (b))

include/sbi/sbi_version.h

@@ -11,7 +11,7 @@
 #define __SBI_VERSION_H__
 
 #define OPENSBI_VERSION_MAJOR 1
-#define OPENSBI_VERSION_MINOR 6
+#define OPENSBI_VERSION_MINOR 7
 
 /**
  * OpenSBI 32-bit version with:

include/sbi/sbi_visibility.h

@@ -0,0 +1,18 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive
+ */
+
+#ifndef __SBI_VISIBILITY_H__
+#define __SBI_VISIBILITY_H__
+
+#ifndef __DTS__
+/*
+ * Declare all global objects with hidden visibility so access is PC-relative
+ * instead of going through the GOT.
+ */
+#pragma GCC visibility push(hidden)
+#endif
+
+#endif

include/sbi_utils/cache/cache.h

@@ -0,0 +1,69 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive Inc.
+ */
+
+#ifndef __CACHE_H__
+#define __CACHE_H__
+
+#include <sbi/sbi_list.h>
+#include <sbi/sbi_types.h>
+
+#define CACHE_NAME_LEN	32
+
+struct cache_device;
+
+struct cache_ops {
+	/** Warm init **/
+	int (*warm_init)(struct cache_device *dev);
+	/** Flush entire cache **/
+	int (*cache_flush_all)(struct cache_device *dev);
+};
+
+struct cache_device {
+	/** Name of the device **/
+	char name[CACHE_NAME_LEN];
+	/** List node for search **/
+	struct sbi_dlist node;
+	/** Point to the next level cache **/
+	struct cache_device *next;
+	/** Cache Management Operations **/
+	struct cache_ops *ops;
+	/** CPU private cache **/
+	bool cpu_private;
+	/** The unique id of this cache device **/
+	u32 id;
+};
+
+/**
+ * Find a registered cache device
+ *
+ * @param id unique ID of the cache device
+ *
+ * @return the cache device or NULL
+ */
+struct cache_device *cache_find(u32 id);
+
+/**
+ * Register a cache device
+ *
+ * cache_device->id must be initialized already and must not change during the life
+ * of the cache_device object.
+ *
+ * @param dev the cache device to register
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int cache_add(struct cache_device *dev);
+
+/**
+ * Flush the entire cache
+ *
+ * @param dev the cache to flush
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int cache_flush_all(struct cache_device *dev);
+
+#endif

include/sbi_utils/cache/fdt_cache.h

@@ -0,0 +1,34 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive Inc.
+ */
+
+#ifndef __FDT_CACHE_H__
+#define __FDT_CACHE_H__
+
+#include <sbi_utils/cache/cache.h>
+
+/**
+ * Register a cache device using information from the DT
+ *
+ * @param fdt devicetree blob
+ * @param noff offset of a node in the devicetree blob
+ * @param dev cache device to register for this devicetree node
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int fdt_cache_add(const void *fdt, int noff, struct cache_device *dev);
+
+/**
+ * Get the cache device referencd by the "next-level-cache" property of a DT node
+ *
+ * @param fdt devicetree blob
+ * @param noff offset of a node in the devicetree blob
+ * @param out_dev location to return the cache device
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int fdt_next_cache_get(const void *fdt, int noff, struct cache_device **out_dev);
+
+#endif

include/sbi_utils/cache/fdt_cmo_helper.h

@@ -0,0 +1,40 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive Inc.
+ */
+
+#ifndef __FDT_CMO_HELPER_H__
+#define __FDT_CMO_HELPER_H__
+
+#ifdef CONFIG_FDT_CACHE
+/**
+ * Flush the private first level cache of the current hart
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int fdt_cmo_private_flc_flush_all(void);
+
+/**
+ * Flush the last level cache of the current hart
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int fdt_cmo_llc_flush_all(void);
+
+/**
+ * Initialize the cache devices for each hart
+ *
+ * @param fdt devicetree blob
+ * @param cold_boot cold init or warm init
+ *
+ * @return 0 on success, or a negative error code on failure
+ */
+int fdt_cmo_init(bool cold_boot);
+
+#else
+
+static inline int fdt_cmo_init(bool cold_boot) { return 0; }
+
+#endif /* CONFIG_FDT_CACHE */
+#endif /* __FDT_CMO_HELPER_H__ */

include/sbi_utils/cppc/fdt_cppc.h

@@ -1,26 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2024 Ventana Micro Systems Inc.
- *
- * Authors:
- *   Anup Patel <apatel@ventanamicro.com>
- */
-
-#ifndef __FDT_CPPC_H__
-#define __FDT_CPPC_H__
-
-#include <sbi/sbi_types.h>
-#include <sbi_utils/fdt/fdt_driver.h>
-
-#ifdef CONFIG_FDT_CPPC
-
-void fdt_cppc_init(const void *fdt);
-
-#else
-
-static inline void fdt_cppc_init(const void *fdt) { }
-
-#endif
-
-#endif

include/sbi_utils/fdt/fdt_driver.h

@@ -18,6 +18,9 @@ struct fdt_driver {
 	bool experimental;
 };
 
+/* List of early FDT drivers generated at compile time */
+extern const struct fdt_driver *const fdt_early_drivers[];
+
 /**
  * Initialize a driver instance for a specific DT node
  *

include/sbi_utils/fdt/fdt_helper.h

@@ -34,13 +34,6 @@ struct platform_uart_data {
 	unsigned long reg_offset;
 };
 
-const struct fdt_match *fdt_match_node(const void *fdt, int nodeoff,
-				       const struct fdt_match *match_table);
-
-int fdt_find_match(const void *fdt, int startoff,
-		   const struct fdt_match *match_table,
-		   const struct fdt_match **out_match);
-
 int fdt_parse_phandle_with_args(const void *fdt, int nodeoff,
 				const char *prop, const char *cells_prop,
 				int index, struct fdt_phandle_args *out_args);
@@ -57,9 +50,11 @@ int fdt_parse_hart_id(const void *fdt, int cpu_offset, u32 *hartid);
 
 int fdt_parse_max_enabled_hart_id(const void *fdt, u32 *max_hartid);
 
+int fdt_parse_cbom_block_size(const void *fdt, int cpu_offset, unsigned long  *cbom_block_size);
+
 int fdt_parse_timebase_frequency(const void *fdt, unsigned long *freq);
 
-int fdt_parse_isa_extensions(const void *fdt, unsigned int hard_id,
+int fdt_parse_isa_extensions(const void *fdt, unsigned int hartid,
 			     unsigned long *extensions);
 
 int fdt_parse_gaisler_uart_node(const void *fdt, int nodeoffset,

include/sbi_utils/fdt/fdt_pmu.h

@@ -62,11 +62,6 @@ int fdt_pmu_setup(const void *fdt);
  */
 uint64_t fdt_pmu_get_select_value(uint32_t event_idx);
 
-/** The event index to selector value table instance */
-extern struct fdt_pmu_hw_event_select_map fdt_pmu_evt_select[];
-/** The number of valid entries in fdt_pmu_evt_select[] */
-extern uint32_t hw_event_count;
-
 #else
 
 static inline void fdt_pmu_fixup(void *fdt) { }

include/sbi_utils/hsm/fdt_hsm.h

@@ -1,26 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2024 Ventana Micro Systems Inc.
- *
- * Authors:
- *   Anup Patel <apatel@ventanamicro.com>
- */
-
-#ifndef __FDT_HSM_H__
-#define __FDT_HSM_H__
-
-#include <sbi/sbi_types.h>
-#include <sbi_utils/fdt/fdt_driver.h>
-
-#ifdef CONFIG_FDT_HSM
-
-void fdt_hsm_init(const void *fdt);
-
-#else
-
-static inline void fdt_hsm_init(const void *fdt) { }
-
-#endif
-
-#endif

include/sbi_utils/hsm/fdt_hsm_sifive_inst.h

@@ -0,0 +1,20 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive Inc.
+ */
+
+#ifndef __FDT_HSM_SIFIVE_INST_H__
+#define __FDT_HSM_SIFIVE_INST_H__
+
+static inline void sifive_cease(void)
+{
+	__asm__ __volatile__(".insn 0x30500073" ::: "memory");
+}
+
+static inline void sifive_cflush(void)
+{
+	__asm__ __volatile__(".insn 0xfc000073" ::: "memory");
+}
+
+#endif

include/sbi_utils/hsm/fdt_hsm_sifive_tmc0.h

@@ -0,0 +1,14 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 SiFive Inc.
+ */
+
+#ifndef __FDT_HSM_SIFIVE_TMC0_H__
+#define __FDT_HSM_SIFIVE_TMC0_H__
+
+int sifive_tmc0_set_wakemask_enareq(u32 hartid);
+void sifive_tmc0_set_wakemask_disreq(u32 hartid);
+bool sifive_tmc0_is_pg(u32 hartid);
+
+#endif

include/sbi_utils/ipi/fdt_ipi.h

@@ -1,26 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2020 Western Digital Corporation or its affiliates.
- *
- * Authors:
- *   Anup Patel <anup.patel@wdc.com>
- */
-
-#ifndef __FDT_IPI_H__
-#define __FDT_IPI_H__
-
-#include <sbi/sbi_types.h>
-#include <sbi_utils/fdt/fdt_driver.h>
-
-#ifdef CONFIG_FDT_IPI
-
-int fdt_ipi_init(void);
-
-#else
-
-static inline int fdt_ipi_init(void) { return 0; }
-
-#endif
-
-#endif

include/sbi_utils/irqchip/aplic.h

@@ -33,6 +33,7 @@ struct aplic_delegate_data {
 struct aplic_data {
 	/* Private members */
 	struct sbi_irqchip_device irqchip;
+	struct sbi_dlist node;
 	/* Public members */
 	unsigned long addr;
 	unsigned long size;
@@ -48,4 +49,6 @@ struct aplic_data {
 
 int aplic_cold_irqchip_init(struct aplic_data *aplic);
 
+void aplic_reinit_all(void);
+
 #endif

include/sbi_utils/irqchip/fdt_irqchip.h

@@ -11,14 +11,10 @@
 #define __FDT_IRQCHIP_H__
 
 #include <sbi/sbi_types.h>
+#include <sbi_utils/fdt/fdt_driver.h>
 
 #ifdef CONFIG_FDT_IRQCHIP
 
-struct fdt_irqchip {
-	const struct fdt_match *match_table;
-	int (*cold_init)(const void *fdt, int nodeoff, const struct fdt_match *match);
-};
-
 int fdt_irqchip_init(void);
 
 #else

include/sbi_utils/mailbox/rpmi_mailbox.h

@@ -11,6 +11,7 @@
 #define __RPMI_MAILBOX_H__
 
 #include <sbi/sbi_error.h>
+#include <sbi_utils/mailbox/mailbox.h>
 #include <sbi_utils/mailbox/rpmi_msgprot.h>
 
 #define rpmi_u32_count(__var)	(sizeof(__var) / sizeof(u32))

include/sbi_utils/mailbox/rpmi_msgprot.h

@@ -175,7 +175,7 @@ enum rpmi_error {
 	RPMI_ERR_VENDOR_START	= -128,
 };
 
-/** RPMI Message Arguments */
+/** RPMI Mailbox Message Arguments */
 struct rpmi_message_args {
 	u32 flags;
 #define RPMI_MSG_FLAGS_NO_TX		(1U << 0)
@@ -189,6 +189,20 @@ struct rpmi_message_args {
 	u32 rx_data_len;
 };
 
+/** RPMI Mailbox Channel Attribute IDs */
+enum rpmi_channel_attribute_id {
+	RPMI_CHANNEL_ATTR_PROTOCOL_VERSION = 0,
+	RPMI_CHANNEL_ATTR_MAX_DATA_LEN,
+	RPMI_CHANNEL_ATTR_P2A_DOORBELL_SYSMSI_INDEX,
+	RPMI_CHANNEL_ATTR_TX_TIMEOUT,
+	RPMI_CHANNEL_ATTR_RX_TIMEOUT,
+	RPMI_CHANNEL_ATTR_SERVICEGROUP_ID,
+	RPMI_CHANNEL_ATTR_SERVICEGROUP_VERSION,
+	RPMI_CHANNEL_ATTR_IMPL_ID,
+	RPMI_CHANNEL_ATTR_IMPL_VERSION,
+	RPMI_CHANNEL_ATTR_MAX,
+};
+
 /*
  * RPMI SERVICEGROUPS AND SERVICES
  */
@@ -197,11 +211,15 @@ struct rpmi_message_args {
 enum rpmi_servicegroup_id {
 	RPMI_SRVGRP_ID_MIN = 0,
 	RPMI_SRVGRP_BASE = 0x0001,
-	RPMI_SRVGRP_SYSTEM_RESET = 0x0002,
-	RPMI_SRVGRP_SYSTEM_SUSPEND = 0x0003,
-	RPMI_SRVGRP_HSM = 0x0004,
-	RPMI_SRVGRP_CPPC = 0x0005,
-	RPMI_SRVGRP_CLOCK = 0x0007,
+	RPMI_SRVGRP_SYSTEM_MSI = 0x0002,
+	RPMI_SRVGRP_SYSTEM_RESET = 0x0003,
+	RPMI_SRVGRP_SYSTEM_SUSPEND = 0x0004,
+	RPMI_SRVGRP_HSM = 0x0005,
+	RPMI_SRVGRP_CPPC = 0x0006,
+	RPMI_SRVGRP_VOLTAGE = 0x00007,
+	RPMI_SRVGRP_CLOCK = 0x0008,
+	RPMI_SRVGRP_DEVICE_POWER = 0x0009,
+	RPMI_SRVGRP_PERFORMANCE = 0x0000A,
 	RPMI_SRVGRP_ID_MAX_COUNT,
 
 	/* Reserved range for service groups */
@@ -232,12 +250,10 @@ enum rpmi_base_service_id {
 	RPMI_BASE_SRV_GET_PLATFORM_INFO = 0x05,
 	RPMI_BASE_SRV_PROBE_SERVICE_GROUP = 0x06,
 	RPMI_BASE_SRV_GET_ATTRIBUTES = 0x07,
-	RPMI_BASE_SRV_SET_MSI = 0x08,
 };
 
-#define RPMI_BASE_FLAGS_F0_PRIVILEGE		(1U << 2)
-#define RPMI_BASE_FLAGS_F0_EV_NOTIFY		(1U << 1)
-#define RPMI_BASE_FLAGS_F0_MSI_EN		(1U)
+#define RPMI_BASE_FLAGS_F0_PRIVILEGE		(1U << 1)
+#define RPMI_BASE_FLAGS_F0_EV_NOTIFY		(1U << 0)
 
 enum rpmi_base_context_priv_level {
 	RPMI_BASE_CONTEXT_PRIV_S_MODE,
@@ -258,6 +274,92 @@ struct rpmi_base_get_platform_info_resp {
 	char plat_info[];
 };
 
+/** RPMI System MSI ServiceGroup Service IDs */
+enum rpmi_sysmsi_service_id {
+	RPMI_SYSMSI_SRV_ENABLE_NOTIFICATION = 0x01,
+	RPMI_SYSMSI_SRV_GET_ATTRIBUTES = 0x2,
+	RPMI_SYSMSI_SRV_GET_MSI_ATTRIBUTES = 0x3,
+	RPMI_SYSMSI_SRV_SET_MSI_STATE = 0x4,
+	RPMI_SYSMSI_SRV_GET_MSI_STATE = 0x5,
+	RPMI_SYSMSI_SRV_SET_MSI_TARGET = 0x6,
+	RPMI_SYSMSI_SRV_GET_MSI_TARGET = 0x7,
+	RPMI_SYSMSI_SRV_ID_MAX_COUNT,
+};
+
+/** Response for system MSI service group attributes */
+struct rpmi_sysmsi_get_attributes_resp {
+	s32 status;
+	u32 sys_num_msi;
+	u32 flag0;
+	u32 flag1;
+};
+
+/** Request for system MSI attributes */
+struct rpmi_sysmsi_get_msi_attributes_req {
+	u32 sys_msi_index;
+};
+
+/** Response for system MSI attributes */
+struct rpmi_sysmsi_get_msi_attributes_resp {
+	s32 status;
+	u32 flag0;
+	u32 flag1;
+	u8 name[16];
+};
+
+#define RPMI_SYSMSI_MSI_ATTRIBUTES_FLAG0_PREF_PRIV	(1U << 0)
+
+/** Request for system MSI set state */
+struct rpmi_sysmsi_set_msi_state_req {
+	u32 sys_msi_index;
+	u32 sys_msi_state;
+};
+
+#define RPMI_SYSMSI_MSI_STATE_ENABLE			(1U << 0)
+#define RPMI_SYSMSI_MSI_STATE_PENDING			(1U << 1)
+
+/** Response for system MSI set state */
+struct rpmi_sysmsi_set_msi_state_resp {
+	s32 status;
+};
+
+/** Request for system MSI get state */
+struct rpmi_sysmsi_get_msi_state_req {
+	u32 sys_msi_index;
+};
+
+/** Response for system MSI get state */
+struct rpmi_sysmsi_get_msi_state_resp {
+	s32 status;
+	u32 sys_msi_state;
+};
+
+/** Request for system MSI set target */
+struct rpmi_sysmsi_set_msi_target_req {
+	u32 sys_msi_index;
+	u32 sys_msi_address_low;
+	u32 sys_msi_address_high;
+	u32 sys_msi_data;
+};
+
+/** Response for system MSI set target */
+struct rpmi_sysmsi_set_msi_target_resp {
+	s32 status;
+};
+
+/** Request for system MSI get target */
+struct rpmi_sysmsi_get_msi_target_req {
+	u32 sys_msi_index;
+};
+
+/** Response for system MSI get target */
+struct rpmi_sysmsi_get_msi_target_resp {
+	s32 status;
+	u32 sys_msi_address_low;
+	u32 sys_msi_address_high;
+	u32 sys_msi_data;
+};
+
 /** RPMI System Reset ServiceGroup Service IDs */
 enum rpmi_system_reset_service_id {
 	RPMI_SYSRST_SRV_ENABLE_NOTIFICATION = 0x01,
@@ -512,6 +614,86 @@ struct rpmi_cppc_hart_list_resp {
 	u32 hartid[(RPMI_MSG_DATA_SIZE(RPMI_SLOT_SIZE_MIN) - (sizeof(u32) * 3)) / sizeof(u32)];
 };
 
+/** RPMI Voltage ServiceGroup Service IDs */
+enum rpmi_voltage_service_id {
+	RPMI_VOLTAGE_SRV_ENABLE_NOTIFICATION = 0x01,
+	RPMI_VOLTAGE_SRV_GET_NUM_DOMAINS = 0x02,
+	RPMI_VOLTAGE_SRV_GET_ATTRIBUTES = 0x03,
+	RPMI_VOLTAGE_SRV_GET_SUPPORTED_LEVELS = 0x04,
+	RPMI_VOLTAGE_SRV_SET_CONFIG = 0x05,
+	RPMI_VOLTAGE_SRV_GET_CONFIG = 0x06,
+	RPMI_VOLTAGE_SRV_SET_LEVEL = 0x07,
+	RPMI_VOLTAGE_SRV_GET_LEVEL = 0x08,
+	RPMI_VOLTAGE_SRV_MAX_COUNT,
+};
+
+struct rpmi_voltage_get_num_domains_resp {
+       s32 status;
+       u32 num_domains;
+};
+
+struct rpmi_voltage_get_attributes_req {
+        u32 domain_id;
+};
+
+struct rpmi_voltage_get_attributes_resp {
+        s32 status;
+        u32 flags;
+        u32 num_levels;
+        u32 transition_latency;
+        u8 name[16];
+};
+
+struct rpmi_voltage_get_supported_rate_req {
+        u32 domain_id;
+        u32 index;
+};
+
+struct rpmi_voltage_get_supported_rate_resp {
+        s32 status;
+        u32 flags;
+        u32 remaining;
+        u32 returned;
+	u32 level[0];
+};
+
+struct rpmi_voltage_set_config_req {
+        u32 domain_id;
+#define RPMI_CLOCK_CONFIG_ENABLE                (1U << 0)
+        u32 config;
+};
+
+struct rpmi_voltage_set_config_resp {
+        s32 status;
+};
+
+struct rpmi_voltage_get_config_req {
+        u32 domain_id;
+};
+
+struct rpmi_voltage_get_config_resp {
+        s32 status;
+        u32 config;
+};
+
+struct rpmi_voltage_set_level_req {
+        u32 domain_id;
+        s32 level;
+};
+
+struct rpmi_voltage_set_level_resp {
+        s32 status;
+};
+
+struct rpmi_voltage_get_level_req {
+        u32 domain_id;
+};
+
+struct rpmi_voltage_get_level_resp {
+        s32 status;
+        s32 level;
+};
+
 /** RPMI Clock ServiceGroup Service IDs */
 enum rpmi_clock_service_id {
 	RPMI_CLOCK_SRV_ENABLE_NOTIFICATION = 0x01,
@@ -604,4 +786,165 @@ struct rpmi_clock_get_rate_resp {
 	u32 clock_rate_high;
 };
 
+/** RPMI Device Power ServiceGroup Service IDs */
+enum rpmi_dpwr_service_id {
+	RPMI_DPWR_SRV_ENABLE_NOTIFICATION = 0x01,
+	RPMI_DPWR_SRV_GET_NUM_DOMAINS = 0x02,
+	RPMI_DPWR_SRV_GET_ATTRIBUTES = 0x03,
+	RPMI_DPWR_SRV_SET_STATE = 0x04,
+	RPMI_DPWR_SRV_GET_STATE = 0x05,
+	RPMI_DPWR_SRV_MAX_COUNT,
+};
+
+struct rpmi_dpwr_get_num_domain_resp {
+	s32 status;
+	u32 num_domain;
+};
+
+struct rpmi_dpwr_get_attrs_req {
+	u32 domain_id;
+};
+
+struct rpmi_dpwr_get_attrs_resp {
+	s32 status;
+	u32 flags;
+	u32 transition_latency;
+	u8 name[16];
+};
+
+struct rpmi_dpwr_set_state_req {
+	u32 domain_id;
+	u32 state;
+};
+
+struct rpmi_dpwr_set_state_resp {
+	s32 status;
+};
+
+struct rpmi_dpwr_get_state_req {
+	u32 domain_id;
+};
+
+struct rpmi_dpwr_get_state_resp {
+	s32 status;
+	u32 state;
+};
+
+/** RPMI Performance ServiceGroup Service IDs */
+enum rpmi_performance_service_id {
+	RPMI_PERF_SRV_ENABLE_NOTIFICATION = 0x01,
+	RPMI_PERF_SRV_GET_NUM_DOMAINS = 0x02,
+	RPMI_PERF_SRV_GET_ATTRIBUTES = 0x03,
+	RPMI_PERF_SRV_GET_SUPPORTED_LEVELS = 0x04,
+	RPMI_PERF_SRV_GET_LEVEL = 0x05,
+	RPMI_PERF_SRV_SET_LEVEL = 0x06,
+	RPMI_PERF_SRV_GET_LIMIT = 0x07,
+	RPMI_PERF_SRV_SET_LIMIT = 0x08,
+	RPMI_PERF_SRV_GET_FAST_CHANNEL_REGION = 0x09,
+	RPMI_PERF_SRV_GET_FAST_CHANNEL_ATTRIBUTES = 0x0A,
+	RPMI_PERF_SRV_MAX_COUNT,
+};
+
+struct rpmi_perf_get_num_domain_resp {
+	s32 status;
+	u32 num_domains;
+};
+
+struct rpmi_perf_get_attrs_req {
+	u32 domain_id;
+};
+
+struct rpmi_perf_get_attrs_resp {
+	s32 status;
+	u32 flags;
+	u32 num_level;
+	u32 latency;
+	u8 name[16];
+};
+
+struct rpmi_perf_get_supported_level_req {
+	u32 domain_id;
+	u32 perf_level_index;
+};
+
+struct rpmi_perf_domain_level {
+	u32 level_index;
+	u32 opp_level;
+	u32 power_cost_uw;
+	u32 transition_latency_us;
+};
+
+struct rpmi_perf_get_supported_level_resp {
+	s32 status;
+	u32 reserve;
+	u32 remaining;
+	u32 returned;
+	struct rpmi_perf_domain_level level[0];
+};
+
+struct rpmi_perf_get_level_req {
+	u32 domain_id;
+};
+
+struct rpmi_perf_get_level_resp {
+	s32 status;
+	u32 level_index;
+};
+
+struct rpmi_perf_set_level_req {
+	u32 domain_id;
+	u32 level_index;
+};
+
+struct rpmi_perf_set_level_resp {
+	s32 status;
+};
+
+struct rpmi_perf_get_limit_req {
+	u32 domain_id;
+};
+
+struct rpmi_perf_get_limit_resp {
+	s32 status;
+	u32 level_index_max;
+	u32 level_index_min;
+};
+
+struct rpmi_perf_set_limit_req {
+	u32 domain_id;
+	u32 level_index_max;
+	u32 level_index_min;
+};
+
+struct rpmi_perf_set_limit_resp {
+	s32 status;
+};
+
+struct rpmi_perf_get_fast_chn_region_resp {
+	s32 status;
+	u32 region_phy_addr_low;
+	u32 region_phy_addr_high;
+	u32 region_size_low;
+	u32 region_size_high;
+};
+
+struct rpmi_perf_get_fast_chn_attr_req {
+	u32 domain_id;
+	u32 service_id;
+};
+
+struct rpmi_perf_get_fast_chn_attr_resp {
+	s32 status;
+	u32 flags;
+	u32 region_offset_low;
+	u32 region_offset_high;
+	u32 region_size;
+	u32 db_addr_low;
+	u32 db_addr_high;
+	u32 db_id_low;
+	u32 db_id_high;
+	u32 db_perserved_low;
+	u32 db_perserved_high;
+};
+
 #endif /* !__RPMI_MSGPROT_H__ */

include/sbi_utils/mpxy/fdt_mpxy.h

@@ -15,11 +15,11 @@
 
 #ifdef CONFIG_FDT_MPXY
 
-void fdt_mpxy_init(const void *fdt);
+int fdt_mpxy_init(const void *fdt);
 
 #else
 
-static inline void fdt_mpxy_init(const void *fdt) { }
+static inline int fdt_mpxy_init(const void *fdt) { return 0; }
 
 #endif
 

include/sbi_utils/mpxy/fdt_mpxy_rpmi_mbox.h

@@ -0,0 +1,85 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2024 Ventana Micro Systems Inc.
+ *
+ * Authors:
+ *   Anup Patel <apatel@ventanamicro.com>
+ */
+
+#ifndef __FDT_MPXY_RPMI_MBOX_H__
+#define __FDT_MPXY_RPMI_MBOX_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_mpxy.h>
+#include <sbi_utils/mailbox/fdt_mailbox.h>
+#include <sbi_utils/mailbox/rpmi_msgprot.h>
+#include <sbi_utils/mpxy/fdt_mpxy.h>
+
+/** Convert the mpxy attribute ID to attribute array index */
+#define attr_id2index(attr_id)	(attr_id - SBI_MPXY_ATTR_MSGPROTO_ATTR_START)
+
+enum mpxy_msgprot_rpmi_attr_id {
+	MPXY_MSGPROT_RPMI_ATTR_SERVICEGROUP_ID = SBI_MPXY_ATTR_MSGPROTO_ATTR_START,
+	MPXY_MSGPROT_RPMI_ATTR_SERVICEGROUP_VERSION,
+	MPXY_MSGPROT_RPMI_ATTR_IMPL_ID,
+	MPXY_MSGPROT_RPMI_ATTR_IMPL_VERSION,
+	MPXY_MSGPROT_RPMI_ATTR_MAX_ID
+};
+
+/**
+ * MPXY message protocol attributes for RPMI
+ * Order of attribute fields must follow the
+ * attribute IDs in `enum mpxy_msgprot_rpmi_attr_id`
+ */
+struct mpxy_rpmi_channel_attrs {
+	u32 servicegrp_id;
+	u32 servicegrp_ver;
+	u32 impl_id;
+	u32 impl_ver;
+};
+
+/** Make sure all attributes are packed for direct memcpy */
+#define assert_field_offset(field, attr_offset)				\
+	_Static_assert(							\
+		((offsetof(struct mpxy_rpmi_channel_attrs, field)) /	\
+		 sizeof(u32)) == (attr_offset - SBI_MPXY_ATTR_MSGPROTO_ATTR_START),\
+		"field " #field						\
+		" from struct mpxy_rpmi_channel_attrs invalid offset, expected " #attr_offset)
+
+assert_field_offset(servicegrp_id, MPXY_MSGPROT_RPMI_ATTR_SERVICEGROUP_ID);
+assert_field_offset(servicegrp_ver, MPXY_MSGPROT_RPMI_ATTR_SERVICEGROUP_VERSION);
+assert_field_offset(impl_id, MPXY_MSGPROT_RPMI_ATTR_IMPL_ID);
+assert_field_offset(impl_ver, MPXY_MSGPROT_RPMI_ATTR_IMPL_VERSION);
+
+/** MPXY RPMI service data for each service group */
+struct mpxy_rpmi_service_data {
+	u8 id;
+	u32 min_tx_len;
+	u32 max_tx_len;
+	u32 min_rx_len;
+	u32 max_rx_len;
+};
+
+/** MPXY RPMI mbox data for each service group */
+struct mpxy_rpmi_mbox_data {
+	u32 servicegrp_id;
+	u32 num_services;
+	struct mpxy_rpmi_service_data *service_data;
+
+	/** Transfer RPMI service group message */
+	int (*xfer_group)(void *context, struct mbox_chan *chan,
+			  struct mbox_xfer *xfer);
+
+	/** Setup RPMI service group context for MPXY */
+	int (*setup_group)(void **context, struct mbox_chan *chan,
+			   const struct mpxy_rpmi_mbox_data *data);
+
+	/** Cleanup RPMI service group context for MPXY */
+	void (*cleanup_group)(void *context);
+};
+
+/** Common probe function for MPXY RPMI drivers */
+int mpxy_rpmi_mbox_init(const void *fdt, int nodeoff, const struct fdt_match *match);
+
+#endif

include/sbi_utils/reset/fdt_reset.h

@@ -1,31 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2020 Western Digital Corporation or its affiliates.
- *
- * Authors:
- *   Anup Patel <anup.patel@wdc.com>
- */
-
-#ifndef __FDT_RESET_H__
-#define __FDT_RESET_H__
-
-#include <sbi/sbi_types.h>
-#include <sbi_utils/fdt/fdt_driver.h>
-
-#ifdef CONFIG_FDT_RESET
-
-/**
- * fdt_reset_init() - initialize reset drivers based on the device-tree
- *
- * This function shall be invoked in final init.
- */
-void fdt_reset_init(const void *fdt);
-
-#else
-
-static inline void fdt_reset_init(const void *fdt) { }
-
-#endif
-
-#endif

include/sbi_utils/serial/uart8250.h

@@ -12,7 +12,9 @@
 
 #include <sbi/sbi_types.h>
 
+#define UART_CAP_UUE	BIT(0)	/* Check UUE capability for XScale PXA UARTs */
+
 int uart8250_init(unsigned long base, u32 in_freq, u32 baudrate, u32 reg_shift,
-		  u32 reg_width, u32 reg_offset);
+		  u32 reg_width, u32 reg_offset, u32 caps);
 
 #endif

include/sbi_utils/suspend/fdt_suspend.h

@@ -1,26 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2024 Ventana Micro Systems Inc.
- *
- * Authors:
- *   Anup Patel <apatel@ventanamicro.com>
- */
-
-#ifndef __FDT_SUSPEND_H__
-#define __FDT_SUSPEND_H__
-
-#include <sbi/sbi_types.h>
-#include <sbi_utils/fdt/fdt_driver.h>
-
-#ifdef CONFIG_FDT_SUSPEND
-
-void fdt_suspend_init(const void *fdt);
-
-#else
-
-static inline void fdt_suspend_init(const void *fdt) { }
-
-#endif
-
-#endif

include/sbi_utils/timer/aclint_mtimer.h

@@ -42,6 +42,11 @@ struct aclint_mtimer_data {
 	void (*time_wr)(bool timecmp, u64 value, volatile u64 *addr);
 };
 
+struct aclint_mtimer_data *aclint_get_mtimer_data(void);
+
+void aclint_mtimer_update(struct aclint_mtimer_data *mt,
+			  struct aclint_mtimer_data *ref);
+
 void aclint_mtimer_sync(struct aclint_mtimer_data *mt);
 
 void aclint_mtimer_set_reference(struct aclint_mtimer_data *mt,

lib/sbi/objects.mk

@@ -79,6 +79,7 @@ libsbi-objs-y += sbi_heap.o
 libsbi-objs-y += sbi_math.o
 libsbi-objs-y += sbi_hfence.o
 libsbi-objs-y += sbi_hsm.o
+libsbi-objs-y += sbi_illegal_atomic.o
 libsbi-objs-y += sbi_illegal_insn.o
 libsbi-objs-y += sbi_init.o
 libsbi-objs-y += sbi_ipi.o

lib/sbi/riscv_asm.c

@@ -93,77 +93,91 @@ void misa_string(int xlen, char *out, unsigned int out_sz)
 
 unsigned long csr_read_num(int csr_num)
 {
-#define switchcase_csr_read(__csr_num, __val)		\
+#define switchcase_csr_read(__csr_num)			\
 	case __csr_num:					\
-		__val = csr_read(__csr_num);		\
-		break;
-#define switchcase_csr_read_2(__csr_num, __val)	\
-	switchcase_csr_read(__csr_num + 0, __val)	\
-	switchcase_csr_read(__csr_num + 1, __val)
-#define switchcase_csr_read_4(__csr_num, __val)	\
-	switchcase_csr_read_2(__csr_num + 0, __val)	\
-	switchcase_csr_read_2(__csr_num + 2, __val)
-#define switchcase_csr_read_8(__csr_num, __val)	\
-	switchcase_csr_read_4(__csr_num + 0, __val)	\
-	switchcase_csr_read_4(__csr_num + 4, __val)
-#define switchcase_csr_read_16(__csr_num, __val)	\
-	switchcase_csr_read_8(__csr_num + 0, __val)	\
-	switchcase_csr_read_8(__csr_num + 8, __val)
-#define switchcase_csr_read_32(__csr_num, __val)	\
-	switchcase_csr_read_16(__csr_num + 0, __val)	\
-	switchcase_csr_read_16(__csr_num + 16, __val)
-#define switchcase_csr_read_64(__csr_num, __val)	\
-	switchcase_csr_read_32(__csr_num + 0, __val)	\
-	switchcase_csr_read_32(__csr_num + 32, __val)
-
-	unsigned long ret = 0;
+		return csr_read(__csr_num);
+#define switchcase_csr_read_2(__csr_num)		\
+	switchcase_csr_read(__csr_num + 0)		\
+	switchcase_csr_read(__csr_num + 1)
+#define switchcase_csr_read_4(__csr_num)		\
+	switchcase_csr_read_2(__csr_num + 0)		\
+	switchcase_csr_read_2(__csr_num + 2)
+#define switchcase_csr_read_8(__csr_num)		\
+	switchcase_csr_read_4(__csr_num + 0)		\
+	switchcase_csr_read_4(__csr_num + 4)
+#define switchcase_csr_read_16(__csr_num)		\
+	switchcase_csr_read_8(__csr_num + 0)		\
+	switchcase_csr_read_8(__csr_num + 8)
+#define switchcase_csr_read_32(__csr_num)		\
+	switchcase_csr_read_16(__csr_num + 0)		\
+	switchcase_csr_read_16(__csr_num + 16)
+#define switchcase_csr_read_64(__csr_num)		\
+	switchcase_csr_read_32(__csr_num + 0)		\
+	switchcase_csr_read_32(__csr_num + 32)
+#define switchcase_csr_read_128(__csr_num)		\
+	switchcase_csr_read_64(__csr_num + 0)		\
+	switchcase_csr_read_64(__csr_num + 64)
+#define switchcase_csr_read_256(__csr_num)		\
+	switchcase_csr_read_128(__csr_num + 0)		\
+	switchcase_csr_read_128(__csr_num + 128)
 
 	switch (csr_num) {
-	switchcase_csr_read_16(CSR_PMPCFG0, ret)
-	switchcase_csr_read_64(CSR_PMPADDR0, ret)
-	switchcase_csr_read(CSR_MCYCLE, ret)
-	switchcase_csr_read(CSR_MINSTRET, ret)
-	switchcase_csr_read(CSR_MHPMCOUNTER3, ret)
-	switchcase_csr_read_4(CSR_MHPMCOUNTER4, ret)
-	switchcase_csr_read_8(CSR_MHPMCOUNTER8, ret)
-	switchcase_csr_read_16(CSR_MHPMCOUNTER16, ret)
-	switchcase_csr_read(CSR_MCOUNTINHIBIT, ret)
-	switchcase_csr_read(CSR_MCYCLECFG, ret)
-	switchcase_csr_read(CSR_MINSTRETCFG, ret)
-	switchcase_csr_read(CSR_MHPMEVENT3, ret)
-	switchcase_csr_read_4(CSR_MHPMEVENT4, ret)
-	switchcase_csr_read_8(CSR_MHPMEVENT8, ret)
-	switchcase_csr_read_16(CSR_MHPMEVENT16, ret)
+	switchcase_csr_read_16(CSR_PMPCFG0)
+	switchcase_csr_read_64(CSR_PMPADDR0)
+	switchcase_csr_read(CSR_MCYCLE)
+	switchcase_csr_read(CSR_MINSTRET)
+	switchcase_csr_read(CSR_MHPMCOUNTER3)
+	switchcase_csr_read_4(CSR_MHPMCOUNTER4)
+	switchcase_csr_read_8(CSR_MHPMCOUNTER8)
+	switchcase_csr_read_16(CSR_MHPMCOUNTER16)
+	switchcase_csr_read(CSR_MCOUNTINHIBIT)
+	switchcase_csr_read(CSR_MCYCLECFG)
+	switchcase_csr_read(CSR_MINSTRETCFG)
+	switchcase_csr_read(CSR_MHPMEVENT3)
+	switchcase_csr_read_4(CSR_MHPMEVENT4)
+	switchcase_csr_read_8(CSR_MHPMEVENT8)
+	switchcase_csr_read_16(CSR_MHPMEVENT16)
 #if __riscv_xlen == 32
-	switchcase_csr_read(CSR_MCYCLEH, ret)
-	switchcase_csr_read(CSR_MINSTRETH, ret)
-	switchcase_csr_read(CSR_MHPMCOUNTER3H, ret)
-	switchcase_csr_read_4(CSR_MHPMCOUNTER4H, ret)
-	switchcase_csr_read_8(CSR_MHPMCOUNTER8H, ret)
-	switchcase_csr_read_16(CSR_MHPMCOUNTER16H, ret)
+	switchcase_csr_read(CSR_MCYCLEH)
+	switchcase_csr_read(CSR_MINSTRETH)
+	switchcase_csr_read(CSR_MHPMCOUNTER3H)
+	switchcase_csr_read_4(CSR_MHPMCOUNTER4H)
+	switchcase_csr_read_8(CSR_MHPMCOUNTER8H)
+	switchcase_csr_read_16(CSR_MHPMCOUNTER16H)
 	/**
 	 * The CSR range M[CYCLE, INSTRET]CFGH are available only if smcntrpmf
 	 * extension is present. The caller must ensure that.
 	 */
-	switchcase_csr_read(CSR_MCYCLECFGH, ret)
-	switchcase_csr_read(CSR_MINSTRETCFGH, ret)
+	switchcase_csr_read(CSR_MCYCLECFGH)
+	switchcase_csr_read(CSR_MINSTRETCFGH)
 	/**
 	 * The CSR range MHPMEVENT[3-16]H are available only if sscofpmf
 	 * extension is present. The caller must ensure that.
 	 */
-	switchcase_csr_read(CSR_MHPMEVENT3H, ret)
-	switchcase_csr_read_4(CSR_MHPMEVENT4H, ret)
-	switchcase_csr_read_8(CSR_MHPMEVENT8H, ret)
-	switchcase_csr_read_16(CSR_MHPMEVENT16H, ret)
+	switchcase_csr_read(CSR_MHPMEVENT3H)
+	switchcase_csr_read_4(CSR_MHPMEVENT4H)
+	switchcase_csr_read_8(CSR_MHPMEVENT8H)
+	switchcase_csr_read_16(CSR_MHPMEVENT16H)
 #endif
+	switchcase_csr_read_256(CSR_CUSTOM0_U_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM1_U_RO_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM2_S_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM3_S_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM4_S_RO_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM5_HS_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM6_HS_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM7_HS_RO_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM8_M_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM9_M_RW_BASE)
+	switchcase_csr_read_64(CSR_CUSTOM10_M_RO_BASE)
 
 	default:
 		sbi_panic("%s: Unknown CSR %#x", __func__, csr_num);
-		break;
+		return 0;
 	}
 
-	return ret;
-
+#undef switchcase_csr_read_256
+#undef switchcase_csr_read_128
 #undef switchcase_csr_read_64
 #undef switchcase_csr_read_32
 #undef switchcase_csr_read_16
@@ -197,6 +211,12 @@ void csr_write_num(int csr_num, unsigned long val)
 #define switchcase_csr_write_64(__csr_num, __val)	\
 	switchcase_csr_write_32(__csr_num + 0, __val)	\
 	switchcase_csr_write_32(__csr_num + 32, __val)
+#define switchcase_csr_write_128(__csr_num, __val)	\
+	switchcase_csr_write_64(__csr_num + 0, __val)	\
+	switchcase_csr_write_64(__csr_num + 64, __val)
+#define switchcase_csr_write_256(__csr_num, __val)	\
+	switchcase_csr_write_128(__csr_num + 0, __val)	\
+	switchcase_csr_write_128(__csr_num + 128, __val)
 
 	switch (csr_num) {
 	switchcase_csr_write_16(CSR_PMPCFG0, val)
@@ -228,12 +248,21 @@ void csr_write_num(int csr_num, unsigned long val)
 	switchcase_csr_write_4(CSR_MHPMEVENT4, val)
 	switchcase_csr_write_8(CSR_MHPMEVENT8, val)
 	switchcase_csr_write_16(CSR_MHPMEVENT16, val)
+	switchcase_csr_write_256(CSR_CUSTOM0_U_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM2_S_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM3_S_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM5_HS_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM6_HS_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM8_M_RW_BASE, val)
+	switchcase_csr_write_64(CSR_CUSTOM9_M_RW_BASE, val)
 
 	default:
 		sbi_panic("%s: Unknown CSR %#x", __func__, csr_num);
 		break;
 	}
 
+#undef switchcase_csr_write_256
+#undef switchcase_csr_write_128
 #undef switchcase_csr_write_64
 #undef switchcase_csr_write_32
 #undef switchcase_csr_write_16

lib/sbi/riscv_atomic.c

@@ -12,7 +12,7 @@
 #include <sbi/riscv_atomic.h>
 #include <sbi/riscv_barrier.h>
 
-#ifndef __riscv_atomic
+#if !defined(__riscv_atomic) && !defined(__riscv_zalrsc)
 #error "opensbi strongly relies on the A extension of RISC-V"
 #endif
 
@@ -31,6 +31,7 @@ void atomic_write(atomic_t *atom, long value)
 
 long atomic_add_return(atomic_t *atom, long value)
 {
+#ifdef __riscv_atomic
 	long ret;
 #if __SIZEOF_LONG__ == 4
 	__asm__ __volatile__("	amoadd.w.aqrl  %1, %2, %0"
@@ -43,6 +44,29 @@ long atomic_add_return(atomic_t *atom, long value)
 			     : "r"(value)
 			     : "memory");
 #endif
+#elif __riscv_zalrsc
+	long ret, temp;
+#if __SIZEOF_LONG__ == 4
+	__asm__ __volatile__("1:lr.w.aqrl	%1,%0\n"
+			     "  addw	%2,%1,%3\n"
+			     "  sc.w.aqrl	%2,%2,%0\n"
+			     "  bnez	%2,1b"
+			     : "+A"(atom->counter), "=&r"(ret), "=&r"(temp)
+			     : "r"(value)
+			     : "memory");
+#elif __SIZEOF_LONG__ == 8
+	__asm__ __volatile__("1:lr.d.aqrl	%1,%0\n"
+			     "  add	%2,%1,%3\n"
+			     "  sc.d.aqrl	%2,%2,%0\n"
+			     "  bnez	%2,1b"
+			     : "+A"(atom->counter), "=&r"(ret), "=&r"(temp)
+			     : "r"(value)
+			     : "memory");
+#endif
+#else
+#error "need a or zalrsc"
+#endif
+
 	return ret + value;
 }
 
@@ -51,6 +75,7 @@ long atomic_sub_return(atomic_t *atom, long value)
 	return atomic_add_return(atom, -value);
 }
 
+#ifdef __riscv_atomic
 #define __axchg(ptr, new, size)							\
 	({									\
 		__typeof__(ptr) __ptr = (ptr);					\
@@ -76,6 +101,39 @@ long atomic_sub_return(atomic_t *atom, long value)
 		}								\
 		__ret;								\
 	})
+#elif __riscv_zalrsc
+#define __axchg(ptr, new, size)							\
+	({									\
+		__typeof__(ptr) __ptr = (ptr);					\
+		__typeof__(new) __new = (new);					\
+		__typeof__(*(ptr)) __ret, __temp;					\
+		switch (size) {							\
+		case 4:								\
+			__asm__ __volatile__ (					\
+				"1:	lr.w.aqrl %0, %1\n"			\
+				"	sc.w.aqrl %2, %3, %1\n"			\
+				"	bnez	  %2, 1b\n"			\
+				: "=&r" (__ret), "+A" (*__ptr), "=&r" (__temp)	\
+				: "r" (__new)					\
+				: "memory");					\
+			break;							\
+		case 8:								\
+			__asm__ __volatile__ (					\
+				"1:	lr.d.aqrl %0, %1\n"			\
+				"	sc.d.aqrl %2, %3, %1\n"			\
+				"	bnez	  %2, 1b\n"			\
+				: "=&r" (__ret), "+A" (*__ptr), "=&r" (__temp)	\
+				: "r" (__new)					\
+				: "memory");					\
+			break;							\
+		default:							\
+			break;							\
+		}								\
+		__ret;								\
+	})
+#else
+#error "need a or zalrsc"
+#endif
 
 #define axchg(ptr, x)								\
 	({									\

lib/sbi/riscv_locks.c

@@ -53,7 +53,16 @@ void spin_lock(spinlock_t *lock)
 
 	__asm__ __volatile__(
 		/* Atomically increment the next ticket. */
+#ifdef __riscv_atomic
 		"	amoadd.w.aqrl	%0, %4, %3\n"
+#elif __riscv_zalrsc
+		"3:	lr.w.aqrl	%0, %3\n"
+		"	addw	%1, %0, %4\n"
+		"	sc.w.aqrl	%1, %1, %3\n"
+		"	bnez	%1, 3b\n"
+#else
+#error "need a or zalrsc"
+#endif
 
 		/* Did we get the lock? */
 		"	srli	%1, %0, %6\n"

lib/sbi/sbi_dbtr.c

@@ -336,6 +336,19 @@ static void dbtr_trigger_setup(struct sbi_dbtr_trigger *trig,
 		if (__test_bit(RV_DBTR_BIT(MC6, VS), &tdata1))
 			__set_bit(RV_DBTR_BIT(TS, VS), &trig->state);
 		break;
+	case RISCV_DBTR_TRIG_ICOUNT:
+		if (__test_bit(RV_DBTR_BIT(ICOUNT, U), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, U), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(ICOUNT, S), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, S), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(ICOUNT, VU), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, VU), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(ICOUNT, VS), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, VS), &trig->state);
+		break;
 	default:
 		sbi_dprintf("%s: Unknown type (tdata1: 0x%lx Type: %ld)\n",
 			    __func__, tdata1, TDATA1_GET_TYPE(tdata1));
@@ -379,6 +392,16 @@ static void dbtr_trigger_enable(struct sbi_dbtr_trigger *trig)
 		update_bit(state & RV_DBTR_BIT_MASK(TS, S),
 			   RV_DBTR_BIT(MC6, S), &trig->tdata1);
 		break;
+	case RISCV_DBTR_TRIG_ICOUNT:
+		update_bit(state & RV_DBTR_BIT_MASK(TS, VU),
+			   RV_DBTR_BIT(ICOUNT, VU), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, VS),
+			   RV_DBTR_BIT(ICOUNT, VS), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, U),
+			   RV_DBTR_BIT(ICOUNT, U), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, S),
+			   RV_DBTR_BIT(ICOUNT, S), &trig->tdata1);
+		break;
 	default:
 		break;
 	}
@@ -418,6 +441,12 @@ static void dbtr_trigger_disable(struct sbi_dbtr_trigger *trig)
 		__clear_bit(RV_DBTR_BIT(MC6, U), &trig->tdata1);
 		__clear_bit(RV_DBTR_BIT(MC6, S), &trig->tdata1);
 		break;
+	case RISCV_DBTR_TRIG_ICOUNT:
+		__clear_bit(RV_DBTR_BIT(ICOUNT, VU), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(ICOUNT, VS), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(ICOUNT, U), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(ICOUNT, S), &trig->tdata1);
+		break;
 	default:
 		break;
 	}
@@ -441,6 +470,7 @@ static int dbtr_trigger_supported(unsigned long type)
 	switch (type) {
 	case RISCV_DBTR_TRIG_MCONTROL:
 	case RISCV_DBTR_TRIG_MCONTROL6:
+	case RISCV_DBTR_TRIG_ICOUNT:
 		return 1;
 	default:
 		break;
@@ -462,6 +492,11 @@ static int dbtr_trigger_valid(unsigned long type, unsigned long tdata)
 		    !(tdata & RV_DBTR_BIT_MASK(MC6, M)))
 			return 1;
 		break;
+	case RISCV_DBTR_TRIG_ICOUNT:
+		if (!(tdata & RV_DBTR_BIT_MASK(ICOUNT, DMODE)) &&
+		    !(tdata & RV_DBTR_BIT_MASK(ICOUNT, M)))
+			return 1;
+		break;
 	default:
 		break;
 	}
@@ -506,7 +541,7 @@ int sbi_dbtr_read_trig(unsigned long smode,
 {
 	struct sbi_dbtr_data_msg *xmit;
 	struct sbi_dbtr_trigger *trig;
-	struct sbi_dbtr_shmem_entry *entry;
+	union sbi_dbtr_shmem_entry *entry;
 	void *shmem_base = NULL;
 	struct sbi_dbtr_hart_triggers_state *hs = NULL;
 
@@ -523,16 +558,21 @@ int sbi_dbtr_read_trig(unsigned long smode,
 
 	shmem_base = hart_shmem_base(hs);
 
+	sbi_hart_map_saddr((unsigned long)shmem_base,
+			   trig_count * sizeof(*entry));
 	for_each_trig_entry(shmem_base, trig_count, typeof(*entry), entry) {
-		sbi_hart_map_saddr((unsigned long)entry, sizeof(*entry));
 		xmit = &entry->data;
 		trig = INDEX_TO_TRIGGER((_idx + trig_idx_base));
+		csr_write(CSR_TSELECT, trig->index);
+		trig->tdata1 = csr_read(CSR_TDATA1);
+		trig->tdata2 = csr_read(CSR_TDATA2);
+		trig->tdata3 = csr_read(CSR_TDATA3);
 		xmit->tstate = cpu_to_lle(trig->state);
 		xmit->tdata1 = cpu_to_lle(trig->tdata1);
 		xmit->tdata2 = cpu_to_lle(trig->tdata2);
 		xmit->tdata3 = cpu_to_lle(trig->tdata3);
-		sbi_hart_unmap_saddr();
 	}
+	sbi_hart_unmap_saddr();
 
 	return SBI_SUCCESS;
 }
@@ -541,7 +581,7 @@ int sbi_dbtr_install_trig(unsigned long smode,
 			  unsigned long trig_count, unsigned long *out)
 {
 	void *shmem_base = NULL;
-	struct sbi_dbtr_shmem_entry *entry;
+	union sbi_dbtr_shmem_entry *entry;
 	struct sbi_dbtr_data_msg *recv;
 	struct sbi_dbtr_id_msg *xmit;
 	unsigned long ctrl;
@@ -556,10 +596,11 @@ int sbi_dbtr_install_trig(unsigned long smode,
 		return SBI_ERR_NO_SHMEM;
 
 	shmem_base = hart_shmem_base(hs);
+	sbi_hart_map_saddr((unsigned long)shmem_base,
+			   trig_count * sizeof(*entry));
 
 	/* Check requested triggers configuration */
 	for_each_trig_entry(shmem_base, trig_count, typeof(*entry), entry) {
-		sbi_hart_map_saddr((unsigned long)entry, sizeof(*entry));
 		recv = (struct sbi_dbtr_data_msg *)(&entry->data);
 		ctrl = recv->tdata1;
 
@@ -574,11 +615,11 @@ int sbi_dbtr_install_trig(unsigned long smode,
 			sbi_hart_unmap_saddr();
 			return SBI_ERR_FAILED;
 		}
-		sbi_hart_unmap_saddr();
 	}
 
 	if (hs->available_trigs < trig_count) {
 		*out = hs->available_trigs;
+		sbi_hart_unmap_saddr();
 		return SBI_ERR_FAILED;
 	}
 
@@ -590,16 +631,15 @@ int sbi_dbtr_install_trig(unsigned long smode,
 		 */
 		trig = sbi_alloc_trigger();
 
-		sbi_hart_map_saddr((unsigned long)entry, sizeof(*entry));
-
 		recv = (struct sbi_dbtr_data_msg *)(&entry->data);
 		xmit = (struct sbi_dbtr_id_msg *)(&entry->id);
 
 		dbtr_trigger_setup(trig,  recv);
 		dbtr_trigger_enable(trig);
 		xmit->idx = cpu_to_lle(trig->index);
-		sbi_hart_unmap_saddr();
+
 	}
+	sbi_hart_unmap_saddr();
 
 	return SBI_SUCCESS;
 }
@@ -651,15 +691,11 @@ int sbi_dbtr_enable_trig(unsigned long trig_idx_base,
 }
 
 int sbi_dbtr_update_trig(unsigned long smode,
-			 unsigned long trig_idx_base,
-			 unsigned long trig_idx_mask)
+			 unsigned long trig_count)
 {
-	unsigned long trig_mask = trig_idx_mask << trig_idx_base;
-	unsigned long idx = trig_idx_base;
-	struct sbi_dbtr_data_msg *recv;
-	unsigned long uidx = 0;
+	unsigned long trig_idx;
 	struct sbi_dbtr_trigger *trig;
-	struct sbi_dbtr_shmem_entry *entry;
+	union sbi_dbtr_shmem_entry *entry;
 	void *shmem_base = NULL;
 	struct sbi_dbtr_hart_triggers_state *hs = NULL;
 
@@ -672,18 +708,28 @@ int sbi_dbtr_update_trig(unsigned long smode,
 
 	shmem_base = hart_shmem_base(hs);
 
-	for_each_set_bit_from(idx, &trig_mask, hs->total_trigs) {
-		trig = INDEX_TO_TRIGGER(idx);
+	if (trig_count >= hs->total_trigs)
+		return SBI_ERR_BAD_RANGE;
 
-		if (!(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+	for_each_trig_entry(shmem_base, trig_count, typeof(*entry), entry) {
+		sbi_hart_map_saddr((unsigned long)entry, sizeof(*entry));
+		trig_idx = entry->id.idx;
+
+		if (trig_idx >= hs->total_trigs) {
+			sbi_hart_unmap_saddr();
 			return SBI_ERR_INVALID_PARAM;
+		}
 
-		entry = (shmem_base + uidx * sizeof(*entry));
-		recv = &entry->data;
+		trig = INDEX_TO_TRIGGER(trig_idx);
 
-		trig->tdata2 = lle_to_cpu(recv->tdata2);
+		if (!(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED))) {
+			sbi_hart_unmap_saddr();
+			return SBI_ERR_FAILED;
+		}
+
+		dbtr_trigger_setup(trig, &entry->data);
+		sbi_hart_unmap_saddr();
 		dbtr_trigger_enable(trig);
-		uidx++;
 	}
 
 	return SBI_SUCCESS;

lib/sbi/sbi_domain.c

@@ -25,7 +25,6 @@ static u32 domain_count = 0;
 static bool domain_finalized = false;
 
 #define ROOT_REGION_MAX	32
-static u32 root_memregs_count = 0;
 
 struct sbi_domain root = {
 	.name = "root",
@@ -122,6 +121,80 @@ void sbi_domain_memregion_init(unsigned long addr,
 	}
 }
 
+unsigned int sbi_domain_get_smepmp_flags(struct sbi_domain_memregion *reg)
+{
+	unsigned int pmp_flags = 0;
+	unsigned long rstart, rend;
+
+	if ((reg->flags & SBI_DOMAIN_MEMREGION_ACCESS_MASK) == 0) {
+		/*
+		 * Region is inaccessible in all privilege modes.
+		 *
+		 * SmePMP allows two encodings for an inaccessible region:
+		 *   - pmpcfg.LRWX = 0000 (Inaccessible region)
+		 *   - pmpcfg.LRWX = 1000 (Locked inaccessible region)
+		 * We use the first encoding here.
+		 */
+		return 0;
+	} else if (SBI_DOMAIN_MEMREGION_IS_SHARED(reg->flags)) {
+		/* Read only for both M and SU modes */
+		if (SBI_DOMAIN_MEMREGION_IS_SUR_MR(reg->flags))
+			pmp_flags = (PMP_L | PMP_R | PMP_W | PMP_X);
+
+		/* Execute for SU but Read/Execute for M mode */
+		else if (SBI_DOMAIN_MEMREGION_IS_SUX_MRX(reg->flags))
+			/* locked region */
+			pmp_flags = (PMP_L | PMP_W | PMP_X);
+
+		/* Execute only for both M and SU modes */
+		else if (SBI_DOMAIN_MEMREGION_IS_SUX_MX(reg->flags))
+			pmp_flags = (PMP_L | PMP_W);
+
+		/* Read/Write for both M and SU modes */
+		else if (SBI_DOMAIN_MEMREGION_IS_SURW_MRW(reg->flags))
+			pmp_flags = (PMP_W | PMP_X);
+
+		/* Read only for SU mode but Read/Write for M mode */
+		else if (SBI_DOMAIN_MEMREGION_IS_SUR_MRW(reg->flags))
+			pmp_flags = (PMP_W);
+	} else if (SBI_DOMAIN_MEMREGION_M_ONLY_ACCESS(reg->flags)) {
+		/*
+		 * When smepmp is supported and used, M region cannot have RWX
+		 * permissions on any region.
+		 */
+		if ((reg->flags & SBI_DOMAIN_MEMREGION_M_ACCESS_MASK)
+		    == SBI_DOMAIN_MEMREGION_M_RWX) {
+			sbi_printf("%s: M-mode only regions cannot have"
+				   "RWX permissions\n", __func__);
+			return 0;
+		}
+
+		/* M-mode only access regions are always locked */
+		pmp_flags |= PMP_L;
+
+		if (reg->flags & SBI_DOMAIN_MEMREGION_M_READABLE)
+			pmp_flags |= PMP_R;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_M_WRITABLE)
+			pmp_flags |= PMP_W;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_M_EXECUTABLE)
+			pmp_flags |= PMP_X;
+	} else if (SBI_DOMAIN_MEMREGION_SU_ONLY_ACCESS(reg->flags)) {
+		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_READABLE)
+			pmp_flags |= PMP_R;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_WRITABLE)
+			pmp_flags |= PMP_W;
+		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_EXECUTABLE)
+			pmp_flags |= PMP_X;
+	} else {
+		rstart = reg->base;
+		rend = (reg->order < __riscv_xlen) ? rstart + ((1UL << reg->order) - 1) : -1UL;
+		sbi_printf("%s: Unsupported Smepmp permissions on region 0x%"PRILX"-0x%"PRILX"\n",
+			   __func__, rstart, rend);
+	}
+
+	return pmp_flags;
+}
+
 bool sbi_domain_check_addr(const struct sbi_domain *dom,
 			   unsigned long addr, unsigned long mode,
 			   unsigned long access_flags)
@@ -162,7 +235,11 @@ bool sbi_domain_check_addr(const struct sbi_domain *dom,
 			rstart + ((1UL << reg->order) - 1) : -1UL;
 		if (rstart <= addr && addr <= rend) {
 			rmmio = (rflags & SBI_DOMAIN_MEMREGION_MMIO) ? true : false;
-			if (mmio != rmmio)
+			/*
+			 * MMIO devices may appear in regions without the flag set (such as the
+			 * default region), but MMIO device regions should not be used as memory.
+			 */
+			if (!mmio && rmmio)
 				return false;
 			return ((rrwx & rwx) == rwx) ? true : false;
 		}
@@ -218,6 +295,19 @@ static bool is_region_compatible(const struct sbi_domain_memregion *regA,
 static bool is_region_before(const struct sbi_domain_memregion *regA,
 			     const struct sbi_domain_memregion *regB)
 {
+	/*
+	 * Enforce firmware region ordering for memory access
+	 * under SmePMP.
+	 * Place firmware regions first to ensure consistent
+	 * PMP entries during domain context switches.
+	 */
+	if (SBI_DOMAIN_MEMREGION_IS_FIRMWARE(regA->flags) &&
+	   !SBI_DOMAIN_MEMREGION_IS_FIRMWARE(regB->flags))
+		return true;
+	if (!SBI_DOMAIN_MEMREGION_IS_FIRMWARE(regA->flags) &&
+	    SBI_DOMAIN_MEMREGION_IS_FIRMWARE(regB->flags))
+		return false;
+
 	if (regA->order < regB->order)
 		return true;
 
@@ -281,6 +371,17 @@ static void clear_region(struct sbi_domain_memregion* reg)
 	sbi_memset(reg, 0x0, sizeof(*reg));
 }
 
+static int sbi_domain_used_memregions(const struct sbi_domain *dom)
+{
+	int count = 0;
+	struct sbi_domain_memregion *reg;
+
+	sbi_domain_for_each_memregion(dom, reg)
+		count++;
+
+	return count;
+}
+
 static int sanitize_domain(struct sbi_domain *dom)
 {
 	u32 i, j, count;
@@ -319,9 +420,7 @@ static int sanitize_domain(struct sbi_domain *dom)
 	}
 
 	/* Count memory regions */
-	count = 0;
-	sbi_domain_for_each_memregion(dom, reg)
-		count++;
+	count = sbi_domain_used_memregions(dom);
 
 	/* Check presence of firmware regions */
 	if (!dom->fw_region_inited) {
@@ -344,7 +443,7 @@ static int sanitize_domain(struct sbi_domain *dom)
 	}
 
 	/* Remove covered regions */
-	while(i < (count - 1)) {
+	for (i = 0; i < (count - 1);) {
 		is_covered = false;
 		reg = &dom->regions[i];
 
@@ -464,6 +563,8 @@ void sbi_domain_dump(const struct sbi_domain *dom, const char *suffix)
 		sbi_printf("M: ");
 		if (reg->flags & SBI_DOMAIN_MEMREGION_MMIO)
 			sbi_printf("%cI", (k++) ? ',' : '(');
+		if (reg->flags & SBI_DOMAIN_MEMREGION_FW)
+			sbi_printf("%cF", (k++) ? ',' : '(');
 		if (reg->flags & SBI_DOMAIN_MEMREGION_M_READABLE)
 			sbi_printf("%cR", (k++) ? ',' : '(');
 		if (reg->flags & SBI_DOMAIN_MEMREGION_M_WRITABLE)
@@ -603,6 +704,7 @@ static int root_add_memregion(const struct sbi_domain_memregion *reg)
 	int rc;
 	bool reg_merged;
 	struct sbi_domain_memregion *nreg, *nreg1, *nreg2;
+	int root_memregs_count = sbi_domain_used_memregions(&root);
 
 	/* Sanity checks */
 	if (!reg || domain_finalized || !root.regions ||
@@ -685,20 +787,15 @@ int sbi_domain_root_add_memrange(unsigned long addr, unsigned long size,
 	return 0;
 }
 
-int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
+int sbi_domain_startup(struct sbi_scratch *scratch, u32 cold_hartid)
 {
 	int rc;
 	u32 dhart;
 	struct sbi_domain *dom;
-	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
-	/* Initialize and populate domains for the platform */
-	rc = sbi_platform_domains_init(plat);
-	if (rc) {
-		sbi_printf("%s: platform domains_init() failed (error %d)\n",
-			   __func__, rc);
-		return rc;
-	}
+	/* Sanity checks */
+	if (!domain_finalized)
+		return SBI_EINVAL;
 
 	/* Startup boot HART of domains */
 	sbi_domain_for_each(dom) {
@@ -744,6 +841,26 @@ int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
 		}
 	}
 
+	return 0;
+}
+
+int sbi_domain_finalize(struct sbi_scratch *scratch)
+{
+	int rc;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	/* Sanity checks */
+	if (domain_finalized)
+		return SBI_EINVAL;
+
+	/* Initialize and populate domains for the platform */
+	rc = sbi_platform_domains_init(plat);
+	if (rc) {
+		sbi_printf("%s: platform domains_init() failed (error %d)\n",
+			   __func__, rc);
+		return rc;
+	}
+
 	/*
 	 * Set the finalized flag so that the root domain
 	 * regions can't be changed.
@@ -755,11 +872,10 @@ int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
 
 int sbi_domain_init(struct sbi_scratch *scratch, u32 cold_hartid)
 {
-	u32 i;
 	int rc;
 	struct sbi_hartmask *root_hmask;
 	struct sbi_domain_memregion *root_memregs;
-	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	int root_memregs_count = 0;
 
 	SBI_INIT_LIST_HEAD(&domain_list);
 
@@ -804,13 +920,15 @@ int sbi_domain_init(struct sbi_scratch *scratch, u32 cold_hartid)
 	/* Root domain firmware memory region */
 	sbi_domain_memregion_init(scratch->fw_start, scratch->fw_rw_offset,
 				  (SBI_DOMAIN_MEMREGION_M_READABLE |
-				   SBI_DOMAIN_MEMREGION_M_EXECUTABLE),
+				   SBI_DOMAIN_MEMREGION_M_EXECUTABLE |
+				   SBI_DOMAIN_MEMREGION_FW),
 				  &root_memregs[root_memregs_count++]);
 
 	sbi_domain_memregion_init((scratch->fw_start + scratch->fw_rw_offset),
 				  (scratch->fw_size - scratch->fw_rw_offset),
 				  (SBI_DOMAIN_MEMREGION_M_READABLE |
-				   SBI_DOMAIN_MEMREGION_M_WRITABLE),
+				   SBI_DOMAIN_MEMREGION_M_WRITABLE |
+				   SBI_DOMAIN_MEMREGION_FW),
 				  &root_memregs[root_memregs_count++]);
 
 	root.fw_region_inited = true;
@@ -840,7 +958,7 @@ int sbi_domain_init(struct sbi_scratch *scratch, u32 cold_hartid)
 	root.next_mode = scratch->next_mode;
 
 	/* Root domain possible and assigned HARTs */
-	for (i = 0; i < plat->hart_count; i++)
+	sbi_for_each_hartindex(i)
 		sbi_hartmask_set_hartindex(i, root_hmask);
 
 	/* Finally register the root domain */

lib/sbi/sbi_domain_context.c

@@ -15,6 +15,7 @@
 #include <sbi/sbi_string.h>
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_domain_context.h>
+#include <sbi/sbi_platform.h>
 #include <sbi/sbi_trap.h>
 
 /** Context representation for a hart within a domain */
@@ -44,6 +45,8 @@ struct hart_context {
 	unsigned long scounteren;
 	/** Supervisor environment configuration register */
 	unsigned long senvcfg;
+	/** Supervisor resource management configuration register */
+	unsigned long srmcfg;
 
 	/** Reference to the owning domain */
 	struct sbi_domain *dom;
@@ -53,31 +56,30 @@ struct hart_context {
 	bool initialized;
 };
 
-struct domain_context_priv {
-	/** Contexts for possible HARTs indexed by hartindex */
-	struct hart_context *hartindex_to_context_table[SBI_HARTMASK_MAX_BITS];
-};
-
-static struct sbi_domain_data dcpriv = {
-	.data_size = sizeof(struct domain_context_priv),
-};
+static struct sbi_domain_data dcpriv;
 
 static inline struct hart_context *hart_context_get(struct sbi_domain *dom,
 						    u32 hartindex)
 {
-	struct domain_context_priv *dcp = sbi_domain_data_ptr(dom, &dcpriv);
+	struct hart_context **dom_hartindex_to_context_table;
 
-	return (dcp && hartindex < SBI_HARTMASK_MAX_BITS) ?
-		dcp->hartindex_to_context_table[hartindex] : NULL;
+	dom_hartindex_to_context_table = sbi_domain_data_ptr(dom, &dcpriv);
+	if (!dom_hartindex_to_context_table || !sbi_hartindex_valid(hartindex))
+		return NULL;
+
+	return dom_hartindex_to_context_table[hartindex];
 }
 
 static void hart_context_set(struct sbi_domain *dom, u32 hartindex,
 			     struct hart_context *hc)
 {
-	struct domain_context_priv *dcp = sbi_domain_data_ptr(dom, &dcpriv);
+	struct hart_context **dom_hartindex_to_context_table;
 
-	if (dcp && hartindex < SBI_HARTMASK_MAX_BITS)
-		dcp->hartindex_to_context_table[hartindex] = hc;
+	dom_hartindex_to_context_table = sbi_domain_data_ptr(dom, &dcpriv);
+	if (!dom_hartindex_to_context_table || !sbi_hartindex_valid(hartindex))
+		return;
+
+	dom_hartindex_to_context_table[hartindex] = hc;
 }
 
 /** Macro to obtain the current hart's context pointer */
@@ -92,17 +94,23 @@ static void hart_context_set(struct sbi_domain *dom, u32 hartindex,
  *
  * @param ctx pointer to the current HART context
  * @param dom_ctx pointer to the target domain context
+ *
+ * @return 0 on success and negative error code on failure
  */
-static void switch_to_next_domain_context(struct hart_context *ctx,
+static int switch_to_next_domain_context(struct hart_context *ctx,
 					  struct hart_context *dom_ctx)
 {
 	u32 hartindex = current_hartindex();
 	struct sbi_trap_context *trap_ctx;
-	struct sbi_domain *current_dom = ctx->dom;
-	struct sbi_domain *target_dom = dom_ctx->dom;
+	struct sbi_domain *current_dom, *target_dom;
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	unsigned int pmp_count = sbi_hart_pmp_count(scratch);
 
+	if (!ctx || !dom_ctx || ctx == dom_ctx)
+		return SBI_EINVAL;
+
+	current_dom = ctx->dom;
+	target_dom = dom_ctx->dom;
 	/* Assign current hart to target domain */
 	spin_lock(&current_dom->assigned_harts_lock);
 	sbi_hartmask_clear_hartindex(hartindex, &current_dom->assigned_harts);
@@ -116,6 +124,11 @@ static void switch_to_next_domain_context(struct hart_context *ctx,
 
 	/* Reconfigure PMP settings for the new domain */
 	for (int i = 0; i < pmp_count; i++) {
+		/* Don't revoke firmware access permissions */
+		if (sbi_hart_smepmp_is_fw_region(i))
+			continue;
+
+		sbi_platform_pmp_disable(sbi_platform_thishart_ptr(), i);
 		pmp_disable(i);
 	}
 	sbi_hart_pmp_configure(scratch);
@@ -134,6 +147,8 @@ static void switch_to_next_domain_context(struct hart_context *ctx,
 		ctx->scounteren = csr_swap(CSR_SCOUNTEREN, dom_ctx->scounteren);
 	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12)
 		ctx->senvcfg	= csr_swap(CSR_SENVCFG, dom_ctx->senvcfg);
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSQOSID))
+		ctx->srmcfg	= csr_swap(CSR_SRMCFG, dom_ctx->srmcfg);
 
 	/* Save current trap state and restore target domain's trap state */
 	trap_ctx = sbi_trap_get_context(scratch);
@@ -155,13 +170,57 @@ static void switch_to_next_domain_context(struct hart_context *ctx,
 		else
 			sbi_hsm_hart_stop(scratch, true);
 	}
+	return 0;
+}
+
+static int hart_context_init(u32 hartindex)
+{
+	struct hart_context *ctx;
+	struct sbi_domain *dom;
+
+	sbi_domain_for_each(dom) {
+		if (!sbi_hartmask_test_hartindex(hartindex,
+						 dom->possible_harts))
+			continue;
+
+		ctx = sbi_zalloc(sizeof(struct hart_context));
+		if (!ctx)
+			return SBI_ENOMEM;
+
+		/* Bind context and domain */
+		ctx->dom = dom;
+		hart_context_set(dom, hartindex, ctx);
+	}
+
+	return 0;
 }
 
 int sbi_domain_context_enter(struct sbi_domain *dom)
 {
+	int rc;
+	struct hart_context *dom_ctx;
 	struct hart_context *ctx = hart_context_thishart_get();
-	struct hart_context *dom_ctx = hart_context_get(dom, current_hartindex());
 
+	/* Target domain must not be same as the current domain */
+	if (!dom || dom == sbi_domain_thishart_ptr())
+		return SBI_EINVAL;
+
+	/*
+	 * If it's first time to call `enter` on the current hart, no
+	 * context allocated before. Allocate context for each valid
+	 * domain on the current hart.
+	 */
+	if (!ctx) {
+		rc = hart_context_init(current_hartindex());
+		if (rc)
+			return rc;
+
+		ctx = hart_context_thishart_get();
+		if (!ctx)
+			return SBI_EINVAL;
+	}
+
+	dom_ctx = hart_context_get(dom, current_hartindex());
 	/* Validate the domain context existence */
 	if (!dom_ctx)
 		return SBI_EINVAL;
@@ -169,13 +228,12 @@ int sbi_domain_context_enter(struct sbi_domain *dom)
 	/* Update target context's previous context to indicate the caller */
 	dom_ctx->prev_ctx = ctx;
 
-	switch_to_next_domain_context(ctx, dom_ctx);
-
-	return 0;
+	return switch_to_next_domain_context(ctx, dom_ctx);
 }
 
 int sbi_domain_context_exit(void)
 {
+	int rc;
 	u32 hartindex = current_hartindex();
 	struct sbi_domain *dom;
 	struct hart_context *ctx = hart_context_thishart_get();
@@ -187,21 +245,13 @@ int sbi_domain_context_exit(void)
 	 * its context on the current hart if valid.
 	 */
 	if (!ctx) {
-		sbi_domain_for_each(dom) {
-			if (!sbi_hartmask_test_hartindex(hartindex,
-							 dom->possible_harts))
-				continue;
-
-			dom_ctx = sbi_zalloc(sizeof(struct hart_context));
-			if (!dom_ctx)
-				return SBI_ENOMEM;
-
-			/* Bind context and domain */
-			dom_ctx->dom = dom;
-			hart_context_set(dom, hartindex, dom_ctx);
-		}
+		rc = hart_context_init(current_hartindex());
+		if (rc)
+			return rc;
 
 		ctx = hart_context_thishart_get();
+		if (!ctx)
+			return SBI_EINVAL;
 	}
 
 	dom_ctx = ctx->prev_ctx;
@@ -225,13 +275,19 @@ int sbi_domain_context_exit(void)
 	if (!dom_ctx)
 		dom_ctx = hart_context_get(&root, hartindex);
 
-	switch_to_next_domain_context(ctx, dom_ctx);
-
-	return 0;
+	return switch_to_next_domain_context(ctx, dom_ctx);
 }
 
 int sbi_domain_context_init(void)
 {
+	/**
+	 * Allocate per-domain and per-hart context data.
+	 * The data type is "struct hart_context **" whose memory space will be
+	 * dynamically allocated by domain_setup_data_one(). Calculate needed
+	 * size of memory space here.
+	 */
+	dcpriv.data_size = sizeof(struct hart_context *) * sbi_hart_count();
+
 	return sbi_domain_register_data(&dcpriv);
 }
 

lib/sbi/sbi_double_trap.c

@@ -10,6 +10,7 @@
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_ecall_interface.h>
 #include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
 #include <sbi/sbi_sse.h>
 #include <sbi/sbi_trap.h>
 
@@ -28,3 +29,9 @@ int sbi_double_trap_handler(struct sbi_trap_context *tcntx)
 
 	return sbi_sse_inject_event(SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP);
 }
+
+void sbi_double_trap_init(struct sbi_scratch *scratch)
+{
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSDBLTRP))
+		sbi_sse_add_event(SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP, NULL);
+}

lib/sbi/sbi_ecall.c

@@ -93,7 +93,6 @@ int sbi_ecall_register_extension(struct sbi_ecall_extension *ext)
 			return SBI_EINVAL;
 	}
 
-	SBI_INIT_LIST_HEAD(&ext->head);
 	sbi_list_add_tail(&ext->head, &ecall_exts_list);
 
 	return 0;

lib/sbi/sbi_ecall_dbtr.c

@@ -43,7 +43,7 @@ static int sbi_ecall_dbtr_handler(unsigned long extid, unsigned long funcid,
 		ret = sbi_dbtr_enable_trig(regs->a0, regs->a1);
 		break;
 	case SBI_EXT_DBTR_TRIGGER_UPDATE:
-		ret = sbi_dbtr_update_trig(smode, regs->a0, regs->a1);
+		ret = sbi_dbtr_update_trig(smode, regs->a0);
 		break;
 	case SBI_EXT_DBTR_TRIGGER_DISABLE:
 		ret = sbi_dbtr_disable_trig(regs->a0, regs->a1);
@@ -69,7 +69,6 @@ struct sbi_ecall_extension ecall_dbtr = {
 	.name			= "dbtr",
 	.extid_start		= SBI_EXT_DBTR,
 	.extid_end		= SBI_EXT_DBTR,
-	.experimental		= true,
 	.handle			= sbi_ecall_dbtr_handler,
 	.register_extensions	= sbi_ecall_dbtr_register_extensions,
 };

lib/sbi/sbi_ecall_fwft.c

@@ -45,7 +45,6 @@ struct sbi_ecall_extension ecall_fwft = {
 	.name			= "fwft",
 	.extid_start		= SBI_EXT_FWFT,
 	.extid_end		= SBI_EXT_FWFT,
-	.experimental		= true,
 	.register_extensions	= sbi_ecall_fwft_register_extensions,
 	.handle			= sbi_ecall_fwft_handler,
 };

lib/sbi/sbi_ecall_mpxy.c

@@ -20,8 +20,11 @@ static int sbi_ecall_mpxy_handler(unsigned long extid, unsigned long funcid,
 	int ret = 0;
 
 	switch (funcid) {
+	case SBI_EXT_MPXY_GET_SHMEM_SIZE:
+		out->value = sbi_mpxy_get_shmem_size();
+		break;
 	case SBI_EXT_MPXY_SET_SHMEM:
-		ret = sbi_mpxy_set_shmem(regs->a0, regs->a1, regs->a2, regs->a3);
+		ret = sbi_mpxy_set_shmem(regs->a0, regs->a1, regs->a2);
 		break;
 	case SBI_EXT_MPXY_GET_CHANNEL_IDS:
 		ret = sbi_mpxy_get_channel_ids(regs->a0);
@@ -36,7 +39,7 @@ static int sbi_ecall_mpxy_handler(unsigned long extid, unsigned long funcid,
 		ret = sbi_mpxy_send_message(regs->a0, regs->a1,
 					    regs->a2, &out->value);
 		break;
-	case SBI_EXT_MPXY_SEND_MSG_NO_RESP:
+	case SBI_EXT_MPXY_SEND_MSG_WITHOUT_RESP:
 		ret = sbi_mpxy_send_message(regs->a0, regs->a1, regs->a2,
 					    NULL);
 		break;
@@ -64,7 +67,6 @@ struct sbi_ecall_extension ecall_mpxy = {
 	.name			= "mpxy",
 	.extid_start		= SBI_EXT_MPXY,
 	.extid_end		= SBI_EXT_MPXY,
-	.experimental		= true,
 	.register_extensions	= sbi_ecall_mpxy_register_extensions,
 	.handle			= sbi_ecall_mpxy_handler,
 };

lib/sbi/sbi_ecall_sse.c

@@ -59,7 +59,6 @@ struct sbi_ecall_extension ecall_sse = {
 	.name			= "sse",
 	.extid_start		= SBI_EXT_SSE,
 	.extid_end		= SBI_EXT_SSE,
-	.experimental		= true,
 	.register_extensions	= sbi_ecall_sse_register_extensions,
 	.handle			= sbi_ecall_sse_handler,
 };

lib/sbi/sbi_fwft.c

@@ -223,32 +223,32 @@ static int fwft_pmlen_supported(struct fwft_config *conf)
 	return SBI_OK;
 }
 
-static bool fwft_try_to_set_pmm(unsigned long pmm)
-{
-	csr_set(CSR_MENVCFG, pmm);
-	return (csr_read(CSR_MENVCFG) & ENVCFG_PMM) == pmm;
-}
-
 static int fwft_set_pmlen(struct fwft_config *conf, unsigned long value)
 {
-	unsigned long prev;
+	unsigned long pmm, prev;
 
-	if (value > 16)
+	switch (value) {
+	case 0:
+		pmm = ENVCFG_PMM_PMLEN_0;
+		break;
+	case 7:
+		pmm = ENVCFG_PMM_PMLEN_7;
+		break;
+	case 16:
+		pmm = ENVCFG_PMM_PMLEN_16;
+		break;
+	default:
 		return SBI_EINVAL;
+	}
 
 	prev = csr_read_clear(CSR_MENVCFG, ENVCFG_PMM);
-	if (value == 0)
-		return SBI_OK;
-	if (value <= 7) {
-		if (fwft_try_to_set_pmm(ENVCFG_PMM_PMLEN_7))
-			return SBI_OK;
-		csr_clear(CSR_MENVCFG, ENVCFG_PMM);
+	csr_set(CSR_MENVCFG, pmm);
+	if ((csr_read(CSR_MENVCFG) & ENVCFG_PMM) != pmm) {
+		csr_write(CSR_MENVCFG, prev);
+		return SBI_EINVAL;
 	}
-	if (fwft_try_to_set_pmm(ENVCFG_PMM_PMLEN_16))
-		return SBI_OK;
-	csr_write(CSR_MENVCFG, prev);
 
-	return SBI_EINVAL;
+	return SBI_OK;
 }
 
 static int fwft_get_pmlen(struct fwft_config *conf, unsigned long *value)
@@ -337,7 +337,7 @@ int sbi_fwft_set(enum sbi_fwft_feature_t feature, unsigned long value,
 		return SBI_EINVAL;
 
 	if (conf->flags & SBI_FWFT_SET_FLAG_LOCK)
-		return SBI_EDENIED;
+		return SBI_EDENIED_LOCKED;
 
 	ret = conf->feature->set(conf, value);
 	if (ret)

lib/sbi/sbi_hart.c

@@ -30,6 +30,8 @@ extern void __sbi_expected_trap_hext(void);
 void (*sbi_hart_expected_trap)(void) = &__sbi_expected_trap;
 
 static unsigned long hart_features_offset;
+static DECLARE_BITMAP(fw_smepmp_ids, PMP_COUNT);
+static bool fw_smepmp_ids_inited;
 
 static void mstatus_init(struct sbi_scratch *scratch)
 {
@@ -49,10 +51,10 @@ static void mstatus_init(struct sbi_scratch *scratch)
 
 	csr_write(CSR_MSTATUS, mstatus_val);
 
-	/* Disable user mode usage of all perf counters except default ones (CY, TM, IR) */
+	/* Disable user mode usage of all perf counters except TM */
 	if (misa_extension('S') &&
 	    sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_10)
-		csr_write(CSR_SCOUNTEREN, 7);
+		csr_write(CSR_SCOUNTEREN, 0x02);
 
 	/**
 	 * OpenSBI doesn't use any PMU counters in M-mode.
@@ -85,11 +87,11 @@ static void mstatus_init(struct sbi_scratch *scratch)
 #endif
 	}
 
+	if (misa_extension('H'))
+		csr_write(CSR_HSTATUS, 0);
+
 	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SMSTATEEN)) {
-		mstateen_val = csr_read(CSR_MSTATEEN0);
-#if __riscv_xlen == 32
-		mstateen_val |= ((uint64_t)csr_read(CSR_MSTATEEN0H)) << 32;
-#endif
+		mstateen_val = 0;
 		mstateen_val |= SMSTATEEN_STATEN;
 		mstateen_val |= SMSTATEEN0_CONTEXT;
 		mstateen_val |= SMSTATEEN0_HSENVCFG;
@@ -105,17 +107,39 @@ static void mstatus_init(struct sbi_scratch *scratch)
 		else
 			mstateen_val &= ~(SMSTATEEN0_SVSLCT);
 
-		csr_write(CSR_MSTATEEN0, mstateen_val);
-#if __riscv_xlen == 32
-		csr_write(CSR_MSTATEEN0H, mstateen_val >> 32);
-#endif
+		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCTR))
+			mstateen_val |= SMSTATEEN0_CTR;
+		else
+			mstateen_val &= ~SMSTATEEN0_CTR;
+
+		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSQOSID))
+			mstateen_val |= SMSTATEEN0_SRMCFG;
+		else
+			mstateen_val &= ~SMSTATEEN0_SRMCFG;
+
+		csr_write64(CSR_MSTATEEN0, mstateen_val);
+		csr_write64(CSR_MSTATEEN1, SMSTATEEN_STATEN);
+		csr_write64(CSR_MSTATEEN2, SMSTATEEN_STATEN);
+		csr_write64(CSR_MSTATEEN3, SMSTATEEN_STATEN);
+	}
+
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSSTATEEN)) {
+		if (misa_extension('S')) {
+			csr_write(CSR_SSTATEEN0, 0);
+			csr_write(CSR_SSTATEEN1, 0);
+			csr_write(CSR_SSTATEEN2, 0);
+			csr_write(CSR_SSTATEEN3, 0);
+		}
+		if (misa_extension('H')) {
+			csr_write64(CSR_HSTATEEN0, (uint64_t)0);
+			csr_write64(CSR_HSTATEEN1, (uint64_t)0);
+			csr_write64(CSR_HSTATEEN2, (uint64_t)0);
+			csr_write64(CSR_HSTATEEN3, (uint64_t)0);
+		}
 	}
 
 	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12) {
-		menvcfg_val = csr_read(CSR_MENVCFG);
-#if __riscv_xlen == 32
-		menvcfg_val |= ((uint64_t)csr_read(CSR_MENVCFGH)) << 32;
-#endif
+		menvcfg_val = csr_read64(CSR_MENVCFG);
 
 		/* Disable double trap by default */
 		menvcfg_val &= ~ENVCFG_DTE;
@@ -151,10 +175,7 @@ static void mstatus_init(struct sbi_scratch *scratch)
 		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SVADE))
 			menvcfg_val &= ~ENVCFG_ADUE;
 
-		csr_write(CSR_MENVCFG, menvcfg_val);
-#if __riscv_xlen == 32
-		csr_write(CSR_MENVCFGH, menvcfg_val >> 32);
-#endif
+		csr_write64(CSR_MENVCFG, menvcfg_val);
 
 		/* Enable S-mode access to seed CSR */
 		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_ZKR)) {
@@ -203,7 +224,7 @@ static int delegate_traps(struct sbi_scratch *scratch)
 
 	/* Send M-mode interrupts and most exceptions to S-mode */
 	interrupts = MIP_SSIP | MIP_STIP | MIP_SEIP;
-	interrupts |= sbi_pmu_irq_bit();
+	interrupts |= sbi_pmu_irq_mask();
 
 	exceptions = (1U << CAUSE_MISALIGNED_FETCH) | (1U << CAUSE_BREAKPOINT) |
 		     (1U << CAUSE_USER_ECALL);
@@ -287,67 +308,12 @@ unsigned int sbi_hart_mhpm_bits(struct sbi_scratch *scratch)
 	return hfeatures->mhpm_bits;
 }
 
-/*
- * Returns Smepmp flags for a given domain and region based on permissions.
- */
-static unsigned int sbi_hart_get_smepmp_flags(struct sbi_scratch *scratch,
-					      struct sbi_domain *dom,
-					      struct sbi_domain_memregion *reg)
+bool sbi_hart_smepmp_is_fw_region(unsigned int pmp_idx)
 {
-	unsigned int pmp_flags = 0;
+	if (!fw_smepmp_ids_inited)
+		return false;
 
-	if (SBI_DOMAIN_MEMREGION_IS_SHARED(reg->flags)) {
-		/* Read only for both M and SU modes */
-		if (SBI_DOMAIN_MEMREGION_IS_SUR_MR(reg->flags))
-			pmp_flags = (PMP_L | PMP_R | PMP_W | PMP_X);
-
-		/* Execute for SU but Read/Execute for M mode */
-		else if (SBI_DOMAIN_MEMREGION_IS_SUX_MRX(reg->flags))
-			/* locked region */
-			pmp_flags = (PMP_L | PMP_W | PMP_X);
-
-		/* Execute only for both M and SU modes */
-		else if (SBI_DOMAIN_MEMREGION_IS_SUX_MX(reg->flags))
-			pmp_flags = (PMP_L | PMP_W);
-
-		/* Read/Write for both M and SU modes */
-		else if (SBI_DOMAIN_MEMREGION_IS_SURW_MRW(reg->flags))
-			pmp_flags = (PMP_W | PMP_X);
-
-		/* Read only for SU mode but Read/Write for M mode */
-		else if (SBI_DOMAIN_MEMREGION_IS_SUR_MRW(reg->flags))
-			pmp_flags = (PMP_W);
-	} else if (SBI_DOMAIN_MEMREGION_M_ONLY_ACCESS(reg->flags)) {
-		/*
-		 * When smepmp is supported and used, M region cannot have RWX
-		 * permissions on any region.
-		 */
-		if ((reg->flags & SBI_DOMAIN_MEMREGION_M_ACCESS_MASK)
-		    == SBI_DOMAIN_MEMREGION_M_RWX) {
-			sbi_printf("%s: M-mode only regions cannot have"
-				   "RWX permissions\n", __func__);
-			return 0;
-		}
-
-		/* M-mode only access regions are always locked */
-		pmp_flags |= PMP_L;
-
-		if (reg->flags & SBI_DOMAIN_MEMREGION_M_READABLE)
-			pmp_flags |= PMP_R;
-		if (reg->flags & SBI_DOMAIN_MEMREGION_M_WRITABLE)
-			pmp_flags |= PMP_W;
-		if (reg->flags & SBI_DOMAIN_MEMREGION_M_EXECUTABLE)
-			pmp_flags |= PMP_X;
-	} else if (SBI_DOMAIN_MEMREGION_SU_ONLY_ACCESS(reg->flags)) {
-		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_READABLE)
-			pmp_flags |= PMP_R;
-		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_WRITABLE)
-			pmp_flags |= PMP_W;
-		if (reg->flags & SBI_DOMAIN_MEMREGION_SU_EXECUTABLE)
-			pmp_flags |= PMP_X;
-	}
-
-	return pmp_flags;
+	return bitmap_test(fw_smepmp_ids, pmp_idx) ? true : false;
 }
 
 static void sbi_hart_smepmp_set(struct sbi_scratch *scratch,
@@ -361,6 +327,9 @@ static void sbi_hart_smepmp_set(struct sbi_scratch *scratch,
 	unsigned long pmp_addr = reg->base >> PMP_SHIFT;
 
 	if (pmp_log2gran <= reg->order && pmp_addr < pmp_addr_max) {
+		sbi_platform_pmp_set(sbi_platform_ptr(scratch),
+				     pmp_idx, reg->flags, pmp_flags,
+				     reg->base, reg->order);
 		pmp_set(pmp_idx, pmp_flags, reg->base, reg->order);
 	} else {
 		sbi_printf("Can not configure pmp for domain %s because"
@@ -370,6 +339,15 @@ static void sbi_hart_smepmp_set(struct sbi_scratch *scratch,
 	}
 }
 
+static bool is_valid_pmp_idx(unsigned int pmp_count, unsigned int pmp_idx)
+{
+	if (pmp_count > pmp_idx)
+		return true;
+
+	sbi_printf("error: insufficient PMP entries\n");
+	return false;
+}
+
 static int sbi_hart_smepmp_configure(struct sbi_scratch *scratch,
 				     unsigned int pmp_count,
 				     unsigned int pmp_log2gran,
@@ -394,8 +372,8 @@ static int sbi_hart_smepmp_configure(struct sbi_scratch *scratch,
 		/* Skip reserved entry */
 		if (pmp_idx == SBI_SMEPMP_RESV_ENTRY)
 			pmp_idx++;
-		if (pmp_count <= pmp_idx)
-			break;
+		if (!is_valid_pmp_idx(pmp_count, pmp_idx))
+			return SBI_EFAIL;
 
 		/* Skip shared and SU-only regions */
 		if (!SBI_DOMAIN_MEMREGION_M_ONLY_ACCESS(reg->flags)) {
@@ -403,14 +381,31 @@ static int sbi_hart_smepmp_configure(struct sbi_scratch *scratch,
 			continue;
 		}
 
-		pmp_flags = sbi_hart_get_smepmp_flags(scratch, dom, reg);
-		if (!pmp_flags)
-			return 0;
+		/*
+		 * Track firmware PMP entries to preserve them during
+		 * domain switches. Under SmePMP, M-mode requires
+		 * explicit PMP entries to access firmware code/data.
+		 * These entries must remain enabled across domain
+		 * context switches to prevent M-mode access faults.
+		 */
+		if (SBI_DOMAIN_MEMREGION_IS_FIRMWARE(reg->flags)) {
+			if (fw_smepmp_ids_inited) {
+				/* Check inconsistent firmware region */
+				if (!sbi_hart_smepmp_is_fw_region(pmp_idx))
+					return SBI_EINVAL;
+			} else {
+				bitmap_set(fw_smepmp_ids, pmp_idx, 1);
+			}
+		}
+
+		pmp_flags = sbi_domain_get_smepmp_flags(reg);
 
 		sbi_hart_smepmp_set(scratch, dom, reg, pmp_idx++, pmp_flags,
 				    pmp_log2gran, pmp_addr_max);
 	}
 
+	fw_smepmp_ids_inited = true;
+
 	/* Set the MML to enforce new encoding */
 	csr_set(CSR_MSECCFG, MSECCFG_MML);
 
@@ -420,8 +415,8 @@ static int sbi_hart_smepmp_configure(struct sbi_scratch *scratch,
 		/* Skip reserved entry */
 		if (pmp_idx == SBI_SMEPMP_RESV_ENTRY)
 			pmp_idx++;
-		if (pmp_count <= pmp_idx)
-			break;
+		if (!is_valid_pmp_idx(pmp_count, pmp_idx))
+			return SBI_EFAIL;
 
 		/* Skip M-only regions */
 		if (SBI_DOMAIN_MEMREGION_M_ONLY_ACCESS(reg->flags)) {
@@ -429,9 +424,7 @@ static int sbi_hart_smepmp_configure(struct sbi_scratch *scratch,
 			continue;
 		}
 
-		pmp_flags = sbi_hart_get_smepmp_flags(scratch, dom, reg);
-		if (!pmp_flags)
-			return 0;
+		pmp_flags = sbi_domain_get_smepmp_flags(reg);
 
 		sbi_hart_smepmp_set(scratch, dom, reg, pmp_idx++, pmp_flags,
 				    pmp_log2gran, pmp_addr_max);
@@ -457,8 +450,8 @@ static int sbi_hart_oldpmp_configure(struct sbi_scratch *scratch,
 	unsigned long pmp_addr;
 
 	sbi_domain_for_each_memregion(dom, reg) {
-		if (pmp_count <= pmp_idx)
-			break;
+		if (!is_valid_pmp_idx(pmp_count, pmp_idx))
+			return SBI_EFAIL;
 
 		pmp_flags = 0;
 
@@ -478,6 +471,9 @@ static int sbi_hart_oldpmp_configure(struct sbi_scratch *scratch,
 
 		pmp_addr = reg->base >> PMP_SHIFT;
 		if (pmp_log2gran <= reg->order && pmp_addr < pmp_addr_max) {
+			sbi_platform_pmp_set(sbi_platform_ptr(scratch),
+					     pmp_idx, reg->flags, pmp_flags,
+					     reg->base, reg->order);
 			pmp_set(pmp_idx++, pmp_flags, reg->base, reg->order);
 		} else {
 			sbi_printf("Can not configure pmp for domain %s because"
@@ -518,6 +514,9 @@ int sbi_hart_map_saddr(unsigned long addr, unsigned long size)
 		}
 	}
 
+	sbi_platform_pmp_set(sbi_platform_ptr(scratch), SBI_SMEPMP_RESV_ENTRY,
+			     SBI_DOMAIN_MEMREGION_SHARED_SURW_MRW,
+			     pmp_flags, base, order);
 	pmp_set(SBI_SMEPMP_RESV_ENTRY, pmp_flags, base, order);
 
 	return SBI_OK;
@@ -530,6 +529,7 @@ int sbi_hart_unmap_saddr(void)
 	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SMEPMP))
 		return SBI_OK;
 
+	sbi_platform_pmp_disable(sbi_platform_ptr(scratch), SBI_SMEPMP_RESV_ENTRY);
 	return pmp_disable(SBI_SMEPMP_RESV_ENTRY);
 }
 
@@ -688,6 +688,12 @@ const struct sbi_hart_ext_data sbi_hart_ext[] = {
 	__SBI_HART_EXT_DATA(zicfilp, SBI_HART_EXT_ZICFILP),
 	__SBI_HART_EXT_DATA(zicfiss, SBI_HART_EXT_ZICFISS),
 	__SBI_HART_EXT_DATA(ssdbltrp, SBI_HART_EXT_SSDBLTRP),
+	__SBI_HART_EXT_DATA(smctr, SBI_HART_EXT_SMCTR),
+	__SBI_HART_EXT_DATA(ssctr, SBI_HART_EXT_SSCTR),
+	__SBI_HART_EXT_DATA(ssqosid, SBI_HART_EXT_SSQOSID),
+	__SBI_HART_EXT_DATA(ssstateen, SBI_HART_EXT_SSSTATEEN),
+	__SBI_HART_EXT_DATA(xsfcflushdlone, SBI_HART_EXT_XSIFIVE_CFLUSH_D_L1),
+	__SBI_HART_EXT_DATA(xsfcease, SBI_HART_EXT_XSIFIVE_CEASE),
 };
 
 _Static_assert(SBI_HART_EXT_MAX == array_size(sbi_hart_ext),
@@ -714,6 +720,10 @@ void sbi_hart_get_extensions_str(struct sbi_scratch *scratch,
 	sbi_memset(extensions_str, 0, nestr);
 
 	for_each_set_bit(ext, hfeatures->extensions, SBI_HART_EXT_MAX) {
+		if (offset + sbi_strlen(sbi_hart_ext[ext].name) + 1 > nestr) {
+			sbi_printf("%s:extension name is longer than buffer (error)\n", __func__);
+			break;
+		}
 		sbi_snprintf(extensions_str + offset,
 				 nestr - offset,
 				 "%s,", sbi_hart_ext[ext].name);
@@ -726,6 +736,20 @@ void sbi_hart_get_extensions_str(struct sbi_scratch *scratch,
 		sbi_strncpy(extensions_str, "none", nestr);
 }
 
+/**
+ * Check whether a particular CSR is present on the HART
+ *
+ * @param scratch pointer to the HART scratch space
+ * @param csr the CSR number to check
+ */
+bool sbi_hart_has_csr(struct sbi_scratch *scratch, enum sbi_hart_csrs csr)
+{
+	struct sbi_hart_features *hfeatures =
+			sbi_scratch_offset_ptr(scratch, hart_features_offset);
+
+	return __test_bit(csr, hfeatures->csrs);
+}
+
 static unsigned long hart_pmp_get_allowed_addr(void)
 {
 	unsigned long val = 0;
@@ -782,7 +806,6 @@ static int hart_detect_features(struct sbi_scratch *scratch)
 	struct sbi_hart_features *hfeatures =
 		sbi_scratch_offset_ptr(scratch, hart_features_offset);
 	unsigned long val, oldval;
-	bool has_zicntr = false;
 	int rc;
 
 	/* If hart features already detected then do nothing */
@@ -791,6 +814,7 @@ static int hart_detect_features(struct sbi_scratch *scratch)
 
 	/* Clear hart features */
 	sbi_memset(hfeatures->extensions, 0, sizeof(hfeatures->extensions));
+	sbi_memset(hfeatures->csrs, 0, sizeof(hfeatures->csrs));
 	hfeatures->pmp_count = 0;
 	hfeatures->mhpm_mask = 0;
 	hfeatures->priv_version = SBI_HART_PRIV_VER_UNKNOWN;
@@ -917,9 +941,6 @@ __pmp_skip:
 	/* Detect if hart supports sscofpmf */
 	__check_ext_csr(SBI_HART_PRIV_VER_1_11,
 			CSR_SCOUNTOVF, SBI_HART_EXT_SSCOFPMF);
-	/* Detect if hart supports time CSR */
-	__check_ext_csr(SBI_HART_PRIV_VER_UNKNOWN,
-			CSR_TIME, SBI_HART_EXT_ZICNTR);
 	/* Detect if hart has AIA local interrupt CSRs */
 	__check_ext_csr(SBI_HART_PRIV_VER_UNKNOWN,
 			CSR_MTOPI, SBI_HART_EXT_SMAIA);
@@ -929,6 +950,9 @@ __pmp_skip:
 	/* Detect if hart supports mstateen CSRs */
 	__check_ext_csr(SBI_HART_PRIV_VER_1_12,
 			CSR_MSTATEEN0, SBI_HART_EXT_SMSTATEEN);
+	/* Detect if hart supports sstateen CSRs */
+	__check_ext_csr(SBI_HART_PRIV_VER_1_12,
+			CSR_SSTATEEN0, SBI_HART_EXT_SSSTATEEN);
 	/* Detect if hart supports smcntrpmf */
 	__check_ext_csr(SBI_HART_PRIV_VER_1_12,
 			CSR_MCYCLECFG, SBI_HART_EXT_SMCNTRPMF);
@@ -938,8 +962,16 @@ __pmp_skip:
 
 #undef __check_ext_csr
 
-	/* Save trap based detection of Zicntr */
-	has_zicntr = sbi_hart_has_extension(scratch, SBI_HART_EXT_ZICNTR);
+#define __check_csr_existence(__csr, __csr_id)				\
+	csr_read_allowed(__csr, &trap);					\
+	if (!trap.cause)						\
+		__set_bit(__csr_id, hfeatures->csrs);
+
+	__check_csr_existence(CSR_CYCLE, SBI_HART_CSR_CYCLE);
+	__check_csr_existence(CSR_TIME, SBI_HART_CSR_TIME);
+	__check_csr_existence(CSR_INSTRET, SBI_HART_CSR_INSTRET);
+
+#undef __check_csr_existence
 
 	/* Let platform populate extensions */
 	rc = sbi_platform_extensions_init(sbi_platform_thishart_ptr(),
@@ -949,7 +981,9 @@ __pmp_skip:
 
 	/* Zicntr should only be detected using traps */
 	__sbi_hart_update_extension(hfeatures, SBI_HART_EXT_ZICNTR,
-				    has_zicntr);
+			    sbi_hart_has_csr(scratch, SBI_HART_CSR_CYCLE) &&
+			    sbi_hart_has_csr(scratch, SBI_HART_CSR_TIME)  &&
+			    sbi_hart_has_csr(scratch, SBI_HART_CSR_INSTRET));
 
 	/* Extensions implied by other extensions and features */
 	if (hfeatures->mhpm_mask)
@@ -982,10 +1016,6 @@ int sbi_hart_reinit(struct sbi_scratch *scratch)
 	if (rc)
 		return rc;
 
-	rc = delegate_traps(scratch);
-	if (rc)
-		return rc;
-
 	return 0;
 }
 
@@ -1013,6 +1043,10 @@ int sbi_hart_init(struct sbi_scratch *scratch, bool cold_boot)
 	if (rc)
 		return rc;
 
+	rc = delegate_traps(scratch);
+	if (rc)
+		return rc;
+
 	return sbi_hart_reinit(scratch);
 }
 

lib/sbi/sbi_heap.c

@@ -16,7 +16,9 @@
 
 /* Minimum size and alignment of heap allocations */
 #define HEAP_ALLOC_ALIGN		64
-#define HEAP_HOUSEKEEPING_FACTOR	16
+
+/* Number of heap nodes to allocate at once */
+#define HEAP_NODE_BATCH_SIZE		8
 
 struct heap_node {
 	struct sbi_dlist head;
@@ -28,20 +30,50 @@ struct sbi_heap_control {
 	spinlock_t lock;
 	unsigned long base;
 	unsigned long size;
-	unsigned long hkbase;
-	unsigned long hksize;
+	unsigned long resv;
 	struct sbi_dlist free_node_list;
 	struct sbi_dlist free_space_list;
 	struct sbi_dlist used_space_list;
+	struct heap_node init_free_space_node;
 };
 
 struct sbi_heap_control global_hpctrl;
 
+static bool alloc_nodes(struct sbi_heap_control *hpctrl)
+{
+	size_t size = HEAP_NODE_BATCH_SIZE * sizeof(struct heap_node);
+	struct heap_node *n, *new = NULL;
+
+	/* alloc_with_align() requires at most two free nodes */
+	if (hpctrl->free_node_list.next != hpctrl->free_node_list.prev)
+		return true;
+
+	sbi_list_for_each_entry_reverse(n, &hpctrl->free_space_list, head) {
+		if (n->size >= size) {
+			n->size -= size;
+			if (!n->size) {
+				sbi_list_del(&n->head);
+				sbi_list_add_tail(&n->head, &hpctrl->free_node_list);
+			}
+			new = (void *)(n->addr + n->size);
+			break;
+		}
+	}
+	if (!new)
+		return false;
+
+	for (size_t i = 0; i < HEAP_NODE_BATCH_SIZE; i++)
+		sbi_list_add_tail(&new[i].head, &hpctrl->free_node_list);
+	hpctrl->resv += size;
+
+	return true;
+}
+
 static void *alloc_with_align(struct sbi_heap_control *hpctrl,
 			      size_t align, size_t size)
 {
 	void *ret = NULL;
-	struct heap_node *n, *np, *rem;
+	struct heap_node *n, *np;
 	unsigned long lowest_aligned;
 	size_t pad;
 
@@ -53,6 +85,10 @@ static void *alloc_with_align(struct sbi_heap_control *hpctrl,
 
 	spin_lock(&hpctrl->lock);
 
+	/* Ensure at least two free nodes are available for use below */
+	if (!alloc_nodes(hpctrl))
+		goto out;
+
 	np = NULL;
 	sbi_list_for_each_entry(n, &hpctrl->free_space_list, head) {
 		lowest_aligned = ROUNDUP(n->addr, align);
@@ -67,55 +103,34 @@ static void *alloc_with_align(struct sbi_heap_control *hpctrl,
 		goto out;
 
 	if (pad) {
-		if (sbi_list_empty(&hpctrl->free_node_list)) {
-			goto out;
-		}
-
 		n = sbi_list_first_entry(&hpctrl->free_node_list,
 					 struct heap_node, head);
 		sbi_list_del(&n->head);
 
-		if ((size + pad < np->size) &&
-		    !sbi_list_empty(&hpctrl->free_node_list)) {
-			rem = sbi_list_first_entry(&hpctrl->free_node_list,
-						   struct heap_node, head);
-			sbi_list_del(&rem->head);
-			rem->addr = np->addr + (size + pad);
-			rem->size = np->size - (size + pad);
-			sbi_list_add_tail(&rem->head,
-					  &hpctrl->free_space_list);
-		} else if (size + pad != np->size) {
-			/* Can't allocate, return n */
-			sbi_list_add(&n->head, &hpctrl->free_node_list);
-			ret = NULL;
-			goto out;
-		}
+		n->addr = np->addr;
+		n->size = pad;
+		sbi_list_add_tail(&n->head, &np->head);
 
-		n->addr = lowest_aligned;
-		n->size = size;
-		sbi_list_add_tail(&n->head, &hpctrl->used_space_list);
-
-		np->size = pad;
-		ret = (void *)n->addr;
-	} else {
-		if ((size < np->size) &&
-		    !sbi_list_empty(&hpctrl->free_node_list)) {
-			n = sbi_list_first_entry(&hpctrl->free_node_list,
-						 struct heap_node, head);
-			sbi_list_del(&n->head);
-			n->addr = np->addr;
-			n->size = size;
-			np->addr += size;
-			np->size -= size;
-			sbi_list_add_tail(&n->head, &hpctrl->used_space_list);
-			ret = (void *)n->addr;
-		} else if (size == np->size) {
-			sbi_list_del(&np->head);
-			sbi_list_add_tail(&np->head, &hpctrl->used_space_list);
-			ret = (void *)np->addr;
-		}
+		np->addr += pad;
+		np->size -= pad;
 	}
 
+	if (size < np->size) {
+		n = sbi_list_first_entry(&hpctrl->free_node_list,
+					 struct heap_node, head);
+		sbi_list_del(&n->head);
+
+		n->addr = np->addr + size;
+		n->size = np->size - size;
+		sbi_list_add(&n->head, &np->head);
+
+		np->size = size;
+	}
+
+	sbi_list_del(&np->head);
+	sbi_list_add_tail(&np->head, &hpctrl->used_space_list);
+	ret = (void *)np->addr;
+
 out:
 	spin_unlock(&hpctrl->lock);
 
@@ -216,45 +231,32 @@ unsigned long sbi_heap_free_space_from(struct sbi_heap_control *hpctrl)
 
 unsigned long sbi_heap_used_space_from(struct sbi_heap_control *hpctrl)
 {
-	return hpctrl->size - hpctrl->hksize - sbi_heap_free_space();
+	return hpctrl->size - hpctrl->resv - sbi_heap_free_space();
 }
 
 unsigned long sbi_heap_reserved_space_from(struct sbi_heap_control *hpctrl)
 {
-	return hpctrl->hksize;
+	return hpctrl->resv;
 }
 
 int sbi_heap_init_new(struct sbi_heap_control *hpctrl, unsigned long base,
 		       unsigned long size)
 {
-	unsigned long i;
 	struct heap_node *n;
 
 	/* Initialize heap control */
 	SPIN_LOCK_INIT(hpctrl->lock);
 	hpctrl->base = base;
 	hpctrl->size = size;
-	hpctrl->hkbase = hpctrl->base;
-	hpctrl->hksize = hpctrl->size / HEAP_HOUSEKEEPING_FACTOR;
-	hpctrl->hksize &= ~((unsigned long)HEAP_BASE_ALIGN - 1);
+	hpctrl->resv = 0;
 	SBI_INIT_LIST_HEAD(&hpctrl->free_node_list);
 	SBI_INIT_LIST_HEAD(&hpctrl->free_space_list);
 	SBI_INIT_LIST_HEAD(&hpctrl->used_space_list);
 
-	/* Prepare free node list */
-	for (i = 0; i < (hpctrl->hksize / sizeof(*n)); i++) {
-		n = (struct heap_node *)(hpctrl->hkbase + (sizeof(*n) * i));
-		SBI_INIT_LIST_HEAD(&n->head);
-		n->addr = n->size = 0;
-		sbi_list_add_tail(&n->head, &hpctrl->free_node_list);
-	}
-
 	/* Prepare free space list */
-	n = sbi_list_first_entry(&hpctrl->free_node_list,
-				 struct heap_node, head);
-	sbi_list_del(&n->head);
-	n->addr = hpctrl->hkbase + hpctrl->hksize;
-	n->size = hpctrl->size - hpctrl->hksize;
+	n = &hpctrl->init_free_space_node;
+	n->addr = base;
+	n->size = size;
 	sbi_list_add_tail(&n->head, &hpctrl->free_space_list);
 
 	return 0;

lib/sbi/sbi_hsm.c

@@ -37,6 +37,8 @@
 
 static const struct sbi_hsm_device *hsm_dev = NULL;
 static unsigned long hart_data_offset;
+static bool hsm_device_has_hart_hotplug(void);
+static int hsm_device_hart_stop(void);
 
 /** Per hart specific data to manage state transition **/
 struct sbi_hsm_data {
@@ -45,10 +47,8 @@ struct sbi_hsm_data {
 	unsigned long saved_mie;
 	unsigned long saved_mip;
 	unsigned long saved_medeleg;
-	unsigned long saved_menvcfg;
-#if __riscv_xlen == 32
-	unsigned long saved_menvcfgh;
-#endif
+	unsigned long saved_mideleg;
+	u64 saved_menvcfg;
 	atomic_t start_ticket;
 };
 
@@ -170,6 +170,15 @@ static void sbi_hsm_hart_wait(struct sbi_scratch *scratch)
 
 	/* Wait for state transition requested by sbi_hsm_hart_start() */
 	while (atomic_read(&hdata->state) != SBI_HSM_STATE_START_PENDING) {
+		/*
+		 * If the hsm_dev is ready and it support the hotplug, we can
+		 * use the hsm stop for more power saving
+		 */
+		if (hsm_device_has_hart_hotplug()) {
+			sbi_revert_entry_count(scratch);
+			hsm_device_hart_stop();
+		}
+
 		wfi();
 	}
 
@@ -238,7 +247,6 @@ static void hsm_device_hart_resume(void)
 
 int sbi_hsm_init(struct sbi_scratch *scratch, bool cold_boot)
 {
-	u32 i;
 	struct sbi_scratch *rscratch;
 	struct sbi_hsm_data *hdata;
 
@@ -248,7 +256,7 @@ int sbi_hsm_init(struct sbi_scratch *scratch, bool cold_boot)
 			return SBI_ENOMEM;
 
 		/* Initialize hart state data for every hart */
-		for (i = 0; i <= sbi_scratch_last_hartindex(); i++) {
+		sbi_for_each_hartindex(i) {
 			rscratch = sbi_hartindex_to_scratch(i);
 			if (!rscratch)
 				continue;
@@ -356,7 +364,7 @@ int sbi_hsm_hart_start(struct sbi_scratch *scratch,
 	   (hsm_device_has_hart_secondary_boot() && !init_count)) {
 		rc = hsm_device_hart_start(hartid, scratch->warmboot_addr);
 	} else {
-		rc = sbi_ipi_raw_send(hartindex);
+		rc = sbi_ipi_raw_send(hartindex, true);
 	}
 
 	if (!rc)
@@ -419,12 +427,9 @@ void __sbi_hsm_suspend_non_ret_save(struct sbi_scratch *scratch)
 	hdata->saved_mie = csr_read(CSR_MIE);
 	hdata->saved_mip = csr_read(CSR_MIP) & (MIP_SSIP | MIP_STIP);
 	hdata->saved_medeleg = csr_read(CSR_MEDELEG);
-	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12) {
-#if __riscv_xlen == 32
-		hdata->saved_menvcfgh = csr_read(CSR_MENVCFGH);
-#endif
-		hdata->saved_menvcfg = csr_read(CSR_MENVCFG);
-	}
+	hdata->saved_mideleg = csr_read(CSR_MIDELEG);
+	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12)
+		hdata->saved_menvcfg = csr_read64(CSR_MENVCFG);
 }
 
 static void __sbi_hsm_suspend_non_ret_restore(struct sbi_scratch *scratch)
@@ -432,12 +437,9 @@ static void __sbi_hsm_suspend_non_ret_restore(struct sbi_scratch *scratch)
 	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
 							    hart_data_offset);
 
-	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12) {
-		csr_write(CSR_MENVCFG, hdata->saved_menvcfg);
-#if __riscv_xlen == 32
-		csr_write(CSR_MENVCFGH, hdata->saved_menvcfgh);
-#endif
-	}
+	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_12)
+		csr_write64(CSR_MENVCFG, hdata->saved_menvcfg);
+	csr_write(CSR_MIDELEG, hdata->saved_mideleg);
 	csr_write(CSR_MEDELEG, hdata->saved_medeleg);
 	csr_write(CSR_MIE, hdata->saved_mie);
 	csr_set(CSR_MIP, (hdata->saved_mip & (MIP_SSIP | MIP_STIP)));
@@ -453,7 +455,10 @@ void sbi_hsm_hart_resume_start(struct sbi_scratch *scratch)
 					 SBI_HSM_STATE_RESUME_PENDING))
 		sbi_hart_hang();
 
-	hsm_device_hart_resume();
+	if (sbi_system_is_suspended())
+		sbi_system_resume();
+	else
+		hsm_device_hart_resume();
 }
 
 void __noreturn sbi_hsm_hart_resume_finish(struct sbi_scratch *scratch,

lib/sbi/sbi_illegal_atomic.c

@@ -0,0 +1,664 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2025 MIPS
+ *
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_illegal_atomic.h>
+#include <sbi/sbi_illegal_insn.h>
+
+#if !defined(__riscv_atomic) && !defined(__riscv_zalrsc)
+#error "opensbi strongly relies on the A extension of RISC-V"
+#endif
+
+#ifdef __riscv_atomic
+
+int sbi_illegal_atomic(ulong insn, struct sbi_trap_regs *regs)
+{
+	return truly_illegal_insn(insn, regs);
+}
+
+#elif __riscv_zalrsc
+
+#define DEFINE_UNPRIVILEGED_LR_FUNCTION(type, aqrl, insn)			\
+	static type lr_##type##aqrl(const type *addr,				\
+				struct sbi_trap_info *trap)			\
+	{									\
+		register ulong tinfo asm("a3");					\
+		register ulong mstatus = 0;					\
+		register ulong mtvec = (ulong)sbi_hart_expected_trap;		\
+		type ret = 0;							\
+		trap->cause = 0;						\
+		asm volatile(							\
+			"add %[tinfo], %[taddr], zero\n"			\
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"	\
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"	\
+			".option push\n"					\
+			".option norvc\n"					\
+			#insn " %[ret], %[addr]\n"				\
+			".option pop\n"						\
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"		\
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"			\
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),		\
+		      [tinfo] "+&r"(tinfo), [ret] "=&r"(ret)			\
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),		\
+		      [taddr] "r"((ulong)trap)					\
+		    : "a4", "memory");						\
+		return ret;							\
+	}
+
+#define DEFINE_UNPRIVILEGED_SC_FUNCTION(type, aqrl, insn)			\
+	static type sc_##type##aqrl(type *addr, type val,			\
+				struct sbi_trap_info *trap)			\
+	{									\
+		register ulong tinfo asm("a3");					\
+		register ulong mstatus = 0;					\
+		register ulong mtvec = (ulong)sbi_hart_expected_trap;		\
+		type ret = 0;							\
+		trap->cause = 0;						\
+		asm volatile(							\
+			"add %[tinfo], %[taddr], zero\n"			\
+			"csrrw %[mtvec], " STR(CSR_MTVEC) ", %[mtvec]\n"	\
+			"csrrs %[mstatus], " STR(CSR_MSTATUS) ", %[mprv]\n"	\
+			".option push\n"					\
+			".option norvc\n"					\
+			#insn " %[ret], %[val], %[addr]\n"			\
+			".option pop\n"						\
+			"csrw " STR(CSR_MSTATUS) ", %[mstatus]\n"		\
+			"csrw " STR(CSR_MTVEC) ", %[mtvec]"			\
+		    : [mstatus] "+&r"(mstatus), [mtvec] "+&r"(mtvec),		\
+		      [tinfo] "+&r"(tinfo), [ret] "=&r"(ret)			\
+		    : [addr] "m"(*addr), [mprv] "r"(MSTATUS_MPRV),		\
+		      [val] "r"(val), [taddr] "r"((ulong)trap)			\
+		    : "a4", "memory");						\
+		return ret;							\
+	}
+
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s32, , lr.w);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s32, _aq, lr.w.aq);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s32, _rl, lr.w.rl);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s32, _aqrl, lr.w.aqrl);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s32, , sc.w);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s32, _aq, sc.w.aq);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s32, _rl, sc.w.rl);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s32, _aqrl, sc.w.aqrl);
+#if __riscv_xlen == 64
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s64, , lr.d);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s64, _aq, lr.d.aq);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s64, _rl, lr.d.rl);
+DEFINE_UNPRIVILEGED_LR_FUNCTION(s64, _aqrl, lr.d.aqrl);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s64, , sc.d);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s64, _aq, sc.d.aq);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s64, _rl, sc.d.rl);
+DEFINE_UNPRIVILEGED_SC_FUNCTION(s64, _aqrl, sc.d.aqrl);
+#endif
+
+#define DEFINE_ATOMIC_FUNCTION(name, type, func)				\
+	static int atomic_##name(ulong insn, struct sbi_trap_regs *regs)	\
+	{									\
+		struct sbi_trap_info uptrap;					\
+		ulong addr = GET_RS1(insn, regs);				\
+		ulong val = GET_RS2(insn, regs);				\
+		ulong rd_val = 0;						\
+		ulong fail = 1;							\
+		while (fail) {							\
+			rd_val = lr_##type((void *)addr, &uptrap);		\
+			if (uptrap.cause) {					\
+				return sbi_trap_redirect(regs, &uptrap);	\
+			}							\
+			fail = sc_##type((void *)addr, func, &uptrap);	\
+			if (uptrap.cause) {					\
+				return sbi_trap_redirect(regs, &uptrap);	\
+			}							\
+		}								\
+		SET_RD(insn, regs, rd_val);					\
+		regs->mepc += 4;						\
+		return 0;							\
+	}
+
+DEFINE_ATOMIC_FUNCTION(add_w, s32, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_w_aq, s32_aq, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_w_rl, s32_rl, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_w_aqrl, s32_aqrl, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(and_w, s32, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_w_aq, s32_aq, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_w_rl, s32_rl, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_w_aqrl, s32_aqrl, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(or_w, s32, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_w_aq, s32_aq, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_w_rl, s32_rl, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_w_aqrl, s32_aqrl, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(xor_w, s32, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_w_aq, s32_aq, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_w_rl, s32_rl, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_w_aqrl, s32_aqrl, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(swap_w, s32, val);
+DEFINE_ATOMIC_FUNCTION(swap_w_aq, s32_aq, val);
+DEFINE_ATOMIC_FUNCTION(swap_w_rl, s32_rl, val);
+DEFINE_ATOMIC_FUNCTION(swap_w_aqrl, s32_aqrl, val);
+DEFINE_ATOMIC_FUNCTION(max_w, s32, (s32)rd_val > (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_w_aq, s32_aq, (s32)rd_val > (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_w_rl, s32_rl, (s32)rd_val > (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_w_aqrl, s32_aqrl, (s32)rd_val > (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_w, s32, (u32)rd_val > (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_w_aq, s32_aq, (u32)rd_val > (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_w_rl, s32_rl, (u32)rd_val > (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_w_aqrl, s32_aqrl, (u32)rd_val > (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_w, s32, (s32)rd_val < (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_w_aq, s32_aq, (s32)rd_val < (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_w_rl, s32_rl, (s32)rd_val < (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_w_aqrl, s32_aqrl, (s32)rd_val < (s32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_w, s32, (u32)rd_val < (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_w_aq, s32_aq, (u32)rd_val < (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_w_rl, s32_rl, (u32)rd_val < (u32)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_w_aqrl, s32_aqrl, (u32)rd_val < (u32)val ? rd_val : val);
+
+#if __riscv_xlen == 64
+DEFINE_ATOMIC_FUNCTION(add_d, s64, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_d_aq, s64_aq, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_d_rl, s64_rl, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(add_d_aqrl, s64_aqrl, rd_val + val);
+DEFINE_ATOMIC_FUNCTION(and_d, s64, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_d_aq, s64_aq, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_d_rl, s64_rl, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(and_d_aqrl, s64_aqrl, rd_val & val);
+DEFINE_ATOMIC_FUNCTION(or_d, s64, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_d_aq, s64_aq, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_d_rl, s64_rl, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(or_d_aqrl, s64_aqrl, rd_val | val);
+DEFINE_ATOMIC_FUNCTION(xor_d, s64, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_d_aq, s64_aq, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_d_rl, s64_rl, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(xor_d_aqrl, s64_aqrl, rd_val ^ val);
+DEFINE_ATOMIC_FUNCTION(swap_d, s64, val);
+DEFINE_ATOMIC_FUNCTION(swap_d_aq, s64_aq, val);
+DEFINE_ATOMIC_FUNCTION(swap_d_rl, s64_rl, val);
+DEFINE_ATOMIC_FUNCTION(swap_d_aqrl, s64_aqrl, val);
+DEFINE_ATOMIC_FUNCTION(max_d, s64, (s64)rd_val > (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_d_aq, s64_aq, (s64)rd_val > (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_d_rl, s64_rl, (s64)rd_val > (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(max_d_aqrl, s64_aqrl, (s64)rd_val > (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_d, s64, (u64)rd_val > (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_d_aq, s64_aq, (u64)rd_val > (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_d_rl, s64_rl, (u64)rd_val > (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(maxu_d_aqrl, s64_aqrl, (u64)rd_val > (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_d, s64, (s64)rd_val < (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_d_aq, s64_aq, (s64)rd_val < (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_d_rl, s64_rl, (s64)rd_val < (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(min_d_aqrl, s64_aqrl, (s64)rd_val < (s64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_d, s64, (u64)rd_val < (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_d_aq, s64_aq, (u64)rd_val < (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_d_rl, s64_rl, (u64)rd_val < (u64)val ? rd_val : val);
+DEFINE_ATOMIC_FUNCTION(minu_d_aqrl, s64_aqrl, (u64)rd_val < (u64)val ? rd_val : val);
+#endif
+
+static const illegal_insn_func amoadd_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_add_w, /* 8 */
+	atomic_add_w_rl, /* 9 */
+	atomic_add_w_aq, /* 10 */
+	atomic_add_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_add_d, /* 12 */
+	atomic_add_d_rl, /* 13 */
+	atomic_add_d_aq, /* 14 */
+	atomic_add_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amoswap_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_swap_w, /* 8 */
+	atomic_swap_w_rl, /* 9 */
+	atomic_swap_w_aq, /* 10 */
+	atomic_swap_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_swap_d, /* 12 */
+	atomic_swap_d_rl, /* 13 */
+	atomic_swap_d_aq, /* 14 */
+	atomic_swap_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amoxor_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_xor_w, /* 8 */
+	atomic_xor_w_rl, /* 9 */
+	atomic_xor_w_aq, /* 10 */
+	atomic_xor_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_xor_d, /* 12 */
+	atomic_xor_d_rl, /* 13 */
+	atomic_xor_d_aq, /* 14 */
+	atomic_xor_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amoor_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_or_w, /* 8 */
+	atomic_or_w_rl, /* 9 */
+	atomic_or_w_aq, /* 10 */
+	atomic_or_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_or_d, /* 12 */
+	atomic_or_d_rl, /* 13 */
+	atomic_or_d_aq, /* 14 */
+	atomic_or_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amoand_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_and_w, /* 8 */
+	atomic_and_w_rl, /* 9 */
+	atomic_and_w_aq, /* 10 */
+	atomic_and_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_and_d, /* 12 */
+	atomic_and_d_rl, /* 13 */
+	atomic_and_d_aq, /* 14 */
+	atomic_and_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amomin_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_min_w, /* 8 */
+	atomic_min_w_rl, /* 9 */
+	atomic_min_w_aq, /* 10 */
+	atomic_min_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_min_d, /* 12 */
+	atomic_min_d_rl, /* 13 */
+	atomic_min_d_aq, /* 14 */
+	atomic_min_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amomax_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_max_w, /* 8 */
+	atomic_max_w_rl, /* 9 */
+	atomic_max_w_aq, /* 10 */
+	atomic_max_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_max_d, /* 12 */
+	atomic_max_d_rl, /* 13 */
+	atomic_max_d_aq, /* 14 */
+	atomic_max_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amominu_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_minu_w, /* 8 */
+	atomic_minu_w_rl, /* 9 */
+	atomic_minu_w_aq, /* 10 */
+	atomic_minu_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_minu_d, /* 12 */
+	atomic_minu_d_rl, /* 13 */
+	atomic_minu_d_aq, /* 14 */
+	atomic_minu_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static const illegal_insn_func amomaxu_table[32] = {
+	truly_illegal_insn, /* 0 */
+	truly_illegal_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	truly_illegal_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	atomic_maxu_w, /* 8 */
+	atomic_maxu_w_rl, /* 9 */
+	atomic_maxu_w_aq, /* 10 */
+	atomic_maxu_w_aqrl, /* 11 */
+#if __riscv_xlen == 64
+	atomic_maxu_d, /* 12 */
+	atomic_maxu_d_rl, /* 13 */
+	atomic_maxu_d_aq, /* 14 */
+	atomic_maxu_d_aqrl, /* 15 */
+#else
+	truly_illegal_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+#endif
+	truly_illegal_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	truly_illegal_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	truly_illegal_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	truly_illegal_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn, /* 31 */
+};
+
+static int amoadd_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amoadd_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amoswap_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amoswap_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amoxor_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amoxor_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amoor_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amoor_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amoand_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amoand_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amomin_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amomin_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amomax_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amomax_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amominu_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amominu_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static int amomaxu_insn(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amomaxu_table[(GET_FUNC3(insn) << 2) + GET_AQRL(insn)](insn, regs);
+}
+
+static const illegal_insn_func amo_insn_table[32] = {
+	amoadd_insn, /* 0 */
+	amoswap_insn, /* 1 */
+	truly_illegal_insn, /* 2 */
+	truly_illegal_insn, /* 3 */
+	amoxor_insn, /* 4 */
+	truly_illegal_insn, /* 5 */
+	truly_illegal_insn, /* 6 */
+	truly_illegal_insn, /* 7 */
+	amoor_insn, /* 8 */
+	truly_illegal_insn, /* 9 */
+	truly_illegal_insn, /* 10 */
+	truly_illegal_insn, /* 11 */
+	amoand_insn, /* 12 */
+	truly_illegal_insn, /* 13 */
+	truly_illegal_insn, /* 14 */
+	truly_illegal_insn, /* 15 */
+	amomin_insn, /* 16 */
+	truly_illegal_insn, /* 17 */
+	truly_illegal_insn, /* 18 */
+	truly_illegal_insn, /* 19 */
+	amomax_insn, /* 20 */
+	truly_illegal_insn, /* 21 */
+	truly_illegal_insn, /* 22 */
+	truly_illegal_insn, /* 23 */
+	amominu_insn, /* 24 */
+	truly_illegal_insn, /* 25 */
+	truly_illegal_insn, /* 26 */
+	truly_illegal_insn, /* 27 */
+	amomaxu_insn, /* 28 */
+	truly_illegal_insn, /* 29 */
+	truly_illegal_insn, /* 30 */
+	truly_illegal_insn  /* 31 */
+};
+
+int sbi_illegal_atomic(ulong insn, struct sbi_trap_regs *regs)
+{
+	return amo_insn_table[(insn >> 27) & 0x1f](insn, regs);
+}
+
+#else
+#error "need a or zalrsc"
+#endif

lib/sbi/sbi_illegal_insn.c

@@ -13,15 +13,14 @@
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_emulate_csr.h>
 #include <sbi/sbi_error.h>
+#include <sbi/sbi_illegal_atomic.h>
 #include <sbi/sbi_illegal_insn.h>
 #include <sbi/sbi_pmu.h>
 #include <sbi/sbi_trap.h>
 #include <sbi/sbi_unpriv.h>
 #include <sbi/sbi_console.h>
 
-typedef int (*illegal_insn_func)(ulong insn, struct sbi_trap_regs *regs);
-
-static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
+int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
 {
 	struct sbi_trap_info trap;
 
@@ -123,7 +122,7 @@ static const illegal_insn_func illegal_insn_table[32] = {
 	truly_illegal_insn, /* 8 */
 	truly_illegal_insn, /* 9 */
 	truly_illegal_insn, /* 10 */
-	truly_illegal_insn, /* 11 */
+	sbi_illegal_atomic, /* 11 */
 	truly_illegal_insn, /* 12 */
 	truly_illegal_insn, /* 13 */
 	truly_illegal_insn, /* 14 */

lib/sbi/sbi_init.c

@@ -13,6 +13,7 @@
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_cppc.h>
 #include <sbi/sbi_domain.h>
+#include <sbi/sbi_double_trap.h>
 #include <sbi/sbi_ecall.h>
 #include <sbi/sbi_fwft.h>
 #include <sbi/sbi_hart.h>
@@ -160,7 +161,7 @@ static void sbi_boot_print_domains(struct sbi_scratch *scratch)
 static void sbi_boot_print_hart(struct sbi_scratch *scratch, u32 hartid)
 {
 	int xlen;
-	char str[128];
+	char str[256];
 	const struct sbi_domain *dom = sbi_domain_thishart_ptr();
 
 	if (scratch->options & SBI_SCRATCH_NO_BOOT_PRINTS)
@@ -266,12 +267,6 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	if (rc)
 		sbi_hart_hang();
 
-	rc = sbi_sse_init(scratch, true);
-	if (rc) {
-		sbi_printf("%s: sse init failed (error %d)\n", __func__, rc);
-		sbi_hart_hang();
-	}
-
 	rc = sbi_pmu_init(scratch, true);
 	if (rc) {
 		sbi_printf("%s: pmu init failed (error %d)\n",
@@ -285,6 +280,8 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 
 	sbi_boot_print_banner(scratch);
 
+	sbi_double_trap_init(scratch);
+
 	rc = sbi_irqchip_init(scratch, true);
 	if (rc) {
 		sbi_printf("%s: irqchip init failed (error %d)\n",
@@ -321,13 +318,13 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 		sbi_printf("%s: mpxy init failed (error %d)\n", __func__, rc);
 		sbi_hart_hang();
 	}
+
 	/*
-	 * Note: Finalize domains after HSM initialization so that we
-	 * can startup non-root domains.
+	 * Note: Finalize domains after HSM initialization
 	 * Note: Finalize domains before HART PMP configuration so
 	 * that we use correct domain for configuring PMP.
 	 */
-	rc = sbi_domain_finalize(scratch, hartid);
+	rc = sbi_domain_finalize(scratch);
 	if (rc) {
 		sbi_printf("%s: domain finalize failed (error %d)\n",
 			   __func__, rc);
@@ -346,6 +343,16 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 		sbi_hart_hang();
 	}
 
+	/*
+	 * Note: SSE events callbacks can be registered by other drivers so
+	 * sbi_sse_init() needs to be called after all drivers have been probed.
+	 */
+	rc = sbi_sse_init(scratch, true);
+	if (rc) {
+		sbi_printf("%s: sse init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
 	/*
 	 * Note: Ecall initialization should be after platform final
 	 * initialization so that all available platform devices are
@@ -365,6 +372,17 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 
 	run_all_tests();
 
+	/*
+	 * Note: Startup domains after all initialization are done
+	 * otherwise boot HART of non-root domain can crash.
+	 */
+	rc = sbi_domain_startup(scratch, hartid);
+	if (rc) {
+		sbi_printf("%s: domain startup failed (error %d)\n",
+			   __func__, rc);
+		sbi_hart_hang();
+	}
+
 	/*
 	 * Configure PMP at last because if SMEPMP is detected,
 	 * M-mode access to the S/U space will be rescinded.
@@ -408,10 +426,6 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	if (rc)
 		sbi_hart_hang();
 
-	rc = sbi_sse_init(scratch, false);
-	if (rc)
-		sbi_hart_hang();
-
 	rc = sbi_pmu_init(scratch, false);
 	if (rc)
 		sbi_hart_hang();
@@ -444,6 +458,10 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	if (rc)
 		sbi_hart_hang();
 
+	rc = sbi_sse_init(scratch, false);
+	if (rc)
+		sbi_hart_hang();
+
 	/*
 	 * Configure PMP at last because if SMEPMP is detected,
 	 * M-mode access to the S/U space will be rescinded.
@@ -489,7 +507,7 @@ static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
 	if (hstate == SBI_HSM_STATE_SUSPENDED) {
 		init_warm_resume(scratch, hartid);
 	} else {
-		sbi_ipi_raw_clear();
+		sbi_ipi_raw_clear(true);
 		init_warm_startup(scratch, hartid);
 	}
 }
@@ -561,6 +579,19 @@ void __noreturn sbi_init(struct sbi_scratch *scratch)
 		init_warmboot(scratch, hartid);
 }
 
+void sbi_revert_entry_count(struct sbi_scratch *scratch)
+{
+	unsigned long *entry_count, *init_count;
+
+	if (!entry_count_offset || !init_count_offset)
+		sbi_hart_hang();
+
+	entry_count = sbi_scratch_offset_ptr(scratch, entry_count_offset);
+	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
+
+	*entry_count = *init_count;
+}
+
 unsigned long sbi_entry_count(u32 hartindex)
 {
 	struct sbi_scratch *scratch;

lib/sbi/sbi_ipi.c

@@ -15,9 +15,11 @@
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_heap.h>
 #include <sbi/sbi_hsm.h>
 #include <sbi/sbi_init.h>
 #include <sbi/sbi_ipi.h>
+#include <sbi/sbi_list.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_pmu.h>
 #include <sbi/sbi_string.h>
@@ -32,8 +34,14 @@ _Static_assert(
 	"type of sbi_ipi_data.ipi_type has changed, please redefine SBI_IPI_EVENT_MAX"
 	);
 
+struct sbi_ipi_device_node {
+	struct sbi_dlist head;
+	const struct sbi_ipi_device *dev;
+};
+
 static unsigned long ipi_data_off;
 static const struct sbi_ipi_device *ipi_dev = NULL;
+static SBI_LIST_HEAD(ipi_dev_node_list);
 static const struct sbi_ipi_event_ops *ipi_ops_array[SBI_IPI_EVENT_MAX];
 
 static int sbi_ipi_send(struct sbi_scratch *scratch, u32 remote_hartindex,
@@ -80,7 +88,7 @@ static int sbi_ipi_send(struct sbi_scratch *scratch, u32 remote_hartindex,
 	 */
 	if (!__atomic_fetch_or(&ipi_data->ipi_type,
 				BIT(event), __ATOMIC_RELAXED))
-		ret = sbi_ipi_raw_send(remote_hartindex);
+		ret = sbi_ipi_raw_send(remote_hartindex, false);
 
 	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_IPI_SENT);
 
@@ -116,6 +124,11 @@ int sbi_ipi_send_many(ulong hmask, ulong hbase, u32 event, void *data)
 	struct sbi_domain *dom = sbi_domain_thishart_ptr();
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 
+	if (hmask == 0 && hbase != -1UL) {
+		/* Nothing to do, but it's not an error either. */
+		return 0;
+	}
+
 	/* Find the target harts */
 	rc = sbi_hsm_hart_interruptible_mask(dom, &target_mask);
 	if (rc)
@@ -123,6 +136,7 @@ int sbi_ipi_send_many(ulong hmask, ulong hbase, u32 event, void *data)
 
 	if (hbase != -1UL) {
 		struct sbi_hartmask tmp_mask = { 0 };
+		int count = sbi_popcount(hmask);
 
 		for (i = hbase; hmask; i++, hmask >>= 1) {
 			if (hmask & 1UL)
@@ -130,6 +144,9 @@ int sbi_ipi_send_many(ulong hmask, ulong hbase, u32 event, void *data)
 		}
 
 		sbi_hartmask_and(&target_mask, &target_mask, &tmp_mask);
+
+		if (sbi_hartmask_weight(&target_mask) != count)
+			return SBI_EINVAL;
 	}
 
 	/* Send IPIs */
@@ -239,7 +256,7 @@ void sbi_ipi_process(void)
 			sbi_scratch_offset_ptr(scratch, ipi_data_off);
 
 	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_IPI_RECVD);
-	sbi_ipi_raw_clear();
+	sbi_ipi_raw_clear(false);
 
 	ipi_type = atomic_raw_xchg_ulong(&ipi_data->ipi_type, 0);
 	ipi_event = 0;
@@ -254,8 +271,10 @@ void sbi_ipi_process(void)
 	}
 }
 
-int sbi_ipi_raw_send(u32 hartindex)
+int sbi_ipi_raw_send(u32 hartindex, bool all_devices)
 {
+	struct sbi_ipi_device_node *entry;
+
 	if (!ipi_dev || !ipi_dev->ipi_send)
 		return SBI_EINVAL;
 
@@ -270,14 +289,31 @@ int sbi_ipi_raw_send(u32 hartindex)
 	 */
 	wmb();
 
-	ipi_dev->ipi_send(hartindex);
+	if (all_devices) {
+		sbi_list_for_each_entry(entry, &ipi_dev_node_list, head) {
+			if (entry->dev->ipi_send)
+				entry->dev->ipi_send(hartindex);
+		}
+	} else {
+		ipi_dev->ipi_send(hartindex);
+	}
+
 	return 0;
 }
 
-void sbi_ipi_raw_clear(void)
+void sbi_ipi_raw_clear(bool all_devices)
 {
-	if (ipi_dev && ipi_dev->ipi_clear)
-		ipi_dev->ipi_clear();
+	struct sbi_ipi_device_node *entry;
+
+	if (all_devices) {
+		sbi_list_for_each_entry(entry, &ipi_dev_node_list, head) {
+			if (entry->dev->ipi_clear)
+				entry->dev->ipi_clear();
+		}
+	} else {
+		if (ipi_dev && ipi_dev->ipi_clear)
+			ipi_dev->ipi_clear();
+	}
 
 	/*
 	 * Ensure that memory or MMIO writes after this
@@ -296,12 +332,22 @@ const struct sbi_ipi_device *sbi_ipi_get_device(void)
 	return ipi_dev;
 }
 
-void sbi_ipi_set_device(const struct sbi_ipi_device *dev)
+void sbi_ipi_add_device(const struct sbi_ipi_device *dev)
 {
-	if (!dev || ipi_dev)
+	struct sbi_ipi_device_node *entry;
+
+	if (!dev)
 		return;
 
-	ipi_dev = dev;
+	entry = sbi_zalloc(sizeof(*entry));
+	if (!entry)
+		return;
+	SBI_INIT_LIST_HEAD(&entry->head);
+	entry->dev = dev;
+	sbi_list_add_tail(&entry->head, &ipi_dev_node_list);
+
+	if (!ipi_dev || ipi_dev->rating < dev->rating)
+		ipi_dev = dev;
 }
 
 int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot)
@@ -321,11 +367,6 @@ int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot)
 		if (ret < 0)
 			return ret;
 		ipi_halt_event = ret;
-
-		/* Initialize platform IPI support */
-		ret = sbi_platform_ipi_init(sbi_platform_ptr(scratch));
-		if (ret)
-			return ret;
 	} else {
 		if (!ipi_data_off)
 			return SBI_ENOMEM;
@@ -338,7 +379,7 @@ int sbi_ipi_init(struct sbi_scratch *scratch, bool cold_boot)
 	ipi_data->ipi_type = 0x00;
 
 	/* Clear any pending IPIs for the current hart */
-	sbi_ipi_raw_clear();
+	sbi_ipi_raw_clear(true);
 
 	/* Enable software interrupts */
 	csr_set(CSR_MIE, MIP_MSIP);

lib/sbi/sbi_mpxy.c

@@ -11,6 +11,7 @@
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_heap.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_mpxy.h>
 #include <sbi/sbi_scratch.h>
@@ -19,8 +20,8 @@
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_byteorder.h>
 
-/** Offset of pointer to MPXY state in scratch space */
-static unsigned long mpxy_state_offset;
+/** Shared memory size across all harts */
+static unsigned long mpxy_shmem_size = PAGE_SIZE;
 
 /** List of MPXY proxy channels */
 static SBI_LIST_HEAD(mpxy_channel_list);
@@ -43,17 +44,17 @@ static SBI_LIST_HEAD(mpxy_channel_list);
 #define CAP_EVENTSSTATE_POS	2
 #define CAP_EVENTSSTATE_MASK	(1U << CAP_EVENTSSTATE_POS)
 
-/** Channel Capability - Get Notification function support */
-#define CAP_GET_NOTIFICATIONS_POS	3
-#define CAP_GET_NOTIFICATIONS_MASK	(1U << CAP_GET_NOTIFICATIONS_POS)
+/** Channel Capability - Send Message With Response function support */
+#define CAP_SEND_MSG_WITH_RESP_POS	3
+#define CAP_SEND_MSG_WITH_RESP_MASK	(1U << CAP_SEND_MSG_WITH_RESP_POS)
 
 /** Channel Capability - Send Message Without Response function support */
 #define CAP_SEND_MSG_WITHOUT_RESP_POS	4
 #define CAP_SEND_MSG_WITHOUT_RESP_MASK	(1U << CAP_SEND_MSG_WITHOUT_RESP_POS)
 
-/** Channel Capability - Send Message With Response function support */
-#define CAP_SEND_MSG_WITH_RESP_POS	5
-#define CAP_SEND_MSG_WITH_RESP_MASK	(1U << CAP_SEND_MSG_WITH_RESP_POS)
+/** Channel Capability - Get Notification function support */
+#define CAP_GET_NOTIFICATIONS_POS	5
+#define CAP_GET_NOTIFICATIONS_MASK	(1U << CAP_GET_NOTIFICATIONS_POS)
 
 /** Helpers to enable/disable channel capability bits
  * _c: capability variable
@@ -63,17 +64,10 @@ static SBI_LIST_HEAD(mpxy_channel_list);
 #define CAP_DISABLE(_c, _m)	INSERT_FIELD(_c, _m, 0)
 #define CAP_GET(_c, _m)		EXTRACT_FIELD(_c, _m)
 
-#if __riscv_xlen == 64
 #define SHMEM_PHYS_ADDR(_hi, _lo) (_lo)
-#elif __riscv_xlen == 32
-#define SHMEM_PHYS_ADDR(_hi, _lo) (((u64)(_hi) << 32) | (_lo))
-#else
-#error "Undefined XLEN"
-#endif
 
 /** Per hart shared memory */
 struct mpxy_shmem {
-	unsigned long shmem_size;
 	unsigned long shmem_addr_lo;
 	unsigned long shmem_addr_hi;
 };
@@ -87,10 +81,17 @@ struct mpxy_state {
 	struct mpxy_shmem shmem;
 };
 
+static struct mpxy_state *sbi_domain_get_mpxy_state(struct sbi_domain *dom,
+						    u32 hartindex);
+
+/** Macro to obtain the current hart's MPXY state pointer in current domain */
+#define sbi_domain_mpxy_state_thishart_ptr()			\
+	sbi_domain_get_mpxy_state(sbi_domain_thishart_ptr(),	\
+				  current_hartindex())
+
 /** Disable hart shared memory */
 static inline void sbi_mpxy_shmem_disable(struct mpxy_state *ms)
 {
-	ms->shmem.shmem_size = 0;
 	ms->shmem.shmem_addr_lo = INVALID_ADDR;
 	ms->shmem.shmem_addr_hi = INVALID_ADDR;
 }
@@ -170,9 +171,8 @@ bool sbi_mpxy_channel_available(void)
 
 static void mpxy_std_attrs_init(struct sbi_mpxy_channel *channel)
 {
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
 	u32 capability = 0;
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
 
 	/* Reset values */
 	channel->attrs.msi_control = 0;
@@ -228,37 +228,113 @@ int sbi_mpxy_register_channel(struct sbi_mpxy_channel *channel)
 	/* Initialize channel specific attributes */
 	mpxy_std_attrs_init(channel);
 
-	SBI_INIT_LIST_HEAD(&channel->head);
+	/* Update shared memory size if required */
+	if (mpxy_shmem_size < channel->attrs.msg_data_maxlen) {
+		mpxy_shmem_size = channel->attrs.msg_data_maxlen;
+		mpxy_shmem_size = (mpxy_shmem_size + (PAGE_SIZE - 1)) / PAGE_SIZE;
+	}
+
 	sbi_list_add_tail(&channel->head, &mpxy_channel_list);
 
 	return SBI_OK;
 }
 
+/** Setup per domain MPXY state data */
+static int domain_mpxy_state_data_setup(struct sbi_domain *dom,
+					struct sbi_domain_data *data,
+					void *data_ptr)
+{
+	struct mpxy_state **dom_hartindex_to_mpxy_state_table = data_ptr;
+	struct mpxy_state *ms;
+	u32 i;
+
+	sbi_hartmask_for_each_hartindex(i, dom->possible_harts) {
+		ms = sbi_zalloc(sizeof(*ms));
+		if (!ms)
+			return SBI_ENOMEM;
+
+		/*
+		 * TODO: Proper support for checking msi support from
+		 * platform. Currently disable msi and sse and use
+		 * polling
+		 */
+		ms->msi_avail = false;
+		ms->sse_avail = false;
+
+		sbi_mpxy_shmem_disable(ms);
+
+		dom_hartindex_to_mpxy_state_table[i] = ms;
+	}
+
+	return 0;
+}
+
+/** Cleanup per domain MPXY state data */
+static void domain_mpxy_state_data_cleanup(struct sbi_domain *dom,
+					   struct sbi_domain_data *data,
+					   void *data_ptr)
+{
+	struct mpxy_state **dom_hartindex_to_mpxy_state_table = data_ptr;
+	u32 i;
+
+	sbi_hartmask_for_each_hartindex(i, dom->possible_harts)
+		sbi_free(dom_hartindex_to_mpxy_state_table[i]);
+}
+
+static struct sbi_domain_data dmspriv = {
+	.data_setup = domain_mpxy_state_data_setup,
+	.data_cleanup = domain_mpxy_state_data_cleanup,
+};
+
+/**
+ * Get per-domain MPXY state pointer for a given domain and HART index
+ * @param dom pointer to domain
+ * @param hartindex the HART index
+ *
+ * @return per-domain MPXY state pointer for given HART index
+ */
+static struct mpxy_state *sbi_domain_get_mpxy_state(struct sbi_domain *dom,
+						    u32 hartindex)
+{
+	struct mpxy_state **dom_hartindex_to_mpxy_state_table;
+
+	dom_hartindex_to_mpxy_state_table = sbi_domain_data_ptr(dom, &dmspriv);
+	if (!dom_hartindex_to_mpxy_state_table ||
+	    !sbi_hartindex_valid(hartindex))
+		return NULL;
+
+	return dom_hartindex_to_mpxy_state_table[hartindex];
+}
+
 int sbi_mpxy_init(struct sbi_scratch *scratch)
 {
-	mpxy_state_offset = sbi_scratch_alloc_type_offset(struct mpxy_state);
-	if (!mpxy_state_offset)
-		return SBI_ENOMEM;
+	int ret;
 
 	/**
-	 * TODO: Proper support for checking msi support from platform.
-	 * Currently disable msi and sse and use polling
+	 * Allocate per-domain and per-hart MPXY state data.
+	 * The data type is "struct mpxy_state **" whose memory space will be
+	 * dynamically allocated by domain_setup_data_one() and
+	 * domain_mpxy_state_data_setup(). Calculate needed size of memory space
+	 * here.
 	 */
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
-	ms->msi_avail = false;
-	ms->sse_avail = false;
-
-	sbi_mpxy_shmem_disable(ms);
+	dmspriv.data_size = sizeof(struct mpxy_state *) * sbi_hart_count();
+	ret = sbi_domain_register_data(&dmspriv);
+	if (ret)
+		return ret;
 
 	return sbi_platform_mpxy_init(sbi_platform_ptr(scratch));
 }
 
-int sbi_mpxy_set_shmem(unsigned long shmem_size, unsigned long shmem_phys_lo,
-		       unsigned long shmem_phys_hi, unsigned long flags)
+unsigned long sbi_mpxy_get_shmem_size(void)
 {
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
+	return mpxy_shmem_size;
+}
+
+int sbi_mpxy_set_shmem(unsigned long shmem_phys_lo,
+		       unsigned long shmem_phys_hi,
+		       unsigned long flags)
+{
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
 	unsigned long *ret_buf;
 
 	/** Disable shared memory if both hi and lo have all bit 1s */
@@ -272,13 +348,26 @@ int sbi_mpxy_set_shmem(unsigned long shmem_size, unsigned long shmem_phys_lo,
 		return SBI_ERR_INVALID_PARAM;
 
 	/** Check shared memory size and address aligned to 4K Page */
-	if (!shmem_size || (shmem_size & ~PAGE_MASK) ||
-	    (shmem_phys_lo & ~PAGE_MASK))
+	if (shmem_phys_lo & ~PAGE_MASK)
 		return SBI_ERR_INVALID_PARAM;
 
+	/*
+	* On RV32, the M-mode can only access the first 4GB of
+	* the physical address space because M-mode does not have
+	* MMU to access full 34-bit physical address space.
+	* So fail if the upper 32 bits of the physical address
+	* is non-zero on RV32.
+	*
+	* On RV64, kernel sets upper 64bit address part to zero.
+	* So fail if the upper 64bit of the physical address
+	* is non-zero on RV64.
+	*/
+	if (shmem_phys_hi)
+		return SBI_ERR_INVALID_ADDRESS;
+
 	if (!sbi_domain_check_addr_range(sbi_domain_thishart_ptr(),
 				SHMEM_PHYS_ADDR(shmem_phys_hi, shmem_phys_lo),
-				shmem_size, PRV_S,
+				mpxy_shmem_size, PRV_S,
 				SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
 		return SBI_ERR_INVALID_ADDRESS;
 
@@ -286,15 +375,13 @@ int sbi_mpxy_set_shmem(unsigned long shmem_size, unsigned long shmem_phys_lo,
 	if (flags == SBI_EXT_MPXY_SHMEM_FLAG_OVERWRITE_RETURN) {
 		ret_buf = (unsigned long *)(ulong)SHMEM_PHYS_ADDR(shmem_phys_hi,
 								  shmem_phys_lo);
-		sbi_hart_map_saddr((unsigned long)ret_buf, shmem_size);
-		ret_buf[0] = cpu_to_lle(ms->shmem.shmem_size);
-		ret_buf[1] = cpu_to_lle(ms->shmem.shmem_addr_lo);
-		ret_buf[2] = cpu_to_lle(ms->shmem.shmem_addr_hi);
+		sbi_hart_map_saddr((unsigned long)ret_buf, mpxy_shmem_size);
+		ret_buf[0] = cpu_to_lle(ms->shmem.shmem_addr_lo);
+		ret_buf[1] = cpu_to_lle(ms->shmem.shmem_addr_hi);
 		sbi_hart_unmap_saddr();
 	}
 
 	/** Setup the new shared memory */
-	ms->shmem.shmem_size	= shmem_size;
 	ms->shmem.shmem_addr_lo = shmem_phys_lo;
 	ms->shmem.shmem_addr_hi = shmem_phys_hi;
 
@@ -303,15 +390,12 @@ int sbi_mpxy_set_shmem(unsigned long shmem_size, unsigned long shmem_phys_lo,
 
 int sbi_mpxy_get_channel_ids(u32 start_index)
 {
-	u32 node_index = 0, node_ret = 0;
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
 	u32 remaining, returned, max_channelids;
+	u32 node_index = 0, node_ret = 0;
+	struct sbi_mpxy_channel *channel;
 	u32 channels_count = 0;
 	u32 *shmem_base;
-	struct sbi_mpxy_channel *channel;
-
-	/* Check if the shared memory is being setup or not. */
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
 
 	if (!mpxy_shmem_enabled(ms))
 		return SBI_ERR_NO_SHMEM;
@@ -323,12 +407,11 @@ int sbi_mpxy_get_channel_ids(u32 start_index)
 		return SBI_ERR_INVALID_PARAM;
 
 	shmem_base = hart_shmem_base(ms);
-	sbi_hart_map_saddr((unsigned long)hart_shmem_base(ms),
-				ms->shmem.shmem_size);
+	sbi_hart_map_saddr((unsigned long)hart_shmem_base(ms), mpxy_shmem_size);
 
 	/** number of channel ids which can be stored in shmem adjusting
 	 * for remaining and returned fields */
-	max_channelids = (ms->shmem.shmem_size / sizeof(u32)) - 2;
+	max_channelids = (mpxy_shmem_size / sizeof(u32)) - 2;
 	/* total remaining from the start index */
 	remaining = channels_count - start_index;
 	/* how many can be returned */
@@ -358,13 +441,11 @@ int sbi_mpxy_get_channel_ids(u32 start_index)
 
 int sbi_mpxy_read_attrs(u32 channel_id, u32 base_attr_id, u32 attr_count)
 {
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
 	int ret = SBI_SUCCESS;
 	u32 *attr_ptr, end_id;
 	void *shmem_base;
 
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
-
 	if (!mpxy_shmem_enabled(ms))
 		return SBI_ERR_NO_SHMEM;
 
@@ -378,14 +459,13 @@ int sbi_mpxy_read_attrs(u32 channel_id, u32 base_attr_id, u32 attr_count)
 		return SBI_ERR_INVALID_PARAM;
 
 	/* Sanity check for base_attr_id and attr_count */
-	if (!attr_count || (attr_count > (ms->shmem.shmem_size / ATTR_SIZE)))
+	if (!attr_count || (attr_count > (mpxy_shmem_size / ATTR_SIZE)))
 		return SBI_ERR_INVALID_PARAM;
 
 	shmem_base = hart_shmem_base(ms);
 	end_id = base_attr_id + attr_count - 1;
 
-	sbi_hart_map_saddr((unsigned long)hart_shmem_base(ms),
-				ms->shmem.shmem_size);
+	sbi_hart_map_saddr((unsigned long)hart_shmem_base(ms), mpxy_shmem_size);
 
 	/* Standard attributes range check */
 	if (mpxy_is_std_attr(base_attr_id)) {
@@ -442,8 +522,8 @@ out:
 static int mpxy_check_write_std_attr(struct sbi_mpxy_channel *channel,
 				     u32 attr_id, u32 attr_val)
 {
-	int ret = SBI_SUCCESS;
 	struct sbi_mpxy_channel_attrs *attrs = &channel->attrs;
+	int ret = SBI_SUCCESS;
 
 	switch(attr_id) {
 	case SBI_MPXY_ATTR_MSI_CONTROL:
@@ -475,11 +555,9 @@ static int mpxy_check_write_std_attr(struct sbi_mpxy_channel *channel,
  * Write the attribute value
  */
 static void mpxy_write_std_attr(struct sbi_mpxy_channel *channel, u32 attr_id,
-			        u32 attr_val)
+				u32 attr_val)
 {
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
-
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
 	struct sbi_mpxy_channel_attrs *attrs = &channel->attrs;
 
 	switch(attr_id) {
@@ -513,17 +591,16 @@ static void mpxy_write_std_attr(struct sbi_mpxy_channel *channel, u32 attr_id,
 
 int sbi_mpxy_write_attrs(u32 channel_id, u32 base_attr_id, u32 attr_count)
 {
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
+	u32 *mem_ptr, attr_id, end_id, attr_val;
+	struct sbi_mpxy_channel *channel;
 	int ret, mem_idx;
 	void *shmem_base;
-	u32 *mem_ptr, attr_id, end_id, attr_val;
-
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
 
 	if (!mpxy_shmem_enabled(ms))
 		return SBI_ERR_NO_SHMEM;
 
-	struct sbi_mpxy_channel *channel = mpxy_find_channel(channel_id);
+	channel = mpxy_find_channel(channel_id);
 	if (!channel)
 		return SBI_ERR_NOT_SUPPORTED;
 
@@ -533,13 +610,13 @@ int sbi_mpxy_write_attrs(u32 channel_id, u32 base_attr_id, u32 attr_count)
 		return SBI_ERR_INVALID_PARAM;
 
 	/* Sanity check for base_attr_id and attr_count */
-	if (!attr_count || (attr_count > (ms->shmem.shmem_size / ATTR_SIZE)))
+	if (!attr_count || (attr_count > (mpxy_shmem_size / ATTR_SIZE)))
 		return SBI_ERR_INVALID_PARAM;
 
 	shmem_base = hart_shmem_base(ms);
 	end_id = base_attr_id + attr_count - 1;
 
-	sbi_hart_map_saddr((unsigned long)shmem_base, ms->shmem.shmem_size);
+	sbi_hart_map_saddr((unsigned long)shmem_base, mpxy_shmem_size);
 
 	mem_ptr = (u32 *)shmem_base;
 
@@ -604,17 +681,16 @@ int sbi_mpxy_send_message(u32 channel_id, u8 msg_id,
 			  unsigned long msg_data_len,
 			  unsigned long *resp_data_len)
 {
-	int ret;
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
+	struct sbi_mpxy_channel *channel;
 	void *shmem_base, *resp_buf;
 	u32 resp_bufsize;
-
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
+	int ret;
 
 	if (!mpxy_shmem_enabled(ms))
 		return SBI_ERR_NO_SHMEM;
 
-	struct sbi_mpxy_channel *channel = mpxy_find_channel(channel_id);
+	channel = mpxy_find_channel(channel_id);
 	if (!channel)
 		return SBI_ERR_NOT_SUPPORTED;
 
@@ -624,24 +700,23 @@ int sbi_mpxy_send_message(u32 channel_id, u8 msg_id,
 	if (!resp_data_len && !channel->send_message_without_response)
 		return SBI_ERR_NOT_SUPPORTED;
 
-	if (msg_data_len > ms->shmem.shmem_size ||
+	if (msg_data_len > mpxy_shmem_size ||
 		msg_data_len > channel->attrs.msg_data_maxlen)
 		return SBI_ERR_INVALID_PARAM;
 
 	shmem_base = hart_shmem_base(ms);
-	sbi_hart_map_saddr((unsigned long)shmem_base, ms->shmem.shmem_size);
+	sbi_hart_map_saddr((unsigned long)shmem_base, mpxy_shmem_size);
 
 	if (resp_data_len) {
 		resp_buf = shmem_base;
-		resp_bufsize = ms->shmem.shmem_size;
+		resp_bufsize = mpxy_shmem_size;
 		ret = channel->send_message_with_response(channel, msg_id,
 							  shmem_base,
 							  msg_data_len,
 							  resp_buf,
 							  resp_bufsize,
 							  resp_data_len);
-	}
-	else {
+	} else {
 		ret = channel->send_message_without_response(channel, msg_id,
 							     shmem_base,
 							     msg_data_len);
@@ -655,7 +730,7 @@ int sbi_mpxy_send_message(u32 channel_id, u8 msg_id,
 		return SBI_ERR_FAILED;
 
 	if (resp_data_len &&
-	    (*resp_data_len > ms->shmem.shmem_size ||
+	    (*resp_data_len > mpxy_shmem_size ||
 	     *resp_data_len > channel->attrs.msg_data_maxlen))
 		return SBI_ERR_FAILED;
 
@@ -664,34 +739,30 @@ int sbi_mpxy_send_message(u32 channel_id, u8 msg_id,
 
 int sbi_mpxy_get_notification_events(u32 channel_id, unsigned long *events_len)
 {
-	int ret;
+	struct mpxy_state *ms = sbi_domain_mpxy_state_thishart_ptr();
+	struct sbi_mpxy_channel *channel;
 	void *eventsbuf, *shmem_base;
-
-	struct mpxy_state *ms =
-		sbi_scratch_thishart_offset_ptr(mpxy_state_offset);
+	int ret;
 
 	if (!mpxy_shmem_enabled(ms))
 		return SBI_ERR_NO_SHMEM;
 
-	struct sbi_mpxy_channel *channel = mpxy_find_channel(channel_id);
-	if (!channel)
-		return SBI_ERR_NOT_SUPPORTED;
-
-	if (!channel->get_notification_events)
+	channel = mpxy_find_channel(channel_id);
+	if (!channel || !channel->get_notification_events)
 		return SBI_ERR_NOT_SUPPORTED;
 
 	shmem_base = hart_shmem_base(ms);
-	sbi_hart_map_saddr((unsigned long)shmem_base, ms->shmem.shmem_size);
+	sbi_hart_map_saddr((unsigned long)shmem_base, mpxy_shmem_size);
 	eventsbuf = shmem_base;
 	ret = channel->get_notification_events(channel, eventsbuf,
-					       ms->shmem.shmem_size,
+					       mpxy_shmem_size,
 					       events_len);
 	sbi_hart_unmap_saddr();
 
 	if (ret)
 		return ret;
 
-	if (*events_len > ms->shmem.shmem_size)
+	if (*events_len > (mpxy_shmem_size - 16))
 		return SBI_ERR_FAILED;
 
 	return SBI_SUCCESS;

lib/sbi/sbi_pmu.c

@@ -56,6 +56,14 @@ union sbi_pmu_ctr_info {
 #error "Can't handle firmware counters beyond BITS_PER_LONG"
 #endif
 
+/** HW event configuration parameters */
+struct sbi_pmu_hw_event_config {
+	/* event_data value from sbi_pmu_ctr_cfg_match() */
+	uint64_t event_data;
+	/* HW events flags from sbi_pmu_ctr_cfg_match() */
+	uint64_t flags;
+};
+
 /** Per-HART state of the PMU counters */
 struct sbi_pmu_hart_state {
 	/* HART to which this state belongs */
@@ -72,6 +80,12 @@ struct sbi_pmu_hart_state {
 	 * and hence can optimally share the same memory.
 	 */
 	uint64_t fw_counters_data[SBI_PMU_FW_CTR_MAX];
+	/* HW events configuration parameters from
+	 * sbi_pmu_ctr_cfg_match() command which are
+	 * used for restoring RAW hardware events after
+	 * cpu suspending.
+	 */
+	struct sbi_pmu_hw_event_config hw_counters_cfg[SBI_PMU_HW_CTR_MAX];
 };
 
 /** Offset of pointer to PMU HART state in scratch space */
@@ -206,6 +220,12 @@ static int pmu_ctr_validate(struct sbi_pmu_hart_state *phs,
 	return event_idx_type;
 }
 
+static bool pmu_ctr_idx_validate(unsigned long cbase, unsigned long cmask)
+{
+	/* Do a basic sanity check of counter base & mask */
+	return cmask && cbase + sbi_fls(cmask) < total_ctrs;
+}
+
 int sbi_pmu_ctr_fw_read(uint32_t cidx, uint64_t *cval)
 {
 	int event_idx_type;
@@ -309,11 +329,11 @@ int sbi_pmu_add_raw_event_counter_map(uint64_t select, uint64_t select_mask, u32
 void sbi_pmu_ovf_irq()
 {
 	/*
-	 * We need to disable LCOFIP before returning to S-mode or we will loop
-	 * on LCOFIP being triggered
+	 * We need to disable the overflow irq before returning to S-mode or we will loop
+	 * on an irq being triggered
 	 */
-	csr_clear(CSR_MIE, MIP_LCOFIP);
-	sbi_sse_inject_event(SBI_SSE_EVENT_LOCAL_PMU);
+	csr_clear(CSR_MIE, sbi_pmu_irq_mask());
+	sbi_sse_inject_event(SBI_SSE_EVENT_LOCAL_PMU_OVERFLOW);
 }
 
 static int pmu_ctr_enable_irq_hw(int ctr_idx)
@@ -344,7 +364,7 @@ static int pmu_ctr_enable_irq_hw(int ctr_idx)
 	 * Otherwise, there will be race conditions where we may clear the bit
 	 * the software is yet to handle the interrupt.
 	 */
-	if (!(mip_val & MIP_LCOFIP)) {
+	if (!(mip_val & sbi_pmu_irq_mask())) {
 		mhpmevent_curr &= of_mask;
 		csr_write_num(mhpmevent_csr, mhpmevent_curr);
 	}
@@ -405,11 +425,21 @@ int sbi_pmu_irq_bit(void)
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 
 	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
-		return MIP_LCOFIP;
+		return IRQ_PMU_OVF;
 	if (pmu_dev && pmu_dev->hw_counter_irq_bit)
 		return pmu_dev->hw_counter_irq_bit();
 
-	return 0;
+	return -1;
+}
+
+unsigned long sbi_pmu_irq_mask(void)
+{
+	int irq_bit = sbi_pmu_irq_bit();
+
+	if (irq_bit < 0)
+		return 0;
+
+	return BIT(irq_bit);
 }
 
 static int pmu_ctr_start_fw(struct sbi_pmu_hart_state *phs,
@@ -427,7 +457,7 @@ static int pmu_ctr_start_fw(struct sbi_pmu_hart_state *phs,
 		    !pmu_dev->fw_counter_write_value ||
 		    !pmu_dev->fw_counter_start) {
 			return SBI_EINVAL;
-		    }
+		}
 
 		if (ival_update)
 			pmu_dev->fw_counter_write_value(phs->hartid,
@@ -447,6 +477,61 @@ static int pmu_ctr_start_fw(struct sbi_pmu_hart_state *phs,
 	return 0;
 }
 
+static void pmu_update_inhibit_flags(unsigned long flags, uint64_t *mhpmevent_val)
+{
+	if (flags & SBI_PMU_CFG_FLAG_SET_VUINH)
+		*mhpmevent_val |= MHPMEVENT_VUINH;
+	if (flags & SBI_PMU_CFG_FLAG_SET_VSINH)
+		*mhpmevent_val |= MHPMEVENT_VSINH;
+	if (flags & SBI_PMU_CFG_FLAG_SET_UINH)
+		*mhpmevent_val |= MHPMEVENT_UINH;
+	if (flags & SBI_PMU_CFG_FLAG_SET_SINH)
+		*mhpmevent_val |= MHPMEVENT_SINH;
+}
+
+static int pmu_update_hw_mhpmevent(struct sbi_pmu_hw_event *hw_evt, int ctr_idx,
+				   unsigned long flags, unsigned long eindex,
+				   uint64_t data)
+{
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	uint64_t mhpmevent_val;
+
+	/* Get the final mhpmevent value to be written from platform */
+	mhpmevent_val = sbi_platform_pmu_xlate_to_mhpmevent(plat, eindex, data);
+
+	if (!mhpmevent_val || ctr_idx < 3 || ctr_idx >= SBI_PMU_HW_CTR_MAX)
+		return SBI_EFAIL;
+
+	/**
+	 * Always set the OVF bit(disable interrupts) and inhibit counting of
+	 * events in M-mode. The OVF bit should be enabled during the start call.
+	 */
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
+		mhpmevent_val = (mhpmevent_val & ~MHPMEVENT_SSCOF_MASK) |
+				 MHPMEVENT_MINH | MHPMEVENT_OF;
+
+	if (pmu_dev && pmu_dev->hw_counter_disable_irq)
+		pmu_dev->hw_counter_disable_irq(ctr_idx);
+
+	/* Update the inhibit flags based on inhibit flags received from supervisor */
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
+		pmu_update_inhibit_flags(flags, &mhpmevent_val);
+	if (pmu_dev && pmu_dev->hw_counter_filter_mode)
+		pmu_dev->hw_counter_filter_mode(flags, ctr_idx);
+
+#if __riscv_xlen == 32
+	csr_write_num(CSR_MHPMEVENT3 + ctr_idx - 3, mhpmevent_val & 0xFFFFFFFF);
+	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
+		csr_write_num(CSR_MHPMEVENT3H + ctr_idx - 3,
+			      mhpmevent_val >> BITS_PER_LONG);
+#else
+	csr_write_num(CSR_MHPMEVENT3 + ctr_idx - 3, mhpmevent_val);
+#endif
+
+	return 0;
+}
+
 int sbi_pmu_ctr_start(unsigned long cbase, unsigned long cmask,
 		      unsigned long flags, uint64_t ival)
 {
@@ -462,7 +547,7 @@ int sbi_pmu_ctr_start(unsigned long cbase, unsigned long cmask,
 	int i, cidx;
 	uint64_t edata;
 
-	if ((cbase + sbi_fls(cmask)) >= total_ctrs)
+	if (!pmu_ctr_idx_validate(cbase, cmask))
 		return ret;
 
 	if (flags & SBI_PMU_STOP_FLAG_TAKE_SNAPSHOT)
@@ -483,9 +568,20 @@ int sbi_pmu_ctr_start(unsigned long cbase, unsigned long cmask,
 				 : 0x0;
 			ret = pmu_ctr_start_fw(phs, cidx, event_code, edata,
 					       ival, bUpdate);
-		}
-		else
+		} else {
+			if (cidx >= 3) {
+				struct sbi_pmu_hw_event_config *ev_cfg =
+					&phs->hw_counters_cfg[cidx];
+
+				ret = pmu_update_hw_mhpmevent(&hw_event_map[cidx], cidx,
+							ev_cfg->flags,
+							phs->active_events[cidx],
+							ev_cfg->event_data);
+				if (ret)
+					return ret;
+			}
 			ret = pmu_ctr_start_hw(cidx, ival, bUpdate);
+		}
 	}
 
 	return ret;
@@ -567,8 +663,8 @@ int sbi_pmu_ctr_stop(unsigned long cbase, unsigned long cmask,
 	uint32_t event_code;
 	int i, cidx;
 
-	if ((cbase + sbi_fls(cmask)) >= total_ctrs)
-		return SBI_EINVAL;
+	if (!pmu_ctr_idx_validate(cbase, cmask))
+		return ret;
 
 	if (flag & SBI_PMU_STOP_FLAG_TAKE_SNAPSHOT)
 		return SBI_ENO_SHMEM;
@@ -591,68 +687,13 @@ int sbi_pmu_ctr_stop(unsigned long cbase, unsigned long cmask,
 		}
 	}
 
-	/* Clear MIP_LCOFIP to avoid spurious interrupts */
+	/* Clear PMU overflow interrupt to avoid spurious ones */
 	if (phs->sse_enabled)
-		csr_clear(CSR_MIP, MIP_LCOFIP);
+		csr_clear(CSR_MIP, sbi_pmu_irq_mask());
 
 	return ret;
 }
 
-static void pmu_update_inhibit_flags(unsigned long flags, uint64_t *mhpmevent_val)
-{
-	if (flags & SBI_PMU_CFG_FLAG_SET_VUINH)
-		*mhpmevent_val |= MHPMEVENT_VUINH;
-	if (flags & SBI_PMU_CFG_FLAG_SET_VSINH)
-		*mhpmevent_val |= MHPMEVENT_VSINH;
-	if (flags & SBI_PMU_CFG_FLAG_SET_UINH)
-		*mhpmevent_val |= MHPMEVENT_UINH;
-	if (flags & SBI_PMU_CFG_FLAG_SET_SINH)
-		*mhpmevent_val |= MHPMEVENT_SINH;
-}
-
-static int pmu_update_hw_mhpmevent(struct sbi_pmu_hw_event *hw_evt, int ctr_idx,
-				   unsigned long flags, unsigned long eindex,
-				   uint64_t data)
-{
-	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
-	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
-	uint64_t mhpmevent_val;
-
-	/* Get the final mhpmevent value to be written from platform */
-	mhpmevent_val = sbi_platform_pmu_xlate_to_mhpmevent(plat, eindex, data);
-
-	if (!mhpmevent_val || ctr_idx < 3 || ctr_idx >= SBI_PMU_HW_CTR_MAX)
-		return SBI_EFAIL;
-
-	/**
-	 * Always set the OVF bit(disable interrupts) and inhibit counting of
-	 * events in M-mode. The OVF bit should be enabled during the start call.
-	 */
-	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
-		mhpmevent_val = (mhpmevent_val & ~MHPMEVENT_SSCOF_MASK) |
-				 MHPMEVENT_MINH | MHPMEVENT_OF;
-
-	if (pmu_dev && pmu_dev->hw_counter_disable_irq)
-		pmu_dev->hw_counter_disable_irq(ctr_idx);
-
-	/* Update the inhibit flags based on inhibit flags received from supervisor */
-	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
-		pmu_update_inhibit_flags(flags, &mhpmevent_val);
-	if (pmu_dev && pmu_dev->hw_counter_filter_mode)
-		pmu_dev->hw_counter_filter_mode(flags, ctr_idx);
-
-#if __riscv_xlen == 32
-	csr_write_num(CSR_MHPMEVENT3 + ctr_idx - 3, mhpmevent_val & 0xFFFFFFFF);
-	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
-		csr_write_num(CSR_MHPMEVENT3H + ctr_idx - 3,
-			      mhpmevent_val >> BITS_PER_LONG);
-#else
-	csr_write_num(CSR_MHPMEVENT3 + ctr_idx - 3, mhpmevent_val);
-#endif
-
-	return 0;
-}
-
 static int pmu_fixed_ctr_update_inhibit_bits(int fixed_ctr, unsigned long flags)
 {
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
@@ -722,12 +763,13 @@ static int pmu_ctr_find_hw(struct sbi_pmu_hart_state *phs,
 		return SBI_EINVAL;
 
 	/**
-	 * If Sscof is present try to find the programmable counter for
-	 * cycle/instret as well.
+	 * If Sscofpmf or Andes PMU is present, try to find
+	 * the programmable counter for cycle/instret as well.
 	 */
 	fixed_ctr = pmu_ctr_find_fixed_hw(event_idx);
 	if (fixed_ctr >= 0 &&
-	    !sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF))
+	    !sbi_hart_has_extension(scratch, SBI_HART_EXT_SSCOFPMF) &&
+	    !sbi_hart_has_extension(scratch, SBI_HART_EXT_XANDESPMU))
 		return pmu_fixed_ctr_update_inhibit_bits(fixed_ctr, flags);
 
 	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_11)
@@ -763,6 +805,7 @@ static int pmu_ctr_find_hw(struct sbi_pmu_hart_state *phs,
 				continue;
 			/* We found a valid counter that is not started yet */
 			ctr_idx = cbase;
+			break;
 		}
 	}
 
@@ -800,7 +843,7 @@ static int pmu_ctr_find_fw(struct sbi_pmu_hart_state *phs,
 		cidx = i + cbase;
 		if (cidx < num_hw_ctrs || total_ctrs <= cidx)
 			continue;
-		if (phs->active_events[i] != SBI_PMU_EVENT_IDX_INVALID)
+		if (phs->active_events[cidx] != SBI_PMU_EVENT_IDX_INVALID)
 			continue;
 		if (SBI_PMU_FW_PLATFORM == event_code &&
 		    pmu_dev && pmu_dev->fw_counter_match_encoding) {
@@ -810,7 +853,7 @@ static int pmu_ctr_find_fw(struct sbi_pmu_hart_state *phs,
 				continue;
 		}
 
-		return i;
+		return cidx;
 	}
 
 	return SBI_ENOTSUPP;
@@ -828,8 +871,7 @@ int sbi_pmu_ctr_cfg_match(unsigned long cidx_base, unsigned long cidx_mask,
 	int ret, event_type, ctr_idx = SBI_ENOTSUPP;
 	u32 event_code;
 
-	/* Do a basic sanity check of counter base & mask */
-	if ((cidx_base + sbi_fls(cidx_mask)) >= total_ctrs)
+	if (!pmu_ctr_idx_validate(cidx_base, cidx_mask))
 		return SBI_EINVAL;
 
 	event_type = pmu_event_validate(phs, event_idx, event_data);
@@ -857,12 +899,20 @@ int sbi_pmu_ctr_cfg_match(unsigned long cidx_base, unsigned long cidx_mask,
 		/* Any firmware counter can be used track any firmware event */
 		ctr_idx = pmu_ctr_find_fw(phs, cidx_base, cidx_mask,
 					  event_code, event_data);
-		if (event_code == SBI_PMU_FW_PLATFORM)
+		if ((event_code == SBI_PMU_FW_PLATFORM) && (ctr_idx >= num_hw_ctrs))
 			phs->fw_counters_data[ctr_idx - num_hw_ctrs] =
 								event_data;
 	} else {
 		ctr_idx = pmu_ctr_find_hw(phs, cidx_base, cidx_mask, flags,
 					  event_idx, event_data);
+		if (ctr_idx >= 0) {
+			struct sbi_pmu_hw_event_config *ev_cfg =
+					&phs->hw_counters_cfg[ctr_idx];
+
+			ev_cfg->event_data = event_data;
+			/* Remove flags that are used in match call only */
+			ev_cfg->flags = flags & SBI_PMU_CFG_EVENT_MASK;
+		}
 	}
 
 	if (ctr_idx < 0)
@@ -1086,30 +1136,43 @@ void sbi_pmu_exit(struct sbi_scratch *scratch)
 
 static void pmu_sse_enable(uint32_t event_id)
 {
-	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+	unsigned long irq_mask = sbi_pmu_irq_mask();
 
-	phs->sse_enabled = true;
-	csr_clear(CSR_MIDELEG, sbi_pmu_irq_bit());
-	csr_clear(CSR_MIP, MIP_LCOFIP);
-	csr_set(CSR_MIE, MIP_LCOFIP);
+	csr_set(CSR_MIE, irq_mask);
 }
 
 static void pmu_sse_disable(uint32_t event_id)
 {
-	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+	unsigned long irq_mask = sbi_pmu_irq_mask();
 
-	csr_clear(CSR_MIE, MIP_LCOFIP);
-	csr_clear(CSR_MIP, MIP_LCOFIP);
-	csr_set(CSR_MIDELEG, sbi_pmu_irq_bit());
-	phs->sse_enabled = false;
+	csr_clear(CSR_MIE, irq_mask);
+	csr_clear(CSR_MIP, irq_mask);
 }
 
 static void pmu_sse_complete(uint32_t event_id)
 {
-	csr_set(CSR_MIE, MIP_LCOFIP);
+	csr_set(CSR_MIE, sbi_pmu_irq_mask());
+}
+
+static void pmu_sse_register(uint32_t event_id)
+{
+	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	phs->sse_enabled = true;
+	csr_clear(CSR_MIDELEG, sbi_pmu_irq_mask());
+}
+
+static void pmu_sse_unregister(uint32_t event_id)
+{
+	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	phs->sse_enabled = false;
+	csr_set(CSR_MIDELEG, sbi_pmu_irq_mask());
 }
 
 static const struct sbi_sse_cb_ops pmu_sse_cb_ops = {
+	.register_cb = pmu_sse_register,
+	.unregister_cb = pmu_sse_unregister,
 	.enable_cb = pmu_sse_enable,
 	.disable_cb = pmu_sse_disable,
 	.complete_cb = pmu_sse_complete,
@@ -1152,9 +1215,10 @@ int sbi_pmu_init(struct sbi_scratch *scratch, bool cold_boot)
 			return SBI_EINVAL;
 
 		total_ctrs = num_hw_ctrs + SBI_PMU_FW_CTR_MAX;
-	}
 
-	sbi_sse_set_cb_ops(SBI_SSE_EVENT_LOCAL_PMU, &pmu_sse_cb_ops);
+		if (sbi_pmu_irq_bit() >= 0)
+			sbi_sse_add_event(SBI_SSE_EVENT_LOCAL_PMU_OVERFLOW, &pmu_sse_cb_ops);
+	}
 
 	phs = pmu_get_hart_state_ptr(scratch);
 	if (!phs) {

lib/sbi/sbi_scratch.c

@@ -14,18 +14,31 @@
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_string.h>
 
-u32 last_hartindex_having_scratch = 0;
-u32 hartindex_to_hartid_table[SBI_HARTMASK_MAX_BITS + 1] = { -1U };
-struct sbi_scratch *hartindex_to_scratch_table[SBI_HARTMASK_MAX_BITS + 1] = { 0 };
+#define DEFAULT_SCRATCH_ALLOC_ALIGN __SIZEOF_POINTER__
+
+u32 sbi_scratch_hart_count;
+u32 hartindex_to_hartid_table[SBI_HARTMASK_MAX_BITS] = { [0 ... SBI_HARTMASK_MAX_BITS-1] = -1U };
+struct sbi_scratch *hartindex_to_scratch_table[SBI_HARTMASK_MAX_BITS];
 
 static spinlock_t extra_lock = SPIN_LOCK_INITIALIZER;
 static unsigned long extra_offset = SBI_SCRATCH_EXTRA_SPACE_OFFSET;
 
+/*
+ * Get the alignment size.
+ * Return DEFAULT_SCRATCH_ALLOC_ALIGNMENT or riscv,cbom_block_size
+ */
+static unsigned long sbi_get_scratch_alloc_align(void)
+{
+	const struct sbi_platform *plat = sbi_platform_thishart_ptr();
+
+	if (!plat || !plat->cbom_block_size)
+		return DEFAULT_SCRATCH_ALLOC_ALIGN;
+	return plat->cbom_block_size;
+}
+
 u32 sbi_hartid_to_hartindex(u32 hartid)
 {
-	u32 i;
-
-	for (i = 0; i <= last_hartindex_having_scratch; i++)
+	sbi_for_each_hartindex(i)
 		if (hartindex_to_hartid_table[i] == hartid)
 			return i;
 
@@ -36,27 +49,30 @@ typedef struct sbi_scratch *(*hartid2scratch)(ulong hartid, ulong hartindex);
 
 int sbi_scratch_init(struct sbi_scratch *scratch)
 {
-	u32 i, h;
+	u32 h, hart_count;
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
-	for (i = 0; i < plat->hart_count; i++) {
+	hart_count = plat->hart_count;
+	if (hart_count > SBI_HARTMASK_MAX_BITS)
+		hart_count = SBI_HARTMASK_MAX_BITS;
+	sbi_scratch_hart_count = hart_count;
+
+	sbi_for_each_hartindex(i) {
 		h = (plat->hart_index2id) ? plat->hart_index2id[i] : i;
 		hartindex_to_hartid_table[i] = h;
 		hartindex_to_scratch_table[i] =
 			((hartid2scratch)scratch->hartid_to_scratch)(h, i);
 	}
 
-	last_hartindex_having_scratch = plat->hart_count - 1;
-
 	return 0;
 }
 
 unsigned long sbi_scratch_alloc_offset(unsigned long size)
 {
-	u32 i;
 	void *ptr;
 	unsigned long ret = 0;
 	struct sbi_scratch *rscratch;
+	unsigned long scratch_alloc_align = 0;
 
 	/*
 	 * We have a simple brain-dead allocator which never expects
@@ -70,8 +86,14 @@ unsigned long sbi_scratch_alloc_offset(unsigned long size)
 	if (!size)
 		return 0;
 
-	size += __SIZEOF_POINTER__ - 1;
-	size &= ~((unsigned long)__SIZEOF_POINTER__ - 1);
+	scratch_alloc_align = sbi_get_scratch_alloc_align();
+
+	/*
+	 * We let the allocation align to cacheline bytes to avoid livelock on
+	 * certain platforms due to atomic variables from the same cache line.
+	 */
+	size += scratch_alloc_align - 1;
+	size &= ~(scratch_alloc_align - 1);
 
 	spin_lock(&extra_lock);
 
@@ -85,7 +107,7 @@ done:
 	spin_unlock(&extra_lock);
 
 	if (ret) {
-		for (i = 0; i <= sbi_scratch_last_hartindex(); i++) {
+		sbi_for_each_hartindex(i) {
 			rscratch = sbi_hartindex_to_scratch(i);
 			if (!rscratch)
 				continue;

lib/sbi/sbi_sse.c

@@ -23,6 +23,7 @@
 #include <sbi/sbi_pmu.h>
 #include <sbi/sbi_sse.h>
 #include <sbi/sbi_scratch.h>
+#include <sbi/sbi_slist.h>
 #include <sbi/sbi_string.h>
 #include <sbi/sbi_trap.h>
 
@@ -39,21 +40,11 @@
 
 #define EVENT_IS_GLOBAL(__event_id) ((__event_id) & SBI_SSE_EVENT_GLOBAL_BIT)
 
-static const uint32_t supported_events[] = {
-	SBI_SSE_EVENT_LOCAL_RAS,
-	SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP,
-	SBI_SSE_EVENT_GLOBAL_RAS,
-	SBI_SSE_EVENT_LOCAL_PMU,
-	SBI_SSE_EVENT_LOCAL_SOFTWARE,
-	SBI_SSE_EVENT_GLOBAL_SOFTWARE,
-};
-
-#define EVENT_COUNT array_size(supported_events)
-
 #define sse_event_invoke_cb(_event, _cb, ...)                                 \
 	{                                                                     \
-		if (_event->cb_ops && _event->cb_ops->_cb)                    \
-			_event->cb_ops->_cb(_event->event_id, ##__VA_ARGS__); \
+		const struct sbi_sse_cb_ops *__ops = _event->info->cb_ops;    \
+		if (__ops && __ops->_cb)                                      \
+			__ops->_cb(_event->event_id, ##__VA_ARGS__);          \
 	}
 
 struct sse_entry_state {
@@ -110,7 +101,7 @@ struct sbi_sse_event {
 	struct sbi_sse_event_attrs attrs;
 	uint32_t event_id;
 	u32 hartindex;
-	const struct sbi_sse_cb_ops *cb_ops;
+	struct sse_event_info *info;
 	struct sbi_dlist node;
 };
 
@@ -167,6 +158,12 @@ struct sse_global_event {
 	spinlock_t lock;
 };
 
+struct sse_event_info {
+	uint32_t event_id;
+	const struct sbi_sse_cb_ops *cb_ops;
+	SBI_SLIST_NODE(sse_event_info);
+};
+
 static unsigned int local_event_count;
 static unsigned int global_event_count;
 static struct sse_global_event *global_events;
@@ -180,6 +177,58 @@ static u32 sse_ipi_inject_event = SBI_IPI_EVENT_MAX;
 
 static int sse_ipi_inject_send(unsigned long hartid, uint32_t event_id);
 
+struct sse_event_info global_software_event = {
+	.event_id = SBI_SSE_EVENT_GLOBAL_SOFTWARE,
+	SBI_SLIST_NODE_INIT(NULL),
+};
+
+struct sse_event_info local_software_event = {
+	.event_id = SBI_SSE_EVENT_LOCAL_SOFTWARE,
+	SBI_SLIST_NODE_INIT(&global_software_event),
+};
+
+static SBI_SLIST_HEAD(supported_events, sse_event_info) =
+				SBI_SLIST_HEAD_INIT(&local_software_event);
+
+/*
+ * This array is used to distinguish between standard event and platform
+ * events in order to return SBI_ERR_NOT_SUPPORTED for them.
+ */
+static const uint32_t standard_events[] = {
+	SBI_SSE_EVENT_LOCAL_HIGH_PRIO_RAS,
+	SBI_SSE_EVENT_LOCAL_DOUBLE_TRAP,
+	SBI_SSE_EVENT_GLOBAL_HIGH_PRIO_RAS,
+	SBI_SSE_EVENT_LOCAL_PMU_OVERFLOW,
+	SBI_SSE_EVENT_LOCAL_LOW_PRIO_RAS,
+	SBI_SSE_EVENT_GLOBAL_LOW_PRIO_RAS,
+	SBI_SSE_EVENT_LOCAL_SOFTWARE,
+	SBI_SSE_EVENT_GLOBAL_SOFTWARE,
+};
+
+static bool sse_is_standard_event(uint32_t event_id)
+{
+	int i;
+
+	for (i = 0; i < array_size(standard_events); i++) {
+		if (event_id == standard_events[i])
+			return true;
+	}
+
+	return false;
+}
+
+static struct sse_event_info *sse_event_info_get(uint32_t event_id)
+{
+	struct sse_event_info *info;
+
+	SBI_SLIST_FOR_EACH_ENTRY(info, supported_events) {
+		if (info->event_id == event_id)
+			return info;
+	}
+
+	return NULL;
+}
+
 static unsigned long sse_event_state(struct sbi_sse_event *e)
 {
 	return e->attrs.status & SBI_SSE_ATTR_STATUS_STATE_MASK;
@@ -244,30 +293,41 @@ static void sse_event_set_state(struct sbi_sse_event *e,
 	e->attrs.status |= new_state;
 }
 
-static struct sbi_sse_event *sse_event_get(uint32_t event_id)
+static int sse_event_get(uint32_t event_id, struct sbi_sse_event **eret)
 {
 	unsigned int i;
 	struct sbi_sse_event *e;
 	struct sse_hart_state *shs;
 
+	if (!eret)
+		return SBI_EINVAL;
+
 	if (EVENT_IS_GLOBAL(event_id)) {
 		for (i = 0; i < global_event_count; i++) {
 			e = &global_events[i].event;
 			if (e->event_id == event_id) {
 				spin_lock(&global_events[i].lock);
-				return e;
+				*eret = e;
+				return SBI_SUCCESS;
 			}
 		}
 	} else {
 		shs = sse_thishart_state_ptr();
 		for (i = 0; i < local_event_count; i++) {
 			e = &shs->local_events[i];
-			if (e->event_id == event_id)
-				return e;
+			if (e->event_id == event_id) {
+				*eret = e;
+				return SBI_SUCCESS;
+			}
 		}
 	}
 
-	return NULL;
+	/* Check if the event is a standard one but not supported */
+	if (sse_is_standard_event(event_id))
+		return SBI_ENOTSUPP;
+
+	/* If not supported nor a standard event, it is invalid */
+	return SBI_EINVAL;
 }
 
 static void sse_event_put(struct sbi_sse_event *e)
@@ -328,7 +388,7 @@ static int sse_event_set_hart_id_check(struct sbi_sse_event *e,
 	struct sbi_domain *hd = sbi_domain_thishart_ptr();
 
 	if (!sse_event_is_global(e))
-		return SBI_EBAD_RANGE;
+		return SBI_EDENIED;
 
 	if (!sbi_domain_is_assigned_hart(hd, sbi_hartid_to_hartindex(hartid)))
 		return SBI_EINVAL;
@@ -367,10 +427,12 @@ static int sse_event_set_attr_check(struct sbi_sse_event *e, uint32_t attr_id,
 
 		return sse_event_set_hart_id_check(e, val);
 	case SBI_SSE_ATTR_INTERRUPTED_FLAGS:
-		if (val & ~(SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPP |
-			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPIE |
+		if (val & ~(SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPP |
+			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPIE |
 			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPV |
-			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPVP))
+			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPVP |
+			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPELP |
+			    SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SDT))
 			return SBI_EINVAL;
 		__attribute__((__fallthrough__));
 	case SBI_SSE_ATTR_INTERRUPTED_SEPC:
@@ -384,7 +446,13 @@ static int sse_event_set_attr_check(struct sbi_sse_event *e, uint32_t attr_id,
 
 		return SBI_OK;
 	default:
-		return SBI_EBAD_RANGE;
+		/*
+		 * Attribute range validity was already checked by
+		 * sbi_sse_attr_check(). If we end up here, attribute was not
+		 * handled by the above 'case' statements and thus it is
+		 * read-only.
+		 */
+		return SBI_EDENIED;
 	}
 }
 
@@ -452,10 +520,14 @@ static unsigned long sse_interrupted_flags(unsigned long mstatus)
 {
 	unsigned long hstatus, flags = 0;
 
-	if (mstatus & (MSTATUS_SPIE))
-		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPIE;
-	if (mstatus & (MSTATUS_SPP))
-		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPP;
+	if (mstatus & MSTATUS_SPIE)
+		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPIE;
+	if (mstatus & MSTATUS_SPP)
+		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPP;
+	if (mstatus & MSTATUS_SPELP)
+		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPELP;
+	if (mstatus & MSTATUS_SDT)
+		flags |= SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SDT;
 
 	if (misa_extension('H')) {
 		hstatus = csr_read(CSR_HSTATUS);
@@ -513,9 +585,13 @@ static void sse_event_inject(struct sbi_sse_event *e,
 	regs->a7 = e->attrs.entry.arg;
 	regs->mepc = e->attrs.entry.pc;
 
-	/* Return to S-mode with virtualization disabled */
-	regs->mstatus &= ~(MSTATUS_MPP | MSTATUS_SIE);
+	/*
+	 * Return to S-mode with virtualization disabled, not expected landing
+	 * pad, supervisor trap disabled.
+	 */
+	regs->mstatus &= ~(MSTATUS_MPP | MSTATUS_SIE | MSTATUS_SPELP);
 	regs->mstatus |= (PRV_S << MSTATUS_MPP_SHIFT);
+	regs->mstatus |= MSTATUS_SDT;
 
 #if __riscv_xlen == 64
 	regs->mstatus &= ~MSTATUS_MPV;
@@ -566,13 +642,21 @@ static void sse_event_resume(struct sbi_sse_event *e,
 		regs->mstatus |= MSTATUS_SIE;
 
 	regs->mstatus &= ~MSTATUS_SPIE;
-	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPIE)
+	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPIE)
 		regs->mstatus |= MSTATUS_SPIE;
 
 	regs->mstatus &= ~MSTATUS_SPP;
-	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_STATUS_SPP)
+	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPP)
 		regs->mstatus |= MSTATUS_SPP;
 
+	regs->mstatus &= ~MSTATUS_SPELP;
+	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SPELP)
+		regs->mstatus |= MSTATUS_SPELP;
+
+	regs->mstatus &= ~MSTATUS_SDT;
+	if (i_ctx->flags & SBI_SSE_ATTR_INTERRUPTED_FLAGS_SSTATUS_SDT)
+		regs->mstatus |= MSTATUS_SDT;
+
 	regs->a7 = i_ctx->a7;
 	regs->a6 = i_ctx->a6;
 	csr_write(CSR_SEPC, i_ctx->sepc);
@@ -653,8 +737,7 @@ static void sse_ipi_inject_process(struct sbi_scratch *scratch)
 
 	/* Mark all queued events as pending */
 	while (!sbi_fifo_dequeue(sse_inject_fifo_r, &evt)) {
-		e = sse_event_get(evt.event_id);
-		if (!e)
+		if (sse_event_get(evt.event_id, &e))
 			continue;
 
 		sse_event_set_pending(e);
@@ -696,10 +779,9 @@ static int sse_inject_event(uint32_t event_id, unsigned long hartid)
 	int ret;
 	struct sbi_sse_event *e;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
-
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	/* In case of global event, provided hart_id is ignored */
 	if (sse_event_is_global(e))
@@ -788,9 +870,9 @@ int sbi_sse_enable(uint32_t event_id)
 	int ret;
 	struct sbi_sse_event *e;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	sse_enabled_event_lock(e);
 	ret = sse_event_enable(e);
@@ -805,9 +887,9 @@ int sbi_sse_disable(uint32_t event_id)
 	int ret;
 	struct sbi_sse_event *e;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	sse_enabled_event_lock(e);
 	ret = sse_event_disable(e);
@@ -826,7 +908,7 @@ int sbi_sse_hart_mask(void)
 		return SBI_EFAIL;
 
 	if (state->masked)
-		return SBI_EALREADY_STARTED;
+		return SBI_EALREADY_STOPPED;
 
 	state->masked = true;
 
@@ -841,7 +923,7 @@ int sbi_sse_hart_unmask(void)
 		return SBI_EFAIL;
 
 	if (!state->masked)
-		return SBI_EALREADY_STOPPED;
+		return SBI_EALREADY_STARTED;
 
 	state->masked = false;
 
@@ -863,19 +945,26 @@ int sbi_sse_inject_event(uint32_t event_id)
 	return sse_inject_event(event_id, current_hartid());
 }
 
-int sbi_sse_set_cb_ops(uint32_t event_id, const struct sbi_sse_cb_ops *cb_ops)
+int sbi_sse_add_event(uint32_t event_id, const struct sbi_sse_cb_ops *cb_ops)
 {
-	struct sbi_sse_event *e;
+	struct sse_event_info *info;
 
-	if (cb_ops->set_hartid_cb && !EVENT_IS_GLOBAL(event_id))
+	/* Do not allow adding an event twice */
+	info = sse_event_info_get(event_id);
+	if (info)
+		return SBI_EALREADY;
+
+	if (cb_ops && cb_ops->set_hartid_cb && !EVENT_IS_GLOBAL(event_id))
 		return SBI_EINVAL;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	info = sbi_zalloc(sizeof(*info));
+	if (!info)
+		return SBI_ENOMEM;
 
-	e->cb_ops = cb_ops;
-	sse_event_put(e);
+	info->cb_ops = cb_ops;
+	info->event_id = event_id;
+
+	SBI_SLIST_ADD(info, supported_events);
 
 	return SBI_OK;
 }
@@ -943,9 +1032,9 @@ int sbi_sse_read_attrs(uint32_t event_id, uint32_t base_attr_id,
 	if (ret)
 		return ret;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	sbi_hart_map_saddr(output_phys_lo, sizeof(unsigned long) * attr_count);
 
@@ -1008,9 +1097,9 @@ int sbi_sse_write_attrs(uint32_t event_id, uint32_t base_attr_id,
 	if (ret)
 		return ret;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	ret = sse_write_attrs(e, base_attr_id, attr_count, input_phys_lo);
 	sse_event_put(e);
@@ -1033,9 +1122,9 @@ int sbi_sse_register(uint32_t event_id, unsigned long handler_entry_pc,
 					 SBI_DOMAIN_EXECUTE))
 		return SBI_EINVALID_ADDR;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	ret = sse_event_register(e, handler_entry_pc, handler_entry_arg);
 	sse_event_put(e);
@@ -1048,9 +1137,9 @@ int sbi_sse_unregister(uint32_t event_id)
 	int ret;
 	struct sbi_sse_event *e;
 
-	e = sse_event_get(event_id);
-	if (!e)
-		return SBI_EINVAL;
+	ret = sse_event_get(event_id, &e);
+	if (ret)
+		return ret;
 
 	ret = sse_event_unregister(e);
 	sse_event_put(e);
@@ -1058,9 +1147,10 @@ int sbi_sse_unregister(uint32_t event_id)
 	return ret;
 }
 
-static void sse_event_init(struct sbi_sse_event *e, uint32_t event_id)
+static void sse_event_init(struct sbi_sse_event *e, struct sse_event_info *info)
 {
-	e->event_id = event_id;
+	e->event_id = info->event_id;
+	e->info = info;
 	e->hartindex = current_hartindex();
 	e->attrs.hartid = current_hartid();
 	/* Declare all events as injectable */
@@ -1069,10 +1159,10 @@ static void sse_event_init(struct sbi_sse_event *e, uint32_t event_id)
 
 static void sse_event_count_init()
 {
-	unsigned int i;
+	struct sse_event_info *info;
 
-	for (i = 0; i < EVENT_COUNT; i++) {
-		if (EVENT_IS_GLOBAL(supported_events[i]))
+	SBI_SLIST_FOR_EACH_ENTRY(info, supported_events) {
+		if (EVENT_IS_GLOBAL(info->event_id))
 			global_event_count++;
 		else
 			local_event_count++;
@@ -1082,18 +1172,19 @@ static void sse_event_count_init()
 static int sse_global_init()
 {
 	struct sbi_sse_event *e;
-	unsigned int i, ev = 0;
+	unsigned int ev = 0;
+	struct sse_event_info *info;
 
 	global_events = sbi_zalloc(sizeof(*global_events) * global_event_count);
 	if (!global_events)
 		return SBI_ENOMEM;
 
-	for (i = 0; i < EVENT_COUNT; i++) {
-		if (!EVENT_IS_GLOBAL(supported_events[i]))
+	SBI_SLIST_FOR_EACH_ENTRY(info, supported_events) {
+		if (!EVENT_IS_GLOBAL(info->event_id))
 			continue;
 
 		e = &global_events[ev].event;
-		sse_event_init(e, supported_events[i]);
+		sse_event_init(e, info);
 		SPIN_LOCK_INIT(global_events[ev].lock);
 
 		ev++;
@@ -1104,16 +1195,16 @@ static int sse_global_init()
 
 static void sse_local_init(struct sse_hart_state *shs)
 {
-	unsigned int i, ev = 0;
+	unsigned int ev = 0;
+	struct sse_event_info *info;
 
 	SBI_INIT_LIST_HEAD(&shs->enabled_event_list);
 	SPIN_LOCK_INIT(shs->enabled_event_lock);
 
-	for (i = 0; i < EVENT_COUNT; i++) {
-		if (EVENT_IS_GLOBAL(supported_events[i]))
+	SBI_SLIST_FOR_EACH_ENTRY(info, supported_events) {
+		if (EVENT_IS_GLOBAL(info->event_id))
 			continue;
-
-		sse_event_init(&shs->local_events[ev++], supported_events[i]);
+		sse_event_init(&shs->local_events[ev++], info);
 	}
 }
 
@@ -1143,7 +1234,8 @@ int sbi_sse_init(struct sbi_scratch *scratch, bool cold_boot)
 		}
 
 		sse_inject_fifo_mem_off = sbi_scratch_alloc_offset(
-			EVENT_COUNT * sizeof(struct sse_ipi_inject_data));
+			(global_event_count + local_event_count) *
+			sizeof(struct sse_ipi_inject_data));
 		if (!sse_inject_fifo_mem_off) {
 			sbi_scratch_free_offset(sse_inject_fifo_off);
 			sbi_scratch_free_offset(shs_ptr_off);
@@ -1180,7 +1272,8 @@ int sbi_sse_init(struct sbi_scratch *scratch, bool cold_boot)
 	sse_inject_mem =
 		sbi_scratch_offset_ptr(scratch, sse_inject_fifo_mem_off);
 
-	sbi_fifo_init(sse_inject_q, sse_inject_mem, EVENT_COUNT,
+	sbi_fifo_init(sse_inject_q, sse_inject_mem,
+		      (global_event_count + local_event_count),
 		      sizeof(struct sse_ipi_inject_data));
 
 	return 0;
@@ -1188,21 +1281,18 @@ int sbi_sse_init(struct sbi_scratch *scratch, bool cold_boot)
 
 void sbi_sse_exit(struct sbi_scratch *scratch)
 {
-	int i;
 	struct sbi_sse_event *e;
+	struct sse_event_info *info;
 
-	for (i = 0; i < EVENT_COUNT; i++) {
-		e = sse_event_get(supported_events[i]);
-		if (!e)
+	SBI_SLIST_FOR_EACH_ENTRY(info, supported_events) {
+		if (sse_event_get(info->event_id, &e))
 			continue;
 
 		if (e->attrs.hartid != current_hartid())
 			goto skip;
 
-		if (sse_event_state(e) > SBI_SSE_STATE_REGISTERED) {
-			sbi_printf("Event %d in invalid state at exit", i);
+		if (sse_event_state(e) > SBI_SSE_STATE_REGISTERED)
 			sse_event_set_state(e, SBI_SSE_STATE_UNUSED);
-		}
 
 skip:
 		sse_event_put(e);

lib/sbi/sbi_string.c

@@ -68,22 +68,22 @@ char *sbi_strcpy(char *dest, const char *src)
 {
 	char *ret = dest;
 
-	while (*src != '\0') {
-		*dest++ = *src++;
+	while ((*dest++ = *src++) != '\0') {
 	}
-
 	return ret;
 }
 
 char *sbi_strncpy(char *dest, const char *src, size_t count)
 {
-	char *ret = dest;
+	char *tmp = dest;
 
-	while (count-- && *src != '\0') {
-		*dest++ = *src++;
+	while (count) {
+		if ((*tmp = *src) != 0)
+			src++;
+		tmp++;
+		count--;
 	}
-
-	return ret;
+	return dest;
 }
 
 char *sbi_strchr(const char *s, int c)

lib/sbi/sbi_system.c

@@ -87,6 +87,7 @@ void __noreturn sbi_system_reset(u32 reset_type, u32 reset_reason)
 }
 
 static const struct sbi_system_suspend_device *suspend_dev = NULL;
+static bool system_suspended;
 
 const struct sbi_system_suspend_device *sbi_system_suspend_get_device(void)
 {
@@ -137,6 +138,19 @@ bool sbi_system_suspend_supported(u32 sleep_type)
 	       suspend_dev->system_suspend_check(sleep_type) == 0;
 }
 
+bool sbi_system_is_suspended(void)
+{
+	return system_suspended;
+}
+
+void sbi_system_resume(void)
+{
+	if (suspend_dev && suspend_dev->system_resume)
+		suspend_dev->system_resume();
+
+	system_suspended = false;
+}
+
 int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 {
 	struct sbi_domain *dom = sbi_domain_thishart_ptr();
@@ -189,11 +203,14 @@ int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 	__sbi_hsm_suspend_non_ret_save(scratch);
 
 	/* Suspend */
+	system_suspended = true;
 	ret = suspend_dev->system_suspend(sleep_type, scratch->warmboot_addr);
 	if (ret != SBI_OK) {
 		if (!sbi_hsm_hart_change_state(scratch, SBI_HSM_STATE_SUSPENDED,
 					       SBI_HSM_STATE_STARTED))
 			sbi_hart_hang();
+
+		system_suspended = false;
 		return ret;
 	}
 

lib/sbi/sbi_timer.c

@@ -139,12 +139,7 @@ void sbi_timer_event_start(u64 next_event)
 	 * the older software to leverage sstc extension on newer hardware.
 	 */
 	if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(), SBI_HART_EXT_SSTC)) {
-#if __riscv_xlen == 32
-		csr_write(CSR_STIMECMP, next_event & 0xFFFFFFFF);
-		csr_write(CSR_STIMECMPH, next_event >> 32);
-#else
-		csr_write(CSR_STIMECMP, next_event);
-#endif
+		csr_write64(CSR_STIMECMP, next_event);
 	} else if (timer_dev && timer_dev->timer_event_start) {
 		timer_dev->timer_event_start(next_event);
 		csr_clear(CSR_MIP, MIP_STIP);
@@ -190,7 +185,7 @@ int sbi_timer_init(struct sbi_scratch *scratch, bool cold_boot)
 		if (!time_delta_off)
 			return SBI_ENOMEM;
 
-		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_ZICNTR))
+		if (sbi_hart_has_csr(scratch, SBI_HART_CSR_TIME))
 			get_time_val = get_ticks;
 
 		ret = sbi_platform_timer_init(plat);

lib/sbi/sbi_trap.c

@@ -11,6 +11,7 @@
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_console.h>
+#include <sbi/sbi_double_trap.h>
 #include <sbi/sbi_ecall.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
@@ -168,7 +169,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		csr_write(CSR_VSCAUSE, trap->cause);
 
 		/* Set MEPC to VS-mode exception vector base */
-		regs->mepc = csr_read(CSR_VSTVEC);
+		regs->mepc = csr_read(CSR_VSTVEC) & ~MTVEC_MODE;
 
 		/* Set MPP to VS-mode */
 		regs->mstatus &= ~MSTATUS_MPP;
@@ -203,7 +204,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 		csr_write(CSR_SCAUSE, trap->cause);
 
 		/* Set MEPC to S-mode exception vector base */
-		regs->mepc = csr_read(CSR_STVEC);
+		regs->mepc = csr_read(CSR_STVEC) & ~MTVEC_MODE;
 
 		/* Set MPP to S-mode */
 		regs->mstatus &= ~MSTATUS_MPP;
@@ -239,12 +240,13 @@ static int sbi_trap_nonaia_irq(unsigned long irq)
 	case IRQ_M_SOFT:
 		sbi_ipi_process();
 		break;
-	case IRQ_PMU_OVF:
-		sbi_pmu_ovf_irq();
-		break;
 	case IRQ_M_EXT:
 		return sbi_irqchip_process();
 	default:
+		if (irq == sbi_pmu_irq_bit()) {
+			sbi_pmu_ovf_irq();
+			return 0;
+		}
 		return SBI_ENOENT;
 	}
 
@@ -265,15 +267,17 @@ static int sbi_trap_aia_irq(void)
 		case IRQ_M_SOFT:
 			sbi_ipi_process();
 			break;
-		case IRQ_PMU_OVF:
-			sbi_pmu_ovf_irq();
-			break;
 		case IRQ_M_EXT:
 			rc = sbi_irqchip_process();
 			if (rc)
 				return rc;
 			break;
 		default:
+			if (mtopi == sbi_pmu_irq_bit()) {
+				sbi_pmu_ovf_irq();
+				break;
+			}
+
 			return SBI_ENOENT;
 		}
 	}

lib/sbi/sbi_unpriv.c

@@ -24,7 +24,7 @@
 	{                                                                     \
 		register ulong tinfo asm("a3");                               \
 		register ulong mstatus = 0;                                   \
-		register ulong mtvec = sbi_hart_expected_trap_addr();         \
+		register ulong mtvec = (ulong)sbi_hart_expected_trap;         \
 		type ret = 0;                                                 \
 		trap->cause = 0;                                              \
 		asm volatile(                                                 \
@@ -51,7 +51,7 @@
 	{                                                                     \
 		register ulong tinfo asm("a3") = (ulong)trap;                 \
 		register ulong mstatus = 0;                                   \
-		register ulong mtvec = sbi_hart_expected_trap_addr();         \
+		register ulong mtvec = (ulong)sbi_hart_expected_trap;         \
 		trap->cause = 0;                                              \
 		asm volatile(                                                 \
 			"add %[tinfo], %[taddr], zero\n"                      \
@@ -121,7 +121,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 = sbi_hart_expected_trap_addr();
+	register ulong mtvec = (ulong)sbi_hart_expected_trap;
 	ulong insn = 0;
 
 	trap->cause = 0;

lib/sbi/tests/objects.mk

@@ -15,3 +15,9 @@ libsbi-objs-$(CONFIG_SBIUNIT) += tests/riscv_locks_test.o
 
 carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += math_test_suite
 libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_math_test.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += ecall_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_ecall_test.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += bitops_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_bitops_test.o