lib: sbi: check result of pmp_get() in is_pmp_entry_mapped()

pmp_get() may return an error if the given entry, given by the caller
of is_pmp_entry_mapped(), is invalid. This results in the output
parameters for pmp_get() being uninitialized. To avoid using garbage
values, check the result and return early if necessary.

This issue is not being hit because at the moment
is_pmp_entry_mapped() is only being called from a single site with a
valid hardcoded value.

Signed-off-by: Carlos López <carlos.lopezr4096@gmail.com>
Reviewed-by: Anup Patel <anup@brainfault.org>

Makefile

@@ -79,6 +79,7 @@ export PYTHONDONTWRITEBYTECODE=1
 export KCONFIG_DIR=$(platform_build_dir)/kconfig
 export KCONFIG_AUTOLIST=$(KCONFIG_DIR)/auto.list
 export KCONFIG_AUTOHEADER=$(KCONFIG_DIR)/autoconf.h
+export KCONFIG_AUTOCONFIG=$(KCONFIG_DIR)/auto.conf
 export KCONFIG_AUTOCMD=$(KCONFIG_DIR)/auto.conf.cmd
 export KCONFIG_CONFIG=$(KCONFIG_DIR)/.config
 # Additional exports for include paths in Kconfig files
@@ -167,12 +168,22 @@ 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)
 
+# Check whether the linker supports --exclude-libs
+OPENSBI_LD_EXCLUDE_LIBS := $(shell $(CC) $(CLANG_TARGET) $(RELAX_FLAG) $(USE_LD_FLAG) "-Wl,--exclude-libs,ALL" -x c /dev/null -o /dev/null >/dev/null 2>&1 && echo y || echo n)
+
 # Check whether the compiler supports -m(no-)save-restore
 CC_SUPPORT_SAVE_RESTORE := $(shell $(CC) $(CLANG_TARGET) $(RELAX_FLAG) -nostdlib -mno-save-restore -x c /dev/null -o /dev/null 2>&1 | grep -e "-save-restore" >/dev/null && echo n || echo y)
 
+# Check whether the compiler supports -m(no-)strict-align
+CC_SUPPORT_STRICT_ALIGN := $(shell $(CC) $(CLANG_TARGET) $(RELAX_FLAG) -nostdlib -mstrict-align -x c /dev/null -o /dev/null 2>&1 | grep -e "-mstrict-align\|-mno-unaligned-access" >/dev/null && echo n || echo y)
+
 # Check whether the assembler and the compiler support the Zicsr and Zifencei extensions
 CC_SUPPORT_ZICSR_ZIFENCEI := $(shell $(CC) $(CLANG_TARGET) $(RELAX_FLAG) -nostdlib -march=rv$(OPENSBI_CC_XLEN)imafd_zicsr_zifencei -x c /dev/null -o /dev/null 2>&1 | grep "zicsr\|zifencei" > /dev/null && echo n || echo y)
 
+ifneq ($(OPENSBI_LD_PIE),y)
+$(error Your linker does not support creating PIEs, opensbi requires this.)
+endif
+
 # Build Info:
 # OPENSBI_BUILD_TIME_STAMP -- the compilation time stamp
 # OPENSBI_BUILD_COMPILER_VERSION -- the compiler version info
@@ -210,24 +221,28 @@ ifdef PLATFORM
 menuconfig: $(platform_src_dir)/Kconfig $(src_dir)/Kconfig
 	$(CMD_PREFIX)mkdir -p $(KCONFIG_DIR)
 	$(CMD_PREFIX)$(src_dir)/scripts/Kconfiglib/menuconfig.py $(src_dir)/Kconfig
-	$(CMD_PREFIX)$(src_dir)/scripts/Kconfiglib/genconfig.py --header-path $(KCONFIG_AUTOHEADER) --sync-deps $(KCONFIG_DIR) --file-list $(KCONFIG_AUTOLIST) $(src_dir)/Kconfig
 
 .PHONY: savedefconfig
 savedefconfig: $(platform_src_dir)/Kconfig $(src_dir)/Kconfig
 	$(CMD_PREFIX)mkdir -p $(KCONFIG_DIR)
 	$(CMD_PREFIX)$(src_dir)/scripts/Kconfiglib/savedefconfig.py --kconfig $(src_dir)/Kconfig --out $(KCONFIG_DIR)/defconfig
 
-$(KCONFIG_CONFIG): $(platform_src_dir)/configs/$(PLATFORM_DEFCONFIG) $(platform_src_dir)/Kconfig $(src_dir)/Kconfig
+$(KCONFIG_CONFIG): $(platform_src_dir)/configs/$(PLATFORM_DEFCONFIG)
 	$(CMD_PREFIX)mkdir -p $(KCONFIG_DIR)
 	$(CMD_PREFIX)$(src_dir)/scripts/Kconfiglib/defconfig.py --kconfig $(src_dir)/Kconfig $(platform_src_dir)/configs/$(PLATFORM_DEFCONFIG)
+
+$(KCONFIG_AUTOCONFIG): $(KCONFIG_CONFIG)
 	$(CMD_PREFIX)$(src_dir)/scripts/Kconfiglib/genconfig.py --header-path $(KCONFIG_AUTOHEADER) --sync-deps $(KCONFIG_DIR) --file-list $(KCONFIG_AUTOLIST) $(src_dir)/Kconfig
 
-$(KCONFIG_AUTOCMD): $(KCONFIG_CONFIG)
-	$(CMD_PREFIX)mkdir -p $(KCONFIG_DIR)
+$(KCONFIG_AUTOHEADER): $(KCONFIG_AUTOCONFIG);
+
+$(KCONFIG_AUTOLIST): $(KCONFIG_AUTOCONFIG);
+
+$(KCONFIG_AUTOCMD): $(KCONFIG_AUTOLIST)
 	$(CMD_PREFIX)printf "%s: " $(KCONFIG_CONFIG) > $(KCONFIG_AUTOCMD)
 	$(CMD_PREFIX)cat $(KCONFIG_AUTOLIST) | tr '\n' ' ' >> $(KCONFIG_AUTOCMD)
 
-include $(KCONFIG_CONFIG)
+include $(KCONFIG_AUTOCONFIG)
 include $(KCONFIG_AUTOCMD)
 endif
 
@@ -337,17 +352,20 @@ CFLAGS		+=	-O0
 else
 CFLAGS		+=	-O2
 endif
-CFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls -mstrict-align
-# enable -m(no-)save-restore option by CC_SUPPORT_SAVE_RESTORE
+CFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls
+# Optionally supported flags
 ifeq ($(CC_SUPPORT_SAVE_RESTORE),y)
 CFLAGS		+=	-mno-save-restore
 endif
+ifeq ($(CC_SUPPORT_STRICT_ALIGN),y)
+CFLAGS		+=	-mstrict-align
+endif
 CFLAGS		+=	-mabi=$(PLATFORM_RISCV_ABI) -march=$(PLATFORM_RISCV_ISA)
 CFLAGS		+=	-mcmodel=$(PLATFORM_RISCV_CODE_MODEL)
 CFLAGS		+=	$(RELAX_FLAG)
 CFLAGS		+=	$(GENFLAGS)
 CFLAGS		+=	$(platform-cflags-y)
-CFLAGS		+=	-fno-pie -no-pie
+CFLAGS		+=	-fPIE -pie
 CFLAGS		+=	$(firmware-cflags-y)
 
 CPPFLAGS	+=	$(GENFLAGS)
@@ -355,11 +373,15 @@ CPPFLAGS	+=	$(platform-cppflags-y)
 CPPFLAGS	+=	$(firmware-cppflags-y)
 
 ASFLAGS		=	-g -Wall -nostdlib
-ASFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls -mstrict-align
-# enable -m(no-)save-restore option by CC_SUPPORT_SAVE_RESTORE
+ASFLAGS		+=	-fno-omit-frame-pointer -fno-optimize-sibling-calls
+ASFLAGS		+=	-fPIE
+# Optionally supported flags
 ifeq ($(CC_SUPPORT_SAVE_RESTORE),y)
 ASFLAGS		+=	-mno-save-restore
 endif
+ifeq ($(CC_SUPPORT_STRICT_ALIGN),y)
+ASFLAGS		+=	-mstrict-align
+endif
 ASFLAGS		+=	-mabi=$(PLATFORM_RISCV_ABI) -march=$(PLATFORM_RISCV_ISA)
 ASFLAGS		+=	-mcmodel=$(PLATFORM_RISCV_CODE_MODEL)
 ASFLAGS		+=	$(RELAX_FLAG)
@@ -375,8 +397,11 @@ ASFLAGS		+=	$(firmware-asflags-y)
 ARFLAGS		=	rcs
 
 ELFFLAGS	+=	$(USE_LD_FLAG)
+ifeq ($(OPENSBI_LD_EXCLUDE_LIBS),y)
 ELFFLAGS	+=	-Wl,--exclude-libs,ALL
+endif
 ELFFLAGS	+=	-Wl,--build-id=none
+ELFFLAGS	+=	-Wl,--no-dynamic-linker -Wl,-pie
 ELFFLAGS	+=	$(platform-ldflags-y)
 ELFFLAGS	+=	$(firmware-ldflags-y)
 
@@ -490,14 +515,14 @@ $(build_dir)/lib/libsbi.a: $(libsbi-objs-path-y)
 $(platform_build_dir)/lib/libplatsbi.a: $(libsbi-objs-path-y) $(libsbiutils-objs-path-y) $(platform-objs-path-y)
 	$(call compile_ar,$@,$^)
 
-$(build_dir)/%.dep: $(src_dir)/%.carray $(KCONFIG_CONFIG)
-	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_CONFIG))
+$(build_dir)/%.dep: $(src_dir)/%.carray $(KCONFIG_AUTOHEADER)
+	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_AUTOHEADER))
 	$(call compile_gen_dep,$@,.o,$(@:.dep=.c))
 
 $(build_dir)/%.c: $(src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
-$(build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_CONFIG)
+$(build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_AUTOHEADER)
 	$(call compile_cc_dep,$@,$<)
 
 $(build_dir)/%.o: $(src_dir)/%.c
@@ -511,24 +536,24 @@ $(build_dir)/lib/sbi/sbi_init.o: $(libsbi_dir)/sbi_init.c FORCE
 	$(call compile_cc,$@,$<)
 endif
 
-$(build_dir)/%.dep: $(src_dir)/%.S $(KCONFIG_CONFIG)
+$(build_dir)/%.dep: $(src_dir)/%.S $(KCONFIG_AUTOHEADER)
 	$(call compile_as_dep,$@,$<)
 
 $(build_dir)/%.o: $(src_dir)/%.S
 	$(call compile_as,$@,$<)
 
 # Rules for platform sources
-$(platform_build_dir)/%.dep: $(platform_src_dir)/%.carray $(KCONFIG_CONFIG)
-	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_CONFIG))
+$(platform_build_dir)/%.dep: $(platform_src_dir)/%.carray $(KCONFIG_AUTOHEADER)
+	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_AUTOHEADER))
 	$(call compile_gen_dep,$@,.o,$(@:.dep=.c))
 
 $(platform_build_dir)/%.c: $(platform_src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
-$(platform_build_dir)/%.dep: $(platform_src_dir)/%.c $(KCONFIG_CONFIG)
+$(platform_build_dir)/%.dep: $(platform_src_dir)/%.c $(KCONFIG_AUTOHEADER)
 	$(call compile_cc_dep,$@,$<)
 
-$(platform_build_dir)/%.o: $(platform_src_dir)/%.c $(KCONFIG_CONFIG)
+$(platform_build_dir)/%.o: $(platform_src_dir)/%.c $(KCONFIG_AUTOHEADER)
 	$(call compile_cc,$@,$<)
 
 $(platform_build_dir)/%.dep: $(platform_src_dir)/%.S
@@ -537,8 +562,8 @@ $(platform_build_dir)/%.dep: $(platform_src_dir)/%.S
 $(platform_build_dir)/%.o: $(platform_src_dir)/%.S
 	$(call compile_as,$@,$<)
 
-$(platform_build_dir)/%.dep: $(platform_src_dir)/%.dts $(KCONFIG_CONFIG)
-	$(call compile_gen_dep,$@,.dtb,$< $(KCONFIG_CONFIG))
+$(platform_build_dir)/%.dep: $(platform_src_dir)/%.dts $(KCONFIG_AUTOHEADER)
+	$(call compile_gen_dep,$@,.dtb,$< $(KCONFIG_AUTOHEADER))
 	$(call compile_gen_dep,$@,.c,$(@:.dep=.dtb))
 	$(call compile_gen_dep,$@,.o,$(@:.dep=.c))
 
@@ -555,26 +580,26 @@ $(platform_build_dir)/%.bin: $(platform_build_dir)/%.elf
 $(platform_build_dir)/%.elf: $(platform_build_dir)/%.o $(platform_build_dir)/%.elf.ld $(platform_build_dir)/lib/libplatsbi.a
 	$(call compile_elf,$@,$@.ld,$< $(platform_build_dir)/lib/libplatsbi.a)
 
-$(platform_build_dir)/%.dep: $(src_dir)/%.ldS $(KCONFIG_CONFIG)
+$(platform_build_dir)/%.dep: $(src_dir)/%.ldS $(KCONFIG_AUTOHEADER)
 	$(call compile_cpp_dep,$@,.ld,$<)
 
 $(platform_build_dir)/%.ld: $(src_dir)/%.ldS
 	$(call compile_cpp,$@,$<)
 
-$(platform_build_dir)/%.dep: $(src_dir)/%.carray $(KCONFIG_CONFIG)
-	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_CONFIG))
+$(platform_build_dir)/%.dep: $(src_dir)/%.carray $(KCONFIG_AUTOHEADER)
+	$(call compile_gen_dep,$@,.c,$< $(KCONFIG_AUTOHEADER))
 	$(call compile_gen_dep,$@,.o,$(@:.dep=.c))
 
 $(platform_build_dir)/%.c: $(src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
-$(platform_build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_CONFIG)
+$(platform_build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_AUTOHEADER)
 	$(call compile_cc_dep,$@,$<)
 
 $(platform_build_dir)/%.o: $(src_dir)/%.c
 	$(call compile_cc,$@,$<)
 
-$(platform_build_dir)/%.dep: $(src_dir)/%.S $(KCONFIG_CONFIG)
+$(platform_build_dir)/%.dep: $(src_dir)/%.S $(KCONFIG_AUTOHEADER)
 	$(call compile_as_dep,$@,$<)
 
 $(platform_build_dir)/%.o: $(src_dir)/%.S

README.md

@@ -276,8 +276,7 @@ document.
 
 NOTE: Using Clang with a `riscv*-linux-gnu` GNU binutils linker has been seen
 to produce broken binaries with missing relocations; it is therefore currently
-recommended that this combination be avoided or *FW_PIC=n* be used to disable
-building OpenSBI as a position-independent binary.
+recommended that this combination be avoided.
 
 Building with timestamp and compiler info
 -----------------------------------------

docs/domain_support.md

@@ -41,6 +41,7 @@ has following details:
 * **name** - Name of this domain
 * **assigned_harts** - HARTs assigned to this domain
 * **possible_harts** - HARTs possible in this domain
+* **hartindex_to_context_table** - Contexts corresponding to possible HARTs
 * **regions** - Array of memory regions terminated by a memory region
   with order zero
 * **boot_hartid** - HART id of the HART booting this domain. The domain
@@ -80,6 +81,7 @@ following manner:
   platform support
 * **possible_harts** - All valid HARTs of a RISC-V platform are possible
   HARTs of the ROOT domain
+* **hartindex_to_context_table** - Contexts corresponding to ROOT domain's possible HARTs
 * **regions** - Two memory regions available to the ROOT domain:
   **A)** A memory region to protect OpenSBI firmware from S-mode and U-mode
   **B)** A memory region of **order=__riscv_xlen** allowing S-mode and
@@ -126,9 +128,6 @@ The DT properties of a domain configuration DT node are as follows:
 * **compatible** (Mandatory) - The compatible string of the domain
   configuration. This DT property should have value *"opensbi,domain,config"*
 
-* **system-suspend-test** (Optional) - When present, enable a system
-  suspend test implementation which simply waits five seconds and issues a WFI.
-
 ### Domain Memory Region Node
 
 The domain memory region DT node describes details of a memory region and
@@ -237,7 +236,6 @@ be done:
     chosen {
         opensbi-domains {
             compatible = "opensbi,domain,config";
-            system-suspend-test;
 
             tmem: tmem {
                 compatible = "opensbi,domain,memregion";

docs/doxygen.cfg

@@ -796,6 +796,8 @@ INPUT                  = @@SRC_DIR@@/README.md \
                          @@SRC_DIR@@/docs/platform_requirements.md \
                          @@SRC_DIR@@/docs/library_usage.md \
                          @@SRC_DIR@@/docs/domain_support.md \
+                         @@SRC_DIR@@/docs/opensbi_config.md \
+                         @@SRC_DIR@@/docs/writing_tests.md \
                          @@SRC_DIR@@/docs/firmware \
                          @@SRC_DIR@@/docs/platform \
                          @@SRC_DIR@@/include \

docs/firmware/fw.md

@@ -61,20 +61,15 @@ Firmware Configuration and Compilation
 All firmware types support the following common compile time configuration
 parameters:
 
-* **FW_TEXT_START** - Defines the execution address of the OpenSBI firmware.
-  This configuration parameter is mandatory.
+* **FW_TEXT_START** - Defines the compile time address of the OpenSBI
+  firmware. This configuration parameter is optional and assumed to be
+  `0` if not specified.
 * **FW_FDT_PATH** - Path to an external flattened device tree binary file to
   be embedded in the *.rodata* section of the final firmware. If this option
   is not provided then the firmware will expect the FDT to be passed as an
   argument by the prior booting stage.
 * **FW_FDT_PADDING** - Optional zero bytes padding to the embedded flattened
   device tree binary file specified by **FW_FDT_PATH** option.
-* **FW_PIC** - "FW_PIC=y" generates position independent executable firmware
-  images. OpenSBI can run at arbitrary address with appropriate alignment.
-  Therefore, the original relocation mechanism ("FW_PIC=n") will be skipped.
-  In other words, OpenSBI will directly run at the load address without any
-  code movement. This option requires a toolchain with PIE support, and it
-  is on by default.
 
 Additionally, each firmware type as a set of type specific configuration
 parameters. Detailed information for each firmware type can be found in the

docs/firmware/fw_jump.md

@@ -31,9 +31,14 @@ follows:
 
 * **FW_JUMP_ADDR** - Address of the entry point of the booting stage to be
   executed following OpenSBI firmware. This address generally corresponds
-  exactly to the address where this next booting stage was loaded. This is a
-  mandatory parameter. Compilation errors will result from not defining this
-  address.
+  exactly to the address where this next booting stage was loaded.
+  At least one of *FW_JUMP_ADDR* and *FW_JUMP_OFFSET* (see below) should be
+  defined. Compilation errors will result from not defining one of them.
+
+* **FW_JUMP_OFFSET** - Address offset from the opensbi load address where the
+  entry point of the next booting stage is located. This offset is used as
+  relocatable address of the next booting stage entry point. If *FW_JUMP_ADDR*
+  is also defined, the firmware will prefer *FW_JUMP_ADDR*.
 
 * **FW_JUMP_FDT_ADDR** - Address where the *flattened device tree (FDT file)*
   passed by the prior booting stage will be placed in memory before executing
@@ -57,6 +62,12 @@ follows:
   echo fdt overlaps kernel, increase FW_JUMP_FDT_ADDR
   ```
 
+* **FW_JUMP_FDT_OFFSET** - Address offset from the opensbi load address where
+  the FDT will be passed to the next booting stage. This offset is used
+  as relocatable address of the FDT passed to the next booting stage. If
+  *FW_JUMP_FDT_ADDR* is also defined, the firmware will prefer
+  *FW_JUMP_FDT_ADDR*.
+
 *FW_JUMP* Example
 -----------------
 

docs/firmware/fw_payload.md

@@ -23,7 +23,7 @@ The *FW_PAYLOAD* firmware can be enabled by any of the following methods:
 2. Specifying `FW_PAYLOAD=y` in the target platform *objects.mk* configuration
    file.
 
-The compiled *FW_PAYLOAD* firmware ELF file is named *fw_jump.elf*. Its
+The compiled *FW_PAYLOAD* firmware ELF file is named *fw_payload.elf*. Its
 expanded image file is *fw_payload.bin*. Both files are created in the
 platform-specific build directory under the
 *build/platform/<platform_subdir>/firmware* directory.
@@ -36,8 +36,8 @@ options. These configuration parameters can be defined using either the top
 level `make` command line or the target platform *objects.mk* configuration
 file. The parameters currently defined are as follows:
 
-* **FW_PAYLOAD_OFFSET** - Offset from *FW_TEXT_START* where the payload binary
-  will be linked in the final *FW_PAYLOAD* firmware binary image. This
+* **FW_PAYLOAD_OFFSET** - Offset from the opensbi load address where the payload
+  binary will be linked in the final *FW_PAYLOAD* firmware binary image. This
   configuration parameter is mandatory if *FW_PAYLOAD_ALIGN* is not defined.
   Compilation errors will result from an incorrect definition of
   *FW_PAYLOAD_OFFSET* or of *FW_PAYLOAD_ALIGN*, or if neither of these
@@ -62,6 +62,11 @@ file. The parameters currently defined are as follows:
   firmware will pass the FDT address passed by the previous booting stage
   to the next booting stage.
 
+* **FW_PAYLOAD_FDT_OFFSET** - Address offset from the opensbi load address where
+  the FDT will be passed to the next booting stage. This offset is used as
+  relocatable address of the FDT passed to the next booting stage. If
+  *FW_PAYLOAD_FDT_ADDR* is also defined, the firmware will prefer *FW_PAYLOAD_FDT_ADDR*.
+
 *FW_PAYLOAD* Example
 --------------------
 

docs/opensbi_config.md

@@ -0,0 +1,87 @@
+OpenSBI Device Tree Configuration Guideline
+==================================
+
+Some configurations of OpenSBI's Generic Platform can be described
+in the **device tree (DT) blob** (or flattened device tree) passed
+to the OpenSBI firmwares by the previous booting stage. OpenSBI will
+parse and use these configurations during the boot phase, but delete
+them from the device tree at the end of cold boot.
+
+### OpenSBI Configuration Node
+
+All nodes related to OpenSBI configuration should be under the OpenSBI
+configuration DT node. The **/chosen** DT node is the preferred parent
+of the OpenSBI configuration DT node.
+
+The DT properties of a domain configuration DT node are as follows:
+
+* **compatible** (Mandatory) - The compatible string of the OpenSBI
+  configuration. This DT property should have value *"opensbi,config"*
+
+* **cold-boot-harts** (Optional) - If a platform lacks an override
+  cold_boot_allowed() mechanism, this DT property specifies that a
+  set of harts is permitted to perform a cold boot. Otherwise, all
+  harts are allowed to cold boot.
+
+* **system-suspend-test** (Optional) - When present, enable a system
+  suspend test implementation which simply waits five seconds and issues a WFI.
+
+The OpenSBI Configuration Node will be deleted at the end of cold boot
+(replace the node (subtree) with nop tags).
+
+### Example
+
+```text
+    chosen {
+        opensbi-config {
+            compatible = "opensbi,config";
+            cold-boot-harts = <&cpu1 &cpu2 &cpu3 &cpu4>;
+            system-suspend-test;
+        };
+    };
+
+    cpus {
+        #address-cells = <1>;
+        #size-cells = <0>;
+        timebase-frequency = <10000000>;
+
+        cpu0: cpu@0 {
+            device_type = "cpu";
+            reg = <0x00>;
+            compatible = "riscv";
+            ...
+        };
+
+        cpu1: cpu@1 {
+            device_type = "cpu";
+            reg = <0x01>;
+            compatible = "riscv";
+            ...
+        };
+
+        cpu2: cpu@2 {
+            device_type = "cpu";
+            reg = <0x02>;
+            compatible = "riscv";
+            ...
+        };
+
+        cpu3: cpu@3 {
+            device_type = "cpu";
+            reg = <0x03>;
+            compatible = "riscv";
+            ...
+        };
+
+        cpu4: cpu@4 {
+            device_type = "cpu";
+            reg = <0x04>;
+            compatible = "riscv";
+            ...
+        };
+    };
+
+    uart1: serial@10011000 {
+        ...
+    };
+```

docs/platform/generic.md

@@ -9,10 +9,9 @@ boards.
 
 By default, the generic FDT platform makes following assumptions:
 
-1. platform FW_TEXT_START is 0x80000000
-2. platform features are default
-3. platform stack size is default
-4. platform has no quirks or work-arounds
+1. platform features are default
+2. platform stack size is default
+3. platform has no quirks or work-arounds
 
 The above assumptions (except 1) can be overridden by adding special platform
 callbacks which will be called based on FDT root node compatible string.
@@ -33,10 +32,6 @@ Users of the generic FDT platform will have to ensure that:
 To build the platform-specific library and firmware images, provide the
 *PLATFORM=generic* parameter to the top level `make` command.
 
-For custom FW_TEXT_START, we can build the platform-specific library and
-firmware images by passing *PLATFORM=generic FW_TEXT_START=<custom_text_start>*
-parameter to the top level `make` command.
-
 Platform Options
 ----------------
 

docs/writing_tests.md

@@ -0,0 +1,129 @@
+Writing tests for OpenSBI
+=========================
+
+SBIUnit
+-------
+SBIUnit is a set of macros and functions which simplify the test development and
+automate the test execution and evaluation. All of the SBIUnit definitions are
+in the `include/sbi/sbi_unit_test.h` header file, and implementations are
+available in `lib/sbi/tests/sbi_unit_test.c`.
+
+Simple SBIUnit test
+-------------------
+
+For instance, we would like to test the following function from
+`lib/sbi/sbi_string.c`:
+
+```c
+size_t sbi_strlen(const char *str)
+{
+	unsigned long ret = 0;
+
+	while (*str != '\0') {
+		ret++;
+		str++;
+	}
+
+	return ret;
+}
+```
+
+which calculates the string length.
+
+Create the file `lib/sbi/tests/sbi_string_test.c` with the following content:
+
+```c
+#include <sbi/sbi_unit_test.h>
+#include <sbi/sbi_string.h>
+
+static void strlen_test(struct sbiunit_test_case *test)
+{
+	SBIUNIT_EXPECT_EQ(test, sbi_strlen("Hello"), 5);
+	SBIUNIT_EXPECT_EQ(test, sbi_strlen("Hell\0o"), 4);
+}
+
+static struct sbiunit_test_case string_test_cases[] = {
+	SBIUNIT_TEST_CASE(strlen_test),
+	SBIUNIT_END_CASE,
+};
+
+SBIUNIT_TEST_SUITE(string_test_suite, string_test_cases);
+```
+
+Then, add the corresponding Makefile entries to `lib/sbi/tests/objects.mk`:
+```lang-makefile
+...
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += string_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_string_test.o
+```
+
+If you compiled OpenSBI with CONFIG_SBIUNIT enabled before, you may need to
+manually remove the build folder in order to regenerate the carray files:
+`rm -rf build/`.
+
+Recompile OpenSBI with the CONFIG_SBIUNIT option enabled and run it in QEMU.
+You will see something like this:
+```
+# make PLATFORM=generic run
+...
+# Running SBIUNIT tests #
+...
+## Running test suite: string_test_suite
+[PASSED] strlen_test
+1 PASSED / 0 FAILED / 1 TOTAL
+```
+
+Now let's try to change this test in the way that it will fail:
+
+```c
+- SBIUNIT_EXPECT_EQ(test, sbi_strlen("Hello"), 5);
++ SBIUNIT_EXPECT_EQ(test, sbi_strlen("Hello"), 100);
+```
+
+`make all` and `make run` it again:
+```
+...
+# Running SBIUNIT tests #
+...
+## Running test suite: string_test_suite
+[SBIUnit] [.../opensbi/lib/sbi/tests/sbi_string_test.c:6]: strlen_test: Condition "(sbi_strlen("Hello")) == (100)" expected to be true!
+[FAILED] strlen_test
+0 PASSED / 1 FAILED / 1 TOTAL
+```
+Covering the static functions / using the static definitions
+------------------------------------------------------------
+
+SBIUnit also allows you to test static functions. In order to do so, simply
+include your test source in the file you would like to test. Complementing the
+example above, just add this to the `lib/sbi/sbi_string.c` file:
+
+```c
+#ifdef CONFIG_SBIUNIT
+#include "tests/sbi_string_test.c"
+#endif
+```
+
+In this case you should only add a new carray entry pointing to the test suite
+to `lib/sbi/tests/objects.mk`:
+```lang-makefile
+...
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += string_test_suite
+```
+
+You don't have to compile the `sbi_string_test.o` separately, because the
+test code will be included into the `sbi_string` object file.
+
+"Mocking" the structures
+------------------------
+See the example of structure "mocking" in `lib/sbi/tests/sbi_console_test.c`,
+where the sbi_console_device structure was mocked to be used in various
+console-related functions in order to test them.
+
+API Reference
+-------------
+All of the `SBIUNIT_EXPECT_*` macros will cause a test case to fail if the
+corresponding conditions are not met, however, the execution of a particular
+test case will not be stopped.
+
+All of the `SBIUNIT_ASSERT_*` macros will cause a test case to fail and stop
+immediately, triggering a panic.

firmware/fw_base.S

@@ -14,7 +14,7 @@
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_trap.h>
 
-#define BOOT_STATUS_RELOCATE_DONE	1
+#define BOOT_STATUS_LOTTERY_DONE	1
 #define BOOT_STATUS_BOOT_HART_DONE	2
 
 .macro	MOV_3R __d0, __s0, __d1, __s1, __d2, __s2
@@ -31,17 +31,6 @@
 	add	\__d4, \__s4, zero
 .endm
 
-/*
- * If __start_reg <= __check_reg and __check_reg < __end_reg then
- *   jump to __pass
- */
-.macro BRANGE __start_reg, __end_reg, __check_reg, __jump_lable
-	blt	\__check_reg, \__start_reg, 999f
-	bge	\__check_reg, \__end_reg, 999f
-	j	\__jump_lable
-999:
-.endm
-
 	.section .entry, "ax", %progbits
 	.align 3
 	.globl _start
@@ -55,27 +44,18 @@ _start:
 	li	a7, -1
 	beq	a6, a7, _try_lottery
 	/* Jump to relocation wait loop if we are not boot hart */
-	bne	a0, a6, _wait_relocate_copy_done
+	bne	a0, a6, _wait_for_boot_hart
 _try_lottery:
 	/* Jump to relocation wait loop if we don't get relocation lottery */
-	lla	a6, _relocate_lottery
-	li	a7, 1
-	amoadd.w a6, a7, (a6)
-	bnez	a6, _wait_relocate_copy_done
+	lla	a6, _boot_status
+	li	a7, BOOT_STATUS_LOTTERY_DONE
+	amoswap.w a6, a7, (a6)
+	bnez	a6, _wait_for_boot_hart
 
-	/* Save load address */
-	lla	t0, _load_start
-	lla	t1, _fw_start
-	REG_S	t1, 0(t0)
-
-#ifdef FW_PIC
 	/* relocate the global table content */
-	lla	t0, _link_start
-	REG_L	t0, 0(t0)
-	/* t1 shall has the address of _fw_start */
-	sub	t2, t1, t0
-	lla	t3, _runtime_offset
-	REG_S	t2, (t3)
+	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
 	beq	t0, t1, _relocate_done
@@ -92,103 +72,10 @@ _try_lottery:
 3:
 	addi	t0, t0, (REGBYTES * 3)
 	blt	t0, t1, 2b
-	j	_relocate_done
-_wait_relocate_copy_done:
-	j	_wait_for_boot_hart
-#else
-	/* Relocate if load address != link address */
-_relocate:
-	lla	t0, _link_start
-	REG_L	t0, 0(t0)
-	lla	t1, _link_end
-	REG_L	t1, 0(t1)
-	lla	t2, _load_start
-	REG_L	t2, 0(t2)
-	beq	t0, t2, _relocate_done
-	sub	t3, t1, t0
-	add	t3, t3, t2
-	lla	t4, _relocate_done
-	sub	t4, t4, t2
-	add	t4, t4, t0
-	blt	t2, t0, _relocate_copy_to_upper
-_relocate_copy_to_lower:
-	ble	t1, t2, _relocate_copy_to_lower_loop
-	lla	t3, _relocate_lottery
-	BRANGE	t2, t1, t3, _start_hang
-	lla	t3, _boot_status
-	BRANGE	t2, t1, t3, _start_hang
-	lla	t3, _relocate
-	lla	t5, _relocate_done
-	BRANGE	t2, t1, t3, _start_hang
-	BRANGE	t2, t1, t5, _start_hang
-	BRANGE  t3, t5, t2, _start_hang
-_relocate_copy_to_lower_loop:
-	REG_L	t3, 0(t2)
-	REG_S	t3, 0(t0)
-	add	t0, t0, __SIZEOF_POINTER__
-	add	t2, t2, __SIZEOF_POINTER__
-	blt	t0, t1, _relocate_copy_to_lower_loop
-	jr	t4
-_relocate_copy_to_upper:
-	ble	t3, t0, _relocate_copy_to_upper_loop
-	lla	t2, _relocate_lottery
-	BRANGE	t0, t3, t2, _start_hang
-	lla	t2, _boot_status
-	BRANGE	t0, t3, t2, _start_hang
-	lla	t2, _relocate
-	lla	t5, _relocate_done
-	BRANGE	t0, t3, t2, _start_hang
-	BRANGE	t0, t3, t5, _start_hang
-	BRANGE	t2, t5, t0, _start_hang
-_relocate_copy_to_upper_loop:
-	add	t3, t3, -__SIZEOF_POINTER__
-	add	t1, t1, -__SIZEOF_POINTER__
-	REG_L	t2, 0(t3)
-	REG_S	t2, 0(t1)
-	blt	t0, t1, _relocate_copy_to_upper_loop
-	jr	t4
-_wait_relocate_copy_done:
-	lla	t0, _fw_start
-	lla	t1, _link_start
-	REG_L	t1, 0(t1)
-	beq	t0, t1, _wait_for_boot_hart
-	lla	t2, _boot_status
-	lla	t3, _wait_for_boot_hart
-	sub	t3, t3, t0
-	add	t3, t3, t1
-1:
-	/* waitting for relocate copy done (_boot_status == 1) */
-	li	t4, BOOT_STATUS_RELOCATE_DONE
-	REG_L	t5, 0(t2)
-	/* Reduce the bus traffic so that boot hart may proceed faster */
-	nop
-	nop
-	nop
-	bgt     t4, t5, 1b
-	jr	t3
-#endif
 _relocate_done:
-
-	/*
-	 * Mark relocate copy done
-	 * Use _boot_status copy relative to the load address
-	 */
-	lla	t0, _boot_status
-#ifndef FW_PIC
-	lla	t1, _link_start
-	REG_L	t1, 0(t1)
-	lla	t2, _load_start
-	REG_L	t2, 0(t2)
-	sub	t0, t0, t1
-	add	t0, t0, t2
-#endif
-	li	t1, BOOT_STATUS_RELOCATE_DONE
-	REG_S	t1, 0(t0)
-	fence	rw, rw
-
 	/* At this point we are running from link address */
 
-	/* Reset all registers for boot HART */
+	/* Reset all registers except ra, a0, a1, a2, a3 and a4 for boot HART */
 	li	ra, 0
 	call	_reset_regs
 
@@ -319,10 +206,8 @@ _scratch_init:
 	/* Store hartid-to-scratch function address in scratch space */
 	lla	a4, _hartid_to_scratch
 	REG_S	a4, SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET(tp)
-	/* Store trap-exit function address in scratch space */
-	lla	a4, _trap_exit
-	REG_S	a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(tp)
-	/* Clear tmp0 in scratch space */
+	/* Clear trap_context and tmp0 in scratch space */
+	REG_S	zero, SBI_SCRATCH_TRAP_CONTEXT_OFFSET(tp)
 	REG_S	zero, SBI_SCRATCH_TMP0_OFFSET(tp)
 	/* Store firmware options in scratch space */
 	MOV_3R	s0, a0, s1, a1, s2, a2
@@ -358,7 +243,7 @@ _scratch_init:
 	/* t0 = source FDT start address */
 	add	t0, a1, zero
 	/* t2 = source FDT size in big-endian */
-#if __riscv_xlen == 64
+#if __riscv_xlen > 32
 	lwu	t2, 4(t0)
 #else
 	lw	t2, 4(t0)
@@ -415,7 +300,7 @@ _wait_for_boot_hart:
 	bne	t0, t1, _wait_for_boot_hart
 
 _start_warm:
-	/* Reset all registers for non-boot HARTs */
+	/* Reset all registers except ra, a0, a1, a2, a3 and a4 for non-boot HART */
 	li	ra, 0
 	call	_reset_regs
 
@@ -424,7 +309,7 @@ _start_warm:
 
 	/* Find HART count and HART stack size */
 	lla	a4, platform
-#if __riscv_xlen == 64
+#if __riscv_xlen > 32
 	lwu	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
 	lwu	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
 #else
@@ -440,7 +325,7 @@ _start_warm:
 	beqz	s9, 3f
 	li	a4, 0
 1:
-#if __riscv_xlen == 64
+#if __riscv_xlen > 32
 	lwu	a5, (s9)
 #else
 	lw	a5, (s9)
@@ -469,28 +354,14 @@ _start_warm:
 
 	/* Setup trap handler */
 	lla	a4, _trap_handler
-#if __riscv_xlen == 32
 	csrr	a5, CSR_MISA
 	srli	a5, a5, ('H' - 'A')
 	andi	a5, a5, 0x1
-	beq	a5, zero, _skip_trap_handler_rv32_hyp
-	lla	a4, _trap_handler_rv32_hyp
-_skip_trap_handler_rv32_hyp:
-#endif
+	beq	a5, zero, _skip_trap_handler_hyp
+	lla	a4, _trap_handler_hyp
+_skip_trap_handler_hyp:
 	csrw	CSR_MTVEC, a4
 
-#if __riscv_xlen == 32
-	/* Override trap exit for H-extension */
-	csrr	a5, CSR_MISA
-	srli	a5, a5, ('H' - 'A')
-	andi	a5, a5, 0x1
-	beq	a5, zero, _skip_trap_exit_rv32_hyp
-	lla	a4, _trap_exit_rv32_hyp
-	csrr	a5, CSR_MSCRATCH
-	REG_S	a4, SBI_SCRATCH_TRAP_EXIT_OFFSET(a5)
-_skip_trap_exit_rv32_hyp:
-#endif
-
 	/* Initialize SBI runtime */
 	csrr	a0, CSR_MSCRATCH
 	call	sbi_init
@@ -500,20 +371,8 @@ _skip_trap_exit_rv32_hyp:
 
 	.data
 	.align 3
-#ifdef FW_PIC
-_runtime_offset:
-	RISCV_PTR	0
-#endif
-_relocate_lottery:
-	RISCV_PTR	0
 _boot_status:
 	RISCV_PTR	0
-_load_start:
-	RISCV_PTR	_fw_start
-_link_start:
-	RISCV_PTR	FW_TEXT_START
-_link_end:
-	RISCV_PTR	_fw_reloc_end
 
 	.section .entry, "ax", %progbits
 	.align 3
@@ -527,7 +386,7 @@ _hartid_to_scratch:
 	 * t2 -> Temporary
 	 */
 	lla	t2, platform
-#if __riscv_xlen == 64
+#if __riscv_xlen > 32
 	lwu	t0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(t2)
 	lwu	t2, SBI_PLATFORM_HART_COUNT_OFFSET(t2)
 #else
@@ -611,10 +470,10 @@ memcmp:
 	xor	t0, tp, t0
 
 	/* Save original SP on exception stack */
-	REG_S	sp, (SBI_TRAP_REGS_OFFSET(sp) - SBI_TRAP_REGS_SIZE)(t0)
+	REG_S	sp, (SBI_TRAP_REGS_OFFSET(sp) - SBI_TRAP_CONTEXT_SIZE)(t0)
 
-	/* Set SP to exception stack and make room for trap registers */
-	add	sp, t0, -(SBI_TRAP_REGS_SIZE)
+	/* Set SP to exception stack and make room for trap context */
+	add	sp, t0, -(SBI_TRAP_CONTEXT_SIZE)
 
 	/* Restore T0 from scratch space */
 	REG_L	t0, SBI_SCRATCH_TMP0_OFFSET(tp)
@@ -674,6 +533,32 @@ memcmp:
 	REG_S	t6, SBI_TRAP_REGS_OFFSET(t6)(sp)
 .endm
 
+.macro	TRAP_SAVE_INFO have_mstatush have_h_extension
+	csrr	t0, CSR_MCAUSE
+	REG_S	t0, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(cause))(sp)
+	csrr	t0, CSR_MTVAL
+	REG_S	t0, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(tval))(sp)
+.if \have_h_extension
+	csrr	t0, CSR_MTVAL2
+	REG_S	t0, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(tval2))(sp)
+	csrr	t0, CSR_MTINST
+	REG_S	t0, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(tinst))(sp)
+	.if \have_mstatush
+	csrr	t0, CSR_MSTATUSH
+	srli	t0, t0, MSTATUSH_GVA_SHIFT
+	.else
+	csrr	t0, CSR_MSTATUS
+	srli	t0, t0, MSTATUS_GVA_SHIFT
+	.endif
+	and	t0, t0, 0x1
+.else
+	REG_S	zero, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(tval2))(sp)
+	REG_S	zero, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(tinst))(sp)
+	li	t0, 0
+.endif
+	REG_S	t0, (SBI_TRAP_REGS_SIZE + SBI_TRAP_INFO_OFFSET(gva))(sp)
+.endm
+
 .macro	TRAP_CALL_C_ROUTINE
 	/* Call C routine */
 	add	a0, sp, zero
@@ -736,7 +621,6 @@ memcmp:
 	.section .entry, "ax", %progbits
 	.align 3
 	.globl _trap_handler
-	.globl _trap_exit
 _trap_handler:
 	TRAP_SAVE_AND_SETUP_SP_T0
 
@@ -744,9 +628,10 @@ _trap_handler:
 
 	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
 
+	TRAP_SAVE_INFO 0 0
+
 	TRAP_CALL_C_ROUTINE
 
-_trap_exit:
 	TRAP_RESTORE_GENERAL_REGS_EXCEPT_A0_T0
 
 	TRAP_RESTORE_MEPC_MSTATUS 0
@@ -755,29 +640,39 @@ _trap_exit:
 
 	mret
 
-#if __riscv_xlen == 32
 	.section .entry, "ax", %progbits
 	.align 3
-	.globl _trap_handler_rv32_hyp
-	.globl _trap_exit_rv32_hyp
-_trap_handler_rv32_hyp:
+	.globl _trap_handler_hyp
+_trap_handler_hyp:
 	TRAP_SAVE_AND_SETUP_SP_T0
 
+#if __riscv_xlen == 32
 	TRAP_SAVE_MEPC_MSTATUS 1
+#else
+	TRAP_SAVE_MEPC_MSTATUS 0
+#endif
 
 	TRAP_SAVE_GENERAL_REGS_EXCEPT_SP_T0
 
+#if __riscv_xlen == 32
+	TRAP_SAVE_INFO 1 1
+#else
+	TRAP_SAVE_INFO 0 1
+#endif
+
 	TRAP_CALL_C_ROUTINE
 
-_trap_exit_rv32_hyp:
 	TRAP_RESTORE_GENERAL_REGS_EXCEPT_A0_T0
 
+#if __riscv_xlen == 32
 	TRAP_RESTORE_MEPC_MSTATUS 1
+#else
+	TRAP_RESTORE_MEPC_MSTATUS 0
+#endif
 
 	TRAP_RESTORE_A0_T0
 
 	mret
-#endif
 
 	.section .entry, "ax", %progbits
 	.align 3
@@ -786,7 +681,7 @@ _reset_regs:
 
 	/* flush the instruction cache */
 	fence.i
-	/* Reset all registers except ra, a0, a1 and a2 */
+	/* Reset all registers except ra, a0, a1, a2, a3 and a4 */
 	li sp, 0
 	li gp, 0
 	li tp, 0
@@ -795,8 +690,6 @@ _reset_regs:
 	li t2, 0
 	li s0, 0
 	li s1, 0
-	li a3, 0
-	li a4, 0
 	li a5, 0
 	li a6, 0
 	li a7, 0

firmware/fw_base.ldS

@@ -38,6 +38,11 @@
 		. = ALIGN(8);
 	}
 
+	.dynsym :
+	{
+		*(.dynsym)
+	}
+
 	. = ALIGN(0x1000); /* Ensure next section is page aligned */
 
 	.rela.dyn : {

firmware/fw_dynamic.S

@@ -11,12 +11,6 @@
 
 #include "fw_base.S"
 
-	.section .entry, "ax", %progbits
-	.align 3
-_bad_dynamic_info:
-	wfi
-	j	_bad_dynamic_info
-
 	.section .entry, "ax", %progbits
 	.align 3
 	.global fw_boot_hart
@@ -30,10 +24,10 @@ fw_boot_hart:
 	/* Sanity checks */
 	li	a1, FW_DYNAMIC_INFO_MAGIC_VALUE
 	REG_L	a0, FW_DYNAMIC_INFO_MAGIC_OFFSET(a2)
-	bne	a0, a1, _bad_dynamic_info
+	bne	a0, a1, _start_hang
 	li	a1, FW_DYNAMIC_INFO_VERSION_MAX
 	REG_L	a0, FW_DYNAMIC_INFO_VERSION_OFFSET(a2)
-	bgt	a0, a1, _bad_dynamic_info
+	bgt	a0, a1, _start_hang
 
 	/* Read boot HART id */
 	li	a1, FW_DYNAMIC_INFO_VERSION_2

firmware/fw_jump.S

@@ -46,6 +46,10 @@ fw_save_info:
 fw_next_arg1:
 #ifdef FW_JUMP_FDT_ADDR
 	li	a0, FW_JUMP_FDT_ADDR
+#elif defined(FW_JUMP_FDT_OFFSET)
+	lla	a0, _fw_start
+	li	a1, FW_JUMP_FDT_OFFSET
+	add	a0, a0, a1
 #else
 	add	a0, a1, zero
 #endif
@@ -59,8 +63,16 @@ fw_next_arg1:
 	 * The next address should be returned in 'a0'.
 	 */
 fw_next_addr:
+#ifdef FW_JUMP_ADDR
 	lla	a0, _jump_addr
 	REG_L	a0, (a0)
+#elif defined(FW_JUMP_OFFSET)
+	lla	a0, _fw_start
+	li	a1, FW_JUMP_OFFSET
+	add	a0, a0, a1
+#else
+#error "Must define at least FW_JUMP_ADDR or FW_JUMP_OFFSET"
+#endif
 	ret
 
 	.section .entry, "ax", %progbits
@@ -86,11 +98,9 @@ fw_options:
 	add	a0, zero, zero
 	ret
 
-#ifndef FW_JUMP_ADDR
-#error "Must define FW_JUMP_ADDR"
-#endif
-
+#ifdef FW_JUMP_ADDR
 	.section .rodata
 	.align 3
 _jump_addr:
 	RISCV_PTR FW_JUMP_ADDR
+#endif

firmware/fw_payload.S

@@ -46,6 +46,10 @@ fw_save_info:
 fw_next_arg1:
 #ifdef FW_PAYLOAD_FDT_ADDR
 	li	a0, FW_PAYLOAD_FDT_ADDR
+#elif defined(FW_PAYLOAD_FDT_OFFSET)
+	lla	a0, _fw_start
+	li	a1, FW_PAYLOAD_FDT_OFFSET
+	add	a0, a0, a1
 #else
 	add	a0, a1, zero
 #endif

firmware/objects.mk

@@ -13,19 +13,10 @@ firmware-cflags-y +=
 firmware-asflags-y +=
 firmware-ldflags-y +=
 
-ifndef FW_PIC
-FW_PIC := $(OPENSBI_LD_PIE)
-endif
-
-ifeq ($(FW_PIC),y)
-firmware-genflags-y +=	-DFW_PIC
-firmware-asflags-y  +=	-fpic
-firmware-cflags-y   +=	-fPIE -pie
-firmware-ldflags-y  +=	-Wl,--no-dynamic-linker -Wl,-pie
-endif
-
 ifdef FW_TEXT_START
 firmware-genflags-y += -DFW_TEXT_START=$(FW_TEXT_START)
+else
+firmware-genflags-y += -DFW_TEXT_START=0x0
 endif
 
 ifdef FW_FDT_PATH
@@ -38,9 +29,15 @@ endif
 firmware-bins-$(FW_DYNAMIC) += fw_dynamic.bin
 
 firmware-bins-$(FW_JUMP) += fw_jump.bin
+ifdef FW_JUMP_OFFSET
+firmware-genflags-$(FW_JUMP) += -DFW_JUMP_OFFSET=$(FW_JUMP_OFFSET)
+endif
 ifdef FW_JUMP_ADDR
 firmware-genflags-$(FW_JUMP) += -DFW_JUMP_ADDR=$(FW_JUMP_ADDR)
 endif
+ifdef FW_JUMP_FDT_OFFSET
+firmware-genflags-$(FW_JUMP) += -DFW_JUMP_FDT_OFFSET=$(FW_JUMP_FDT_OFFSET)
+endif
 ifdef FW_JUMP_FDT_ADDR
 firmware-genflags-$(FW_JUMP) += -DFW_JUMP_FDT_ADDR=$(FW_JUMP_FDT_ADDR)
 endif
@@ -59,6 +56,9 @@ ifdef FW_PAYLOAD_ALIGN
 firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_ALIGN=$(FW_PAYLOAD_ALIGN)
 endif
 
+ifdef FW_PAYLOAD_FDT_OFFSET
+firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_FDT_OFFSET=$(FW_PAYLOAD_FDT_OFFSET)
+endif
 ifdef FW_PAYLOAD_FDT_ADDR
 firmware-genflags-$(FW_PAYLOAD) += -DFW_PAYLOAD_FDT_ADDR=$(FW_PAYLOAD_FDT_ADDR)
 endif

include/sbi/riscv_barrier.h

@@ -40,7 +40,11 @@
 #define smp_wmb()		RISCV_FENCE(w,w)
 
 /* CPU relax for busy loop */
-#define cpu_relax()		asm volatile ("" : : : "memory")
+#define cpu_relax()						\
+do {								\
+	unsigned long __t;					\
+	__asm__ __volatile__ ("div %0, %0, zero" : "=r" (__t));	\
+} while (0)
 
 /* clang-format on */
 

include/sbi/riscv_dbtr.h

@@ -0,0 +1,249 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Ventana Micro System, Inc.
+ *
+ * Authors:
+ *   Himanshu Chauhan <hchauhan@ventanamicro.com>
+ */
+
+#ifndef __RISCV_DBTR_H__
+#define __RISCV_DBTR_H__
+
+#define RV_MAX_TRIGGERS	32
+
+enum {
+	RISCV_DBTR_TRIG_NONE = 0,
+	RISCV_DBTR_TRIG_LEGACY,
+	RISCV_DBTR_TRIG_MCONTROL,
+	RISCV_DBTR_TRIG_ICOUNT,
+	RISCV_DBTR_TRIG_ITRIGGER,
+	RISCV_DBTR_TRIG_ETRIGGER,
+	RISCV_DBTR_TRIG_MCONTROL6,
+};
+
+#define RV_DBTR_BIT(_prefix, _name)		\
+	RV_DBTR_##_prefix##_##_name##_BIT
+
+#define RV_DBTR_BIT_MASK(_prefix, _name)	\
+	RV_DBTR_##_prefix##_name##_BIT_MASK
+
+#define RV_DBTR_DECLARE_BIT(_prefix, _name, _val)	\
+	RV_DBTR_BIT(_prefix, _name) = _val
+
+#define RV_DBTR_DECLARE_BIT_MASK(_prefix, _name, _width)		\
+	RV_DBTR_BIT_MASK(_prefix, _name) =				\
+		(((1UL << _width) - 1) << RV_DBTR_BIT(_prefix, _name))
+
+#define CLEAR_DBTR_BIT(_target, _prefix, _bit_name)		\
+	__clear_bit(RV_DBTR_BIT(_prefix, _bit_name), &_target)
+
+#define SET_DBTR_BIT(_target, _prefix, _bit_name)		\
+	__set_bit(RV_DBTR_BIT(_prefix, _bit_name), &_target)
+
+/* Trigger Data 1 */
+enum {
+	RV_DBTR_DECLARE_BIT(TDATA1, DATA,   0),
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT(TDATA1, DMODE,  59),
+	RV_DBTR_DECLARE_BIT(TDATA1, TYPE,   60),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT(TDATA1, DMODE,  27),
+	RV_DBTR_DECLARE_BIT(TDATA1, TYPE,   28),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
+enum {
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT_MASK(TDATA1, DATA,  59),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT_MASK(TDATA1, DATA,  27),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+	RV_DBTR_DECLARE_BIT_MASK(TDATA1, DMODE, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TDATA1, TYPE,  4),
+};
+
+/* MC - Match Control Type Register */
+enum {
+	RV_DBTR_DECLARE_BIT(MC, LOAD,    0),
+	RV_DBTR_DECLARE_BIT(MC, STORE,   1),
+	RV_DBTR_DECLARE_BIT(MC, EXEC,    2),
+	RV_DBTR_DECLARE_BIT(MC, U,       3),
+	RV_DBTR_DECLARE_BIT(MC, S,       4),
+	RV_DBTR_DECLARE_BIT(MC, RES2,    5),
+	RV_DBTR_DECLARE_BIT(MC, M,       6),
+	RV_DBTR_DECLARE_BIT(MC, MATCH,   7),
+	RV_DBTR_DECLARE_BIT(MC, CHAIN,   11),
+	RV_DBTR_DECLARE_BIT(MC, ACTION,  12),
+	RV_DBTR_DECLARE_BIT(MC, SIZELO,  16),
+	RV_DBTR_DECLARE_BIT(MC, TIMING,  18),
+	RV_DBTR_DECLARE_BIT(MC, SELECT,  19),
+	RV_DBTR_DECLARE_BIT(MC, HIT,     20),
+#if __riscv_xlen >= 64
+	RV_DBTR_DECLARE_BIT(MC, SIZEHI,  21),
+#endif
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT(MC, MASKMAX, 53),
+	RV_DBTR_DECLARE_BIT(MC, DMODE,   59),
+	RV_DBTR_DECLARE_BIT(MC, TYPE,    60),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT(MC, MASKMAX, 21),
+	RV_DBTR_DECLARE_BIT(MC, DMODE,   27),
+	RV_DBTR_DECLARE_BIT(MC, TYPE,    28),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
+enum {
+	RV_DBTR_DECLARE_BIT_MASK(MC, LOAD,    1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, STORE,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, EXEC,    1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, U,       1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, S,       1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, RES2,    1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, M,       1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, MATCH,   4),
+	RV_DBTR_DECLARE_BIT_MASK(MC, CHAIN,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, ACTION,  4),
+	RV_DBTR_DECLARE_BIT_MASK(MC, SIZELO,  2),
+	RV_DBTR_DECLARE_BIT_MASK(MC, TIMING,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, SELECT,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, HIT,     1),
+#if __riscv_xlen >= 64
+	RV_DBTR_DECLARE_BIT_MASK(MC, SIZEHI,  2),
+#endif
+	RV_DBTR_DECLARE_BIT_MASK(MC, MASKMAX, 6),
+	RV_DBTR_DECLARE_BIT_MASK(MC, DMODE,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC, TYPE,    4),
+};
+
+/* MC6 - Match Control 6 Type Register */
+enum {
+	RV_DBTR_DECLARE_BIT(MC6, LOAD,   0),
+	RV_DBTR_DECLARE_BIT(MC6, STORE,  1),
+	RV_DBTR_DECLARE_BIT(MC6, EXEC,   2),
+	RV_DBTR_DECLARE_BIT(MC6, U,      3),
+	RV_DBTR_DECLARE_BIT(MC6, S,      4),
+	RV_DBTR_DECLARE_BIT(MC6, RES2,   5),
+	RV_DBTR_DECLARE_BIT(MC6, M,      6),
+	RV_DBTR_DECLARE_BIT(MC6, MATCH,  7),
+	RV_DBTR_DECLARE_BIT(MC6, CHAIN,  11),
+	RV_DBTR_DECLARE_BIT(MC6, ACTION, 12),
+	RV_DBTR_DECLARE_BIT(MC6, SIZE,   16),
+	RV_DBTR_DECLARE_BIT(MC6, TIMING, 20),
+	RV_DBTR_DECLARE_BIT(MC6, SELECT, 21),
+	RV_DBTR_DECLARE_BIT(MC6, HIT,    22),
+	RV_DBTR_DECLARE_BIT(MC6, VU,     23),
+	RV_DBTR_DECLARE_BIT(MC6, VS,     24),
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT(MC6, DMODE,  59),
+	RV_DBTR_DECLARE_BIT(MC6, TYPE,   60),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT(MC6, DMODE,  27),
+	RV_DBTR_DECLARE_BIT(MC6, TYPE,   28),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
+enum {
+	RV_DBTR_DECLARE_BIT_MASK(MC6, LOAD,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, STORE,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, EXEC,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, U,      1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, S,      1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, RES2,   1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, M,      1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, MATCH,  4),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, CHAIN,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, ACTION, 4),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, SIZE,   4),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, TIMING, 1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, SELECT, 1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, HIT,    1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, VU,     1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, VS,     1),
+#if __riscv_xlen == 64
+	RV_DBTR_DECLARE_BIT_MASK(MC6, DMODE,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, TYPE,   4),
+#elif __riscv_xlen == 32
+	RV_DBTR_DECLARE_BIT_MASK(MC6, DMODE,  1),
+	RV_DBTR_DECLARE_BIT_MASK(MC6, TYPE,   4),
+#else
+	#error "Unknown __riscv_xlen"
+#endif
+};
+
+#define RV_DBTR_SET_TDATA1_TYPE(_t1, _type)				\
+	do {								\
+		_t1 &= ~RV_DBTR_BIT_MASK(TDATA1, TYPE);			\
+		_t1 |= (((unsigned long)_type				\
+			 << RV_DBTR_BIT(TDATA1, TYPE))			\
+			& RV_DBTR_BIT_MASK(TDATA1, TYPE));		\
+	}while (0);
+
+#define RV_DBTR_SET_MC_TYPE(_t1, _type)				\
+	do {							\
+		_t1 &= ~RV_DBTR_BIT_MASK(MC, TYPE);		\
+		_t1 |= (((unsigned long)_type			\
+			 << RV_DBTR_BIT(MC, TYPE))		\
+			& RV_DBTR_BIT_MASK(MC, TYPE));		\
+	}while (0);
+
+#define RV_DBTR_SET_MC6_TYPE(_t1, _type)			\
+	do {							\
+		_t1 &= ~RV_DBTR_BIT_MASK(MC6, TYPE);		\
+		_t1 |= (((unsigned long)_type			\
+			 << RV_DBTR_BIT(MC6, TYPE))		\
+			& RV_DBTR_BIT_MASK(MC6, TYPE));		\
+	}while (0);
+
+#define RV_DBTR_SET_MC_EXEC(_t1)		\
+	SET_DBTR_BIT(_t1, MC, EXEC)
+
+#define RV_DBTR_SET_MC_LOAD(_t1)		\
+	SET_DBTR_BIT(_t1, MC, LOAD)
+
+#define RV_DBTR_SET_MC_STORE(_t1)		\
+	SET_DBTR_BIT(_t1, MC, STORE)
+
+#define RV_DBTR_SET_MC_SIZELO(_t1, _val)			\
+	do {							\
+		_t1 &= ~RV_DBTR_BIT_MASK(MC, SIZELO);		\
+		_t1 |= ((_val << RV_DBTR_BIT(MC, SIZELO))	\
+			& RV_DBTR_BIT_MASK(MC, SIZELO));	\
+	} while(0);
+
+#define RV_DBTR_SET_MC_SIZEHI(_t1, _val)			\
+	do {							\
+		_t1 &= ~RV_DBTR_BIT_MASK(MC, SIZEHI);		\
+		_t1 |= ((_val << RV_DBTR_BIT(MC, SIZEHI))	\
+			& RV_DBTR_BIT_MASK(MC, SIZEHI));	\
+	} while(0);
+
+#define RV_DBTR_SET_MC6_EXEC(_t1)		\
+	SET_DBTR_BIT(_t1, MC6, EXEC)
+
+#define RV_DBTR_SET_MC6_LOAD(_t1)		\
+	SET_DBTR_BIT(_t1, MC6, LOAD)
+
+#define RV_DBTR_SET_MC6_STORE(_t1)		\
+	SET_DBTR_BIT(_t1, MC6, STORE)
+
+#define RV_DBTR_SET_MC6_SIZE(_t1, _val)				\
+	do {							\
+		_t1 &= ~RV_DBTR_BIT_MASK(MC6, SIZE);		\
+		_t1 |= ((_val << RV_DBTR_BIT(MC6, SIZE))	\
+			& RV_DBTR_BIT_MASK(MC6, SIZE));		\
+	} while(0);
+
+typedef unsigned long riscv_dbtr_tdata1_mcontrol_t;
+typedef unsigned long riscv_dbtr_tdata1_mcontrol6_t;
+typedef unsigned long riscv_dbtr_tdata1_t;
+
+#endif /* __RISCV_DBTR_H__ */

include/sbi/riscv_encoding.h

@@ -80,6 +80,8 @@
 #define HSTATUS_GVA			_UL(0x00000040)
 #define HSTATUS_VSBE			_UL(0x00000020)
 
+#define MCAUSE_IRQ_MASK			(_UL(1) << (__riscv_xlen - 1))
+
 #define IRQ_S_SOFT			1
 #define IRQ_VS_SOFT			2
 #define IRQ_M_SOFT			3
@@ -205,10 +207,12 @@
 
 #endif
 
-#define MHPMEVENT_SSCOF_MASK		_ULL(0xFFFF000000000000)
+#define MHPMEVENT_SSCOF_MASK		_ULL(0xFF00000000000000)
 
 #define ENVCFG_STCE			(_ULL(1) << 63)
 #define ENVCFG_PBMTE			(_ULL(1) << 62)
+#define ENVCFG_ADUE			(_ULL(1) << 61)
+#define ENVCFG_CDE			(_ULL(1) << 60)
 #define ENVCFG_CBZE			(_UL(1) << 7)
 #define ENVCFG_CBCFE			(_UL(1) << 6)
 #define ENVCFG_CBIE_SHIFT		4
@@ -314,6 +318,9 @@
 /* Supervisor Configuration */
 #define CSR_SENVCFG			0x10a
 
+/* Supervisor Conter Inhibit */
+#define CSR_SCOUNTINHIBIT		0x120
+
 /* Supervisor Trap Handling */
 #define CSR_SSCRATCH			0x140
 #define CSR_SEPC			0x141
@@ -328,9 +335,14 @@
 /* Supervisor Protection and Translation */
 #define CSR_SATP			0x180
 
-/* Supervisor-Level Window to Indirectly Accessed Registers (AIA) */
+/* Supervisor Indirect Register Alias */
 #define CSR_SISELECT			0x150
 #define CSR_SIREG			0x151
+#define CSR_SIREG2          		0x152
+#define CSR_SIREG3          		0x153
+#define CSR_SIREG4          		0x155
+#define CSR_SIREG5          		0x156
+#define CSR_SIREG6          		0x157
 
 /* Supervisor-Level Interrupts (AIA) */
 #define CSR_STOPEI			0x15c
@@ -391,9 +403,14 @@
 #define CSR_HVIPRIO1			0x646
 #define CSR_HVIPRIO2			0x647
 
-/* VS-Level Window to Indirectly Accessed Registers (H-extension with AIA) */
+/* Virtual Supervisor Indirect Alias */
 #define CSR_VSISELECT			0x250
 #define CSR_VSIREG			0x251
+#define CSR_VSIREG2         		0x252
+#define CSR_VSIREG3         		0x253
+#define CSR_VSIREG4         		0x255
+#define CSR_VSIREG5         		0x256
+#define CSR_VSIREG6         		0x257
 
 /* VS-Level Interrupts (H-extension with AIA) */
 #define CSR_VSTOPEI			0x25c
@@ -686,6 +703,7 @@
 #define CSR_TDATA1			0x7a1
 #define CSR_TDATA2			0x7a2
 #define CSR_TDATA3			0x7a3
+#define CSR_TINFO			0x7a4
 
 /* Debug Mode Registers */
 #define CSR_DCSR			0x7b0
@@ -693,9 +711,14 @@
 #define CSR_DSCRATCH0			0x7b2
 #define CSR_DSCRATCH1			0x7b3
 
-/* Machine-Level Window to Indirectly Accessed Registers (AIA) */
+/* Machine Indirect Register Alias */
 #define CSR_MISELECT			0x350
 #define CSR_MIREG			0x351
+#define CSR_MIREG2          		0x352
+#define CSR_MIREG3          		0x353
+#define CSR_MIREG4          		0x355
+#define CSR_MIREG5          		0x356
+#define CSR_MIREG6          		0x357
 
 /* Machine-Level Interrupts (AIA) */
 #define CSR_MTOPEI			0x35c
@@ -836,6 +859,13 @@
 #define INSN_MATCH_C_FSWSP		0xe002
 #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_LH			0xfc43
+#define INSN_MATCH_C_SH		0x8c00
+#define INSN_MASK_C_SH			0xfc43
+
 #define INSN_MASK_WFI			0xffffff00
 #define INSN_MATCH_WFI			0x10500000
 

include/sbi/sbi_bitmap.h

@@ -62,6 +62,11 @@ static inline void bitmap_zero(unsigned long *dst, int nbits)
 	}
 }
 
+static inline int bitmap_test(unsigned long *bmap, int bit)
+{
+	return __test_bit(bit, bmap);
+}
+
 static inline void bitmap_zero_except(unsigned long *dst,
 				      int exception, int nbits)
 {

include/sbi/sbi_byteorder.h

@@ -7,7 +7,12 @@
 #ifndef __SBI_BYTEORDER_H__
 #define __SBI_BYTEORDER_H__
 
-#include <sbi/sbi_types.h>
+#ifdef __ASSEMBLER__
+# define _conv_cast(type, val) (val)
+#else
+# include <sbi/sbi_types.h>
+# define _conv_cast(type, val) ((type)(val))
+#endif
 
 #define BSWAP16(x)	((((x) & 0x00ff) << 8) | \
 			 (((x) & 0xff00) >> 8))
@@ -25,37 +30,47 @@
 			 (((x) & 0xff00000000000000ULL) >> 56))
 
 #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__  /* CPU(little-endian) */
-#define cpu_to_be16(x)		((uint16_t)BSWAP16(x))
-#define cpu_to_be32(x)		((uint32_t)BSWAP32(x))
-#define cpu_to_be64(x)		((uint64_t)BSWAP64(x))
+#define cpu_to_be16(x)		_conv_cast(uint16_t, BSWAP16(x))
+#define cpu_to_be32(x)		_conv_cast(uint32_t, BSWAP32(x))
+#define cpu_to_be64(x)		_conv_cast(uint64_t, BSWAP64(x))
 
-#define be16_to_cpu(x)		((uint16_t)BSWAP16(x))
-#define be32_to_cpu(x)		((uint32_t)BSWAP32(x))
-#define be64_to_cpu(x)		((uint64_t)BSWAP64(x))
+#define be16_to_cpu(x)		_conv_cast(uint16_t, BSWAP16(x))
+#define be32_to_cpu(x)		_conv_cast(uint32_t, BSWAP32(x))
+#define be64_to_cpu(x)		_conv_cast(uint64_t, BSWAP64(x))
 
-#define cpu_to_le16(x)		((uint16_t)(x))
-#define cpu_to_le32(x)		((uint32_t)(x))
-#define cpu_to_le64(x)		((uint64_t)(x))
+#define cpu_to_le16(x)		_conv_cast(uint16_t, (x))
+#define cpu_to_le32(x)		_conv_cast(uint32_t, (x))
+#define cpu_to_le64(x)		_conv_cast(uint64_t, (x))
 
-#define le16_to_cpu(x)		((uint16_t)(x))
-#define le32_to_cpu(x)		((uint32_t)(x))
-#define le64_to_cpu(x)		((uint64_t)(x))
+#define le16_to_cpu(x)		_conv_cast(uint16_t, (x))
+#define le32_to_cpu(x)		_conv_cast(uint32_t, (x))
+#define le64_to_cpu(x)		_conv_cast(uint64_t, (x))
 #else /* CPU(big-endian) */
-#define cpu_to_be16(x)		((uint16_t)(x))
-#define cpu_to_be32(x)		((uint32_t)(x))
-#define cpu_to_be64(x)		((uint64_t)(x))
+#define cpu_to_be16(x)		_conv_cast(uint16_t, (x))
+#define cpu_to_be32(x)		_conv_cast(uint32_t, (x))
+#define cpu_to_be64(x)		_conv_cast(uint64_t, (x))
 
-#define be16_to_cpu(x)		((uint16_t)(x))
-#define be32_to_cpu(x)		((uint32_t)(x))
-#define be64_to_cpu(x)		((uint64_t)(x))
+#define be16_to_cpu(x)		_conv_cast(uint16_t, (x))
+#define be32_to_cpu(x)		_conv_cast(uint32_t, (x))
+#define be64_to_cpu(x)		_conv_cast(uint64_t, (x))
 
-#define cpu_to_le16(x)		((uint16_t)BSWAP16(x))
-#define cpu_to_le32(x)		((uint32_t)BSWAP32(x))
-#define cpu_to_le64(x)		((uint64_t)BSWAP64(x))
+#define cpu_to_le16(x)		_conv_cast(uint16_t, BSWAP16(x))
+#define cpu_to_le32(x)		_conv_cast(uint32_t, BSWAP32(x))
+#define cpu_to_le64(x)		_conv_cast(uint64_t, BSWAP64(x))
 
-#define le16_to_cpu(x)		((uint16_t)BSWAP16(x))
-#define le32_to_cpu(x)		((uint32_t)BSWAP32(x))
-#define le64_to_cpu(x)		((uint64_t)BSWAP64(x))
+#define le16_to_cpu(x)		_conv_cast(uint16_t, BSWAP16(x))
+#define le32_to_cpu(x)		_conv_cast(uint32_t, BSWAP32(x))
+#define le64_to_cpu(x)		_conv_cast(uint64_t, BSWAP64(x))
+#endif
+
+#if __riscv_xlen == 64
+#define cpu_to_lle		cpu_to_le64
+#define lle_to_cpu		le64_to_cpu
+#elif __riscv_xlen == 32
+#define cpu_to_lle		cpu_to_le32
+#define lle_to_cpu		le32_to_cpu
+#else
+#error "Unknown __riscv_xlen"
 #endif
 
 #endif /* __SBI_BYTEORDER_H__ */

include/sbi/sbi_dbtr.h

@@ -0,0 +1,125 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Ventana Micro Systems, Inc.
+ *
+ * Authors:
+ *   Himanshu Chauhan <hchauhan@ventanamicro.com>
+ */
+
+#ifndef __SBI_DBTR_H__
+#define __SBI_DBTR_H__
+
+#include <sbi/riscv_dbtr.h>
+#include <sbi/sbi_types.h>
+
+struct sbi_domain;
+
+enum {
+	RV_DBTR_DECLARE_BIT(TS, MAPPED, 0), /* trigger mapped to hw trigger */
+	RV_DBTR_DECLARE_BIT(TS, U, 1),
+	RV_DBTR_DECLARE_BIT(TS, S, 2),
+	RV_DBTR_DECLARE_BIT(TS, VU, 3),
+	RV_DBTR_DECLARE_BIT(TS, VS, 4),
+	RV_DBTR_DECLARE_BIT(TS, HAVE_TRIG, 5), /* H/w dbtr details available */
+	RV_DBTR_DECLARE_BIT(TS, HW_IDX, 8), /* Hardware index of trigger */
+};
+
+enum {
+	RV_DBTR_DECLARE_BIT_MASK(TS, MAPPED, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, U, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, S, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, VU, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, VS, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, HAVE_TRIG, 1),
+	RV_DBTR_DECLARE_BIT_MASK(TS, HW_IDX, (__riscv_xlen-9)),
+};
+
+#if __riscv_xlen == 64
+#define SBI_DBTR_SHMEM_INVALID_ADDR	0xFFFFFFFFFFFFFFFFUL
+#elif __riscv_xlen == 32
+#define SBI_DBTR_SHMEM_INVALID_ADDR	0xFFFFFFFFUL
+#else
+#error "Unexpected __riscv_xlen"
+#endif
+
+struct sbi_dbtr_shmem {
+	unsigned long phys_lo;
+	unsigned long phys_hi;
+};
+
+struct sbi_dbtr_trigger {
+	unsigned long index;
+	unsigned long type_mask;
+	unsigned long state;
+	unsigned long tdata1;
+	unsigned long tdata2;
+	unsigned long tdata3;
+};
+
+struct sbi_dbtr_data_msg {
+	unsigned long tstate;
+	unsigned long tdata1;
+	unsigned long tdata2;
+	unsigned long tdata3;
+};
+
+struct sbi_dbtr_id_msg {
+	unsigned long idx;
+};
+
+struct sbi_dbtr_hart_triggers_state {
+	struct sbi_dbtr_trigger triggers[RV_MAX_TRIGGERS];
+	struct sbi_dbtr_shmem shmem;
+	u32 total_trigs;
+	u32 available_trigs;
+	u32 hartid;
+	u32 probed;
+};
+
+#define TDATA1_GET_TYPE(_t1)					\
+	EXTRACT_FIELD(_t1, RV_DBTR_BIT_MASK(TDATA1, TYPE))
+
+/* Set the hardware index of trigger in logical trigger state */
+#define SET_TRIG_HW_INDEX(_state, _idx)				\
+	do {							\
+		_state &= ~RV_DBTR_BIT_MASK(TS, HW_IDX);	\
+		_state |= (((unsigned long)_idx			\
+			    << RV_DBTR_BIT(TS, HW_IDX))		\
+			   & RV_DBTR_BIT_MASK(TS, HW_IDX));	\
+	}while (0);
+
+/** SBI shared mem messages layout */
+struct sbi_dbtr_shmem_entry {
+	struct sbi_dbtr_data_msg data;
+	struct sbi_dbtr_id_msg id;
+};
+
+#define SBI_DBTR_SHMEM_ALIGN_MASK	((__riscv_xlen / 8) - 1)
+
+/** Initialize debug triggers */
+int sbi_dbtr_init(struct sbi_scratch *scratch, bool coldboot);
+
+/** SBI DBTR extension functions */
+int sbi_dbtr_supported(void);
+int sbi_dbtr_setup_shmem(const struct sbi_domain *dom, unsigned long smode,
+			 unsigned long shmem_phys_lo,
+			 unsigned long shmem_phys_hi);
+int sbi_dbtr_num_trig(unsigned long trig_tdata1, unsigned long *out);
+int sbi_dbtr_read_trig(unsigned long smode,
+		       unsigned long trig_idx_base, unsigned long trig_count);
+int sbi_dbtr_install_trig(unsigned long smode,
+			  unsigned long trig_count, unsigned long *out);
+int sbi_dbtr_uninstall_trig(unsigned long trig_idx_base,
+			    unsigned long trig_idx_mask);
+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);
+int sbi_dbtr_disable_trig(unsigned long trig_idx_base,
+			  unsigned long trig_idx_mask);
+
+int sbi_dbtr_get_total_triggers(void);
+
+#endif

include/sbi/sbi_domain.h

@@ -10,8 +10,10 @@
 #ifndef __SBI_DOMAIN_H__
 #define __SBI_DOMAIN_H__
 
+#include <sbi/riscv_locks.h>
 #include <sbi/sbi_types.h>
 #include <sbi/sbi_hartmask.h>
+#include <sbi/sbi_domain_context.h>
 
 struct sbi_scratch;
 
@@ -172,10 +174,14 @@ struct sbi_domain {
 	 * in the coldboot path
 	 */
 	struct sbi_hartmask assigned_harts;
+	/** Spinlock for accessing assigned_harts */
+	spinlock_t assigned_harts_lock;
 	/** Name of this domain */
 	char name[64];
 	/** Possible HARTs in this domain */
 	const struct sbi_hartmask *possible_harts;
+	/** Contexts for possible HARTs indexed by hartindex */
+	struct sbi_context *hartindex_to_context_table[SBI_HARTMASK_MAX_BITS];
 	/** Array of memory regions terminated by a region with order zero */
 	struct sbi_domain_memregion *regions;
 	/** HART id of the HART booting this domain */
@@ -200,6 +206,9 @@ extern struct sbi_domain root;
 /** Get pointer to sbi_domain from HART index */
 struct sbi_domain *sbi_hartindex_to_domain(u32 hartindex);
 
+/** Update HART local pointer to point to specified domain */
+void sbi_update_hartindex_to_domain(u32 hartindex, struct sbi_domain *dom);
+
 /** Get pointer to sbi_domain for current HART */
 #define sbi_domain_thishart_ptr() \
 	sbi_hartindex_to_domain(sbi_hartid_to_hartindex(current_hartid()))

include/sbi/sbi_domain_context.h

@@ -0,0 +1,77 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) IPADS@SJTU 2023. All rights reserved.
+ */
+
+#ifndef __SBI_DOMAIN_CONTEXT_H__
+#define __SBI_DOMAIN_CONTEXT_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_domain.h>
+
+/** Context representation for a hart within a domain */
+struct sbi_context {
+	/** Trap-related states such as GPRs, mepc, and mstatus */
+	struct sbi_trap_context trap_ctx;
+
+	/** Supervisor status register */
+	unsigned long sstatus;
+	/** Supervisor interrupt enable register */
+	unsigned long sie;
+	/** Supervisor trap vector base address register */
+	unsigned long stvec;
+	/** Supervisor scratch register for temporary storage */
+	unsigned long sscratch;
+	/** Supervisor exception program counter register */
+	unsigned long sepc;
+	/** Supervisor cause register */
+	unsigned long scause;
+	/** Supervisor trap value register */
+	unsigned long stval;
+	/** Supervisor interrupt pending register */
+	unsigned long sip;
+	/** Supervisor address translation and protection register */
+	unsigned long satp;
+	/** Counter-enable register */
+	unsigned long scounteren;
+	/** Supervisor environment configuration register */
+	unsigned long senvcfg;
+
+	/** Reference to the owning domain */
+	struct sbi_domain *dom;
+	/** Previous context (caller) to jump to during context exits */
+	struct sbi_context *prev_ctx;
+	/** Is context initialized and runnable */
+	bool initialized;
+};
+
+/** Get the context pointer for a given hart index and domain */
+#define sbi_hartindex_to_domain_context(__hartindex, __d) \
+	(__d)->hartindex_to_context_table[__hartindex]
+
+/** Macro to obtain the current hart's context pointer */
+#define sbi_domain_context_thishart_ptr()                  \
+	sbi_hartindex_to_domain_context(                   \
+		sbi_hartid_to_hartindex(current_hartid()), \
+		sbi_domain_thishart_ptr())
+
+/**
+ * Enter a specific domain context synchronously
+ * @param dom pointer to domain
+ *
+ * @return 0 on success and negative error code on failure
+ */
+int sbi_domain_context_enter(struct sbi_domain *dom);
+
+/**
+ * Exit the current domain context, and then return to the caller
+ * of sbi_domain_context_enter or attempt to start the next domain
+ * context to be initialized
+ *
+ * @return 0 on success and negative error code on failure
+ */
+int sbi_domain_context_exit(void);
+
+#endif // __SBI_DOMAIN_CONTEXT_H__

include/sbi/sbi_ecall.h

@@ -18,7 +18,7 @@
 #define SBI_OPENSBI_IMPID		1
 
 struct sbi_trap_regs;
-struct sbi_trap_info;
+struct sbi_trap_context;
 
 struct sbi_ecall_return {
 	/* Return flag to skip register update */
@@ -87,7 +87,7 @@ int sbi_ecall_register_extension(struct sbi_ecall_extension *ext);
 
 void sbi_ecall_unregister_extension(struct sbi_ecall_extension *ext);
 
-int sbi_ecall_handler(struct sbi_trap_regs *regs);
+int sbi_ecall_handler(struct sbi_trap_context *tcntx);
 
 int sbi_ecall_init(void);
 

include/sbi/sbi_ecall_interface.h

@@ -32,6 +32,9 @@
 #define SBI_EXT_DBCN				0x4442434E
 #define SBI_EXT_SUSP				0x53555350
 #define SBI_EXT_CPPC				0x43505043
+#define SBI_EXT_DBTR				0x44425452
+#define SBI_EXT_SSE				0x535345
+#define SBI_EXT_FWFT				0x46574654
 
 /* SBI function IDs for BASE extension*/
 #define SBI_EXT_BASE_GET_SPEC_VERSION		0x0
@@ -105,6 +108,42 @@
 #define SBI_EXT_PMU_COUNTER_FW_READ_HI	0x6
 #define SBI_EXT_PMU_SNAPSHOT_SET_SHMEM	0x7
 
+/* SBI function IDs for DBTR extension */
+#define SBI_EXT_DBTR_NUM_TRIGGERS	0x0
+#define SBI_EXT_DBTR_SETUP_SHMEM	0x1
+#define SBI_EXT_DBTR_TRIGGER_READ	0x2
+#define SBI_EXT_DBTR_TRIGGER_INSTALL	0x3
+#define SBI_EXT_DBTR_TRIGGER_UPDATE	0x4
+#define SBI_EXT_DBTR_TRIGGER_UNINSTALL	0x5
+#define SBI_EXT_DBTR_TRIGGER_ENABLE	0x6
+#define SBI_EXT_DBTR_TRIGGER_DISABLE	0x7
+
+/* SBI function IDs for FW feature extension */
+#define SBI_EXT_FWFT_SET		0x0
+#define SBI_EXT_FWFT_GET		0x1
+
+enum sbi_fwft_feature_t {
+	SBI_FWFT_MISALIGNED_EXC_DELEG		= 0x0,
+	SBI_FWFT_LANDING_PAD			= 0x1,
+	SBI_FWFT_SHADOW_STACK			= 0x2,
+	SBI_FWFT_DOUBLE_TRAP			= 0x3,
+	SBI_FWFT_PTE_AD_HW_UPDATING		= 0x4,
+	SBI_FWFT_LOCAL_RESERVED_START		= 0x5,
+	SBI_FWFT_LOCAL_RESERVED_END		= 0x3fffffff,
+	SBI_FWFT_LOCAL_PLATFORM_START		= 0x40000000,
+	SBI_FWFT_LOCAL_PLATFORM_END		= 0x7fffffff,
+
+	SBI_FWFT_GLOBAL_RESERVED_START		= 0x80000000,
+	SBI_FWFT_GLOBAL_RESERVED_END		= 0xbfffffff,
+	SBI_FWFT_GLOBAL_PLATFORM_START		= 0xc0000000,
+	SBI_FWFT_GLOBAL_PLATFORM_END		= 0xffffffff,
+};
+
+#define SBI_FWFT_GLOBAL_FEATURE_BIT		(1 << 31)
+#define SBI_FWFT_PLATFORM_FEATURE_BIT		(1 << 30)
+
+#define SBI_FWFT_SET_FLAG_LOCK			(1 << 0)
+
 /** General pmu event codes specified in SBI PMU extension */
 enum sbi_pmu_hw_generic_events_t {
 	SBI_PMU_HW_NO_EVENT			= 0,
@@ -293,6 +332,80 @@ enum sbi_cppc_reg_id {
 	SBI_CPPC_NON_ACPI_LAST		= SBI_CPPC_TRANSITION_LATENCY,
 };
 
+/* SBI Function IDs for SSE extension */
+#define SBI_EXT_SSE_READ_ATTR		0x00000000
+#define SBI_EXT_SSE_WRITE_ATTR		0x00000001
+#define SBI_EXT_SSE_REGISTER		0x00000002
+#define SBI_EXT_SSE_UNREGISTER		0x00000003
+#define SBI_EXT_SSE_ENABLE		0x00000004
+#define SBI_EXT_SSE_DISABLE		0x00000005
+#define SBI_EXT_SSE_COMPLETE		0x00000006
+#define SBI_EXT_SSE_INJECT		0x00000007
+
+/* SBI SSE Event Attributes. */
+enum sbi_sse_attr_id {
+	SBI_SSE_ATTR_STATUS		= 0x00000000,
+	SBI_SSE_ATTR_PRIO		= 0x00000001,
+	SBI_SSE_ATTR_CONFIG		= 0x00000002,
+	SBI_SSE_ATTR_PREFERRED_HART	= 0x00000003,
+	SBI_SSE_ATTR_ENTRY_PC		= 0x00000004,
+	SBI_SSE_ATTR_ENTRY_ARG		= 0x00000005,
+	SBI_SSE_ATTR_INTERRUPTED_SEPC	= 0x00000006,
+	SBI_SSE_ATTR_INTERRUPTED_FLAGS	= 0x00000007,
+	SBI_SSE_ATTR_INTERRUPTED_A6	= 0x00000008,
+	SBI_SSE_ATTR_INTERRUPTED_A7	= 0x00000009,
+
+	SBI_SSE_ATTR_MAX		= 0x0000000A
+};
+
+#define SBI_SSE_ATTR_STATUS_STATE_OFFSET	0
+#define SBI_SSE_ATTR_STATUS_STATE_MASK		0x3
+#define SBI_SSE_ATTR_STATUS_PENDING_OFFSET	2
+#define SBI_SSE_ATTR_STATUS_INJECT_OFFSET	3
+
+#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_HSTATUS_SPV	BIT(2)
+#define SBI_SSE_ATTR_INTERRUPTED_FLAGS_HSTATUS_SPVP	BIT(3)
+
+enum sbi_sse_state {
+	SBI_SSE_STATE_UNUSED		= 0,
+	SBI_SSE_STATE_REGISTERED	= 1,
+	SBI_SSE_STATE_ENABLED		= 2,
+	SBI_SSE_STATE_RUNNING		= 3,
+};
+
+/* SBI SSE Event IDs. */
+#define SBI_SSE_EVENT_LOCAL_RAS			0x00000000
+#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_PLAT_0_START	0x00004000
+#define SBI_SSE_EVENT_GLOBAL_PLAT_0_END		0x00007fff
+
+#define SBI_SSE_EVENT_LOCAL_PMU			0x00010000
+#define SBI_SSE_EVENT_LOCAL_PLAT_1_START	0x00014000
+#define SBI_SSE_EVENT_LOCAL_PLAT_1_END		0x00017fff
+#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
+
+#define SBI_SSE_EVENT_LOCAL_SOFTWARE		0xffff0000
+#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_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)
+
 /* SBI base specification related macros */
 #define SBI_SPEC_VERSION_MAJOR_OFFSET		24
 #define SBI_SPEC_VERSION_MAJOR_MASK		0x7f
@@ -313,8 +426,10 @@ enum sbi_cppc_reg_id {
 #define SBI_ERR_ALREADY_STARTED			-7
 #define SBI_ERR_ALREADY_STOPPED			-8
 #define SBI_ERR_NO_SHMEM			-9
+#define SBI_ERR_INVALID_STATE			-10
+#define SBI_ERR_BAD_RANGE			-11
 
-#define SBI_LAST_ERR				SBI_ERR_NO_SHMEM
+#define SBI_LAST_ERR				SBI_ERR_BAD_RANGE
 
 /* clang-format on */
 

include/sbi/sbi_error.h

@@ -24,6 +24,8 @@
 #define SBI_EALREADY_STARTED	SBI_ERR_ALREADY_STARTED
 #define SBI_EALREADY_STOPPED	SBI_ERR_ALREADY_STOPPED
 #define SBI_ENO_SHMEM		SBI_ERR_NO_SHMEM
+#define SBI_EINVALID_STATE	SBI_ERR_INVALID_STATE
+#define SBI_EBAD_RANGE		SBI_ERR_BAD_RANGE
 
 #define SBI_ENODEV		-1000
 #define SBI_ENOSYS		-1001

include/sbi/sbi_fwft.h

@@ -0,0 +1,23 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2024 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <cleger@rivosinc.com>
+ */
+
+#ifndef __SBI_FW_FEATURE_H__
+#define __SBI_FW_FEATURE_H__
+
+#include <sbi/sbi_ecall_interface.h>
+
+struct sbi_scratch;
+
+int sbi_fwft_set(enum sbi_fwft_feature_t feature, unsigned long value,
+		 unsigned long flags);
+int sbi_fwft_get(enum sbi_fwft_feature_t feature, unsigned long *out_val);
+
+int sbi_fwft_init(struct sbi_scratch *scratch, bool cold_boot);
+
+#endif

include/sbi/sbi_hart.h

@@ -53,6 +53,20 @@ enum sbi_hart_extensions {
 	SBI_HART_EXT_ZICBOM,
 	/** Hart has Svpbmt extension */
 	SBI_HART_EXT_SVPBMT,
+	/** Hart has debug trigger extension */
+	SBI_HART_EXT_SDTRIG,
+	/** Hart has Smcsrind extension */
+	SBI_HART_EXT_SMCSRIND,
+	/** Hart has Smcdeleg extension */
+	SBI_HART_EXT_SMCDELEG,
+	/** Hart has Sscsrind extension */
+	SBI_HART_EXT_SSCSRIND,
+	/** Hart has Ssccfg extension */
+	SBI_HART_EXT_SSCCFG,
+	/** Hart has Svade extension */
+	SBI_HART_EXT_SVADE,
+	/** Hart has Svadu extension */
+	SBI_HART_EXT_SVADU,
 
 	/** Maximum index of Hart extension */
 	SBI_HART_EXT_MAX,

include/sbi/sbi_illegal_insn.h

@@ -12,8 +12,8 @@
 
 #include <sbi/sbi_types.h>
 
-struct sbi_trap_regs;
+struct sbi_trap_context;
 
-int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs);
+int sbi_illegal_insn_handler(struct sbi_trap_context *tcntx);
 
 #endif

include/sbi/sbi_irqchip.h

@@ -13,7 +13,6 @@
 #include <sbi/sbi_types.h>
 
 struct sbi_scratch;
-struct sbi_trap_regs;
 
 /**
  * Set external interrupt handling function
@@ -23,7 +22,7 @@ struct sbi_trap_regs;
  *
  * @param fn function pointer for handling external irqs
  */
-void sbi_irqchip_set_irqfn(int (*fn)(struct sbi_trap_regs *regs));
+void sbi_irqchip_set_irqfn(int (*fn)(void));
 
 /**
  * Process external interrupts
@@ -33,7 +32,7 @@ void sbi_irqchip_set_irqfn(int (*fn)(struct sbi_trap_regs *regs));
  *
  * @param regs pointer for trap registers
  */
-int sbi_irqchip_process(struct sbi_trap_regs *regs);
+int sbi_irqchip_process(void);
 
 /** Initialize interrupt controllers */
 int sbi_irqchip_init(struct sbi_scratch *scratch, bool cold_boot);

include/sbi/sbi_misaligned_ldst.h

@@ -1,23 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2019 Western Digital Corporation or its affiliates.
- *
- * Authors:
- *   Anup Patel <anup.patel@wdc.com>
- */
-
-#ifndef __SBI_MISALIGNED_LDST_H__
-#define __SBI_MISALIGNED_LDST_H__
-
-#include <sbi/sbi_types.h>
-
-struct sbi_trap_regs;
-
-int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
-				struct sbi_trap_regs *regs);
-
-int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
-				 struct sbi_trap_regs *regs);
-
-#endif

include/sbi/sbi_platform.h

@@ -48,11 +48,13 @@
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_version.h>
+#include <sbi/sbi_trap_ldst.h>
 
 struct sbi_domain_memregion;
 struct sbi_ecall_return;
 struct sbi_trap_regs;
 struct sbi_hart_features;
+union sbi_ldst_data;
 
 /** Possible feature flags of a platform */
 enum sbi_platform_features {
@@ -139,6 +141,13 @@ struct sbi_platform_operations {
 	int (*vendor_ext_provider)(long funcid,
 				   struct sbi_trap_regs *regs,
 				   struct sbi_ecall_return *out);
+
+	/** platform specific handler to fixup load fault */
+	int (*emulate_load)(int rlen, unsigned long addr,
+			    union sbi_ldst_data *out_val);
+	/** platform specific handler to fixup store fault */
+	int (*emulate_store)(int wlen, unsigned long addr,
+			     union sbi_ldst_data in_val);
 };
 
 /** Platform default per-HART stack size for exception/interrupt handling */
@@ -675,6 +684,50 @@ static inline int sbi_platform_vendor_ext_provider(
 	return SBI_ENOTSUPP;
 }
 
+/**
+ * Ask platform to emulate the trapped load
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param rlen length of the load: 1/2/4/8...
+ * @param addr virtual address of the load. Platform needs to page-walk and
+ *        find the physical address if necessary
+ * @param out_val value loaded
+ *
+ * @return 0 on success and negative error code on failure
+ */
+static inline int sbi_platform_emulate_load(const struct sbi_platform *plat,
+					    int rlen, unsigned long addr,
+					    union sbi_ldst_data *out_val)
+{
+	if (plat && sbi_platform_ops(plat)->emulate_load) {
+		return sbi_platform_ops(plat)->emulate_load(rlen, addr,
+							    out_val);
+	}
+	return SBI_ENOTSUPP;
+}
+
+/**
+ * Ask platform to emulate the trapped store
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param wlen length of the store: 1/2/4/8...
+ * @param addr virtual address of the store. Platform needs to page-walk and
+ *        find the physical address if necessary
+ * @param in_val value to store
+ *
+ * @return 0 on success and negative error code on failure
+ */
+static inline int sbi_platform_emulate_store(const struct sbi_platform *plat,
+					     int wlen, unsigned long addr,
+					     union sbi_ldst_data in_val)
+{
+	if (plat && sbi_platform_ops(plat)->emulate_store) {
+		return sbi_platform_ops(plat)->emulate_store(wlen, addr,
+							     in_val);
+	}
+	return SBI_ENOTSUPP;
+}
+
 #endif
 
 #endif

include/sbi/sbi_pmu.h

@@ -11,6 +11,7 @@
 #define __SBI_PMU_H__
 
 #include <sbi/sbi_types.h>
+#include <sbi/sbi_trap.h>
 
 struct sbi_scratch;
 
@@ -150,4 +151,6 @@ int sbi_pmu_ctr_cfg_match(unsigned long cidx_base, unsigned long cidx_mask,
 
 int sbi_pmu_ctr_incr_fw(enum sbi_pmu_fw_event_code_id fw_id);
 
+void sbi_pmu_ovf_irq();
+
 #endif

include/sbi/sbi_scratch.h

@@ -36,8 +36,8 @@
 #define SBI_SCRATCH_PLATFORM_ADDR_OFFSET	(9 * __SIZEOF_POINTER__)
 /** Offset of hartid_to_scratch member in sbi_scratch */
 #define SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET	(10 * __SIZEOF_POINTER__)
-/** Offset of trap_exit member in sbi_scratch */
-#define SBI_SCRATCH_TRAP_EXIT_OFFSET		(11 * __SIZEOF_POINTER__)
+/** Offset of trap_context member in sbi_scratch */
+#define SBI_SCRATCH_TRAP_CONTEXT_OFFSET		(11 * __SIZEOF_POINTER__)
 /** Offset of tmp0 member in sbi_scratch */
 #define SBI_SCRATCH_TMP0_OFFSET			(12 * __SIZEOF_POINTER__)
 /** Offset of options member in sbi_scratch */
@@ -77,8 +77,8 @@ struct sbi_scratch {
 	unsigned long platform_addr;
 	/** Address of HART ID to sbi_scratch conversion function */
 	unsigned long hartid_to_scratch;
-	/** Address of trap exit function */
-	unsigned long trap_exit;
+	/** Address of current trap context */
+	unsigned long trap_context;
 	/** Temporary storage */
 	unsigned long tmp0;
 	/** Options for OpenSBI library */
@@ -130,10 +130,10 @@ _Static_assert(
 	"struct sbi_scratch definition has changed, please redefine "
 	"SBI_SCRATCH_HARTID_TO_SCRATCH_OFFSET");
 _Static_assert(
-	offsetof(struct sbi_scratch, trap_exit)
-		== SBI_SCRATCH_TRAP_EXIT_OFFSET,
+	offsetof(struct sbi_scratch, trap_context)
+		== SBI_SCRATCH_TRAP_CONTEXT_OFFSET,
 	"struct sbi_scratch definition has changed, please redefine "
-	"SBI_SCRATCH_TRAP_EXIT_OFFSET");
+	"SBI_SCRATCH_TRAP_CONTEXT_OFFSET");
 _Static_assert(
 	offsetof(struct sbi_scratch, tmp0)
 		== SBI_SCRATCH_TMP0_OFFSET,

include/sbi/sbi_sse.h

@@ -0,0 +1,93 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Rivos Systems.
+ */
+
+#ifndef __SBI_SSE_H__
+#define __SBI_SSE_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_list.h>
+#include <sbi/riscv_locks.h>
+
+struct sbi_scratch;
+struct sbi_trap_regs;
+struct sbi_ecall_return;
+
+#define EXC_MODE_PP_SHIFT		0
+#define EXC_MODE_PP			BIT(EXC_MODE_PP_SHIFT)
+#define EXC_MODE_PV_SHIFT		1
+#define EXC_MODE_PV			BIT(EXC_MODE_PV_SHIFT)
+#define EXC_MODE_SSTATUS_SPIE_SHIFT	2
+#define EXC_MODE_SSTATUS_SPIE		BIT(EXC_MODE_SSTATUS_SPIE_SHIFT)
+
+struct sbi_sse_cb_ops {
+	/**
+	 * Called when hart_id is changed on the event.
+	 */
+	void (*set_hartid_cb)(uint32_t event_id, unsigned long hart_id);
+
+	/**
+	 * Called when the SBI_EXT_SSE_COMPLETE is invoked on the event.
+	 */
+	void (*complete_cb)(uint32_t event_id);
+
+	/**
+	 * Called when the SBI_EXT_SSE_REGISTER is invoked on the event.
+	 */
+	void (*register_cb)(uint32_t event_id);
+
+	/**
+	 * Called when the SBI_EXT_SSE_UNREGISTER is invoked on the event.
+	 */
+	void (*unregister_cb)(uint32_t event_id);
+
+	/**
+	 * Called when the SBI_EXT_SSE_ENABLE is invoked on the event.
+	 */
+	void (*enable_cb)(uint32_t event_id);
+
+	/**
+	 * Called when the SBI_EXT_SSE_DISABLE is invoked on the event.
+	 */
+	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
+ * @return 0 on success, error otherwise
+ */
+int sbi_sse_set_cb_ops(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_*)
+ * @param regs Registers that were used on SBI entry
+ * @return 0 on success, error otherwise
+ */
+int sbi_sse_inject_event(uint32_t event_id);
+
+void sbi_sse_process_pending_events(struct sbi_trap_regs *regs);
+
+
+int sbi_sse_init(struct sbi_scratch *scratch, bool cold_boot);
+void sbi_sse_exit(struct sbi_scratch *scratch);
+
+/* Interface called from sbi_ecall_sse.c */
+int sbi_sse_register(uint32_t event_id, unsigned long handler_entry_pc,
+		     unsigned long handler_entry_arg);
+int sbi_sse_unregister(uint32_t event_id);
+int sbi_sse_enable(uint32_t event_id);
+int sbi_sse_disable(uint32_t event_id);
+int sbi_sse_complete(struct sbi_trap_regs *regs, struct sbi_ecall_return *out);
+int sbi_sse_inject_from_ecall(uint32_t event_id, unsigned long hart_id,
+			      struct sbi_ecall_return *out);
+int sbi_sse_read_attrs(uint32_t event_id, uint32_t base_attr_id,
+		       uint32_t attr_count, unsigned long output_phys_lo,
+		       unsigned long output_phys_hi);
+int sbi_sse_write_attrs(uint32_t event_id, uint32_t base_attr_id,
+			uint32_t attr_count, unsigned long input_phys_lo,
+			unsigned long input_phys_hi);
+
+#endif

include/sbi/sbi_trap.h

@@ -87,20 +87,18 @@
 /** Last member index in sbi_trap_regs */
 #define SBI_TRAP_REGS_last			35
 
-/** Index of epc member in sbi_trap_info */
-#define SBI_TRAP_INFO_epc			0
 /** Index of cause member in sbi_trap_info */
-#define SBI_TRAP_INFO_cause			1
+#define SBI_TRAP_INFO_cause			0
 /** Index of tval member in sbi_trap_info */
-#define SBI_TRAP_INFO_tval			2
+#define SBI_TRAP_INFO_tval			1
 /** Index of tval2 member in sbi_trap_info */
-#define SBI_TRAP_INFO_tval2			3
+#define SBI_TRAP_INFO_tval2			2
 /** Index of tinst member in sbi_trap_info */
-#define SBI_TRAP_INFO_tinst			4
+#define SBI_TRAP_INFO_tinst			3
 /** Index of gva member in sbi_trap_info */
-#define SBI_TRAP_INFO_gva			5
+#define SBI_TRAP_INFO_gva			4
 /** Last member index in sbi_trap_info */
-#define SBI_TRAP_INFO_last			6
+#define SBI_TRAP_INFO_last			5
 
 /* clang-format on */
 
@@ -114,9 +112,15 @@
 /** Size (in bytes) of sbi_trap_info */
 #define SBI_TRAP_INFO_SIZE SBI_TRAP_INFO_OFFSET(last)
 
+/** Size (in bytes) of sbi_trap_context */
+#define SBI_TRAP_CONTEXT_SIZE (SBI_TRAP_REGS_SIZE + \
+			       SBI_TRAP_INFO_SIZE + \
+			       __SIZEOF_POINTER__)
+
 #ifndef __ASSEMBLER__
 
 #include <sbi/sbi_types.h>
+#include <sbi/sbi_scratch.h>
 
 /** Representation of register state at time of trap/interrupt */
 struct sbi_trap_regs {
@@ -194,8 +198,6 @@ struct sbi_trap_regs {
 
 /** Representation of trap details */
 struct sbi_trap_info {
-	/** epc Trap program counter */
-	unsigned long epc;
 	/** cause Trap exception cause */
 	unsigned long cause;
 	/** tval Trap value */
@@ -208,6 +210,16 @@ struct sbi_trap_info {
 	unsigned long gva;
 };
 
+/** Representation of trap context saved on stack */
+struct sbi_trap_context {
+	/** Register state */
+	struct sbi_trap_regs regs;
+	/** Trap details */
+	struct sbi_trap_info trap;
+	/** Pointer to previous trap context */
+	struct sbi_trap_context *prev_context;
+};
+
 static inline unsigned long sbi_regs_gva(const struct sbi_trap_regs *regs)
 {
 	/*
@@ -225,11 +237,20 @@ static inline unsigned long sbi_regs_gva(const struct sbi_trap_regs *regs)
 }
 
 int sbi_trap_redirect(struct sbi_trap_regs *regs,
-		      struct sbi_trap_info *trap);
+		      const struct sbi_trap_info *trap);
 
-struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs);
+static inline struct sbi_trap_context *sbi_trap_get_context(struct sbi_scratch *scratch)
+{
+	return (scratch) ? (void *)scratch->trap_context : NULL;
+}
 
-void __noreturn sbi_trap_exit(const struct sbi_trap_regs *regs);
+static inline void sbi_trap_set_context(struct sbi_scratch *scratch,
+					struct sbi_trap_context *tcntx)
+{
+	scratch->trap_context = (unsigned long)tcntx;
+}
+
+struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx);
 
 #endif
 

include/sbi/sbi_trap_ldst.h

@@ -0,0 +1,31 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <anup.patel@wdc.com>
+ */
+
+#ifndef __SBI_TRAP_LDST_H__
+#define __SBI_TRAP_LDST_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_trap.h>
+
+union sbi_ldst_data {
+	u64 data_u64;
+	u32 data_u32;
+	u8 data_bytes[8];
+	ulong data_ulong;
+};
+
+int sbi_misaligned_load_handler(struct sbi_trap_context *tcntx);
+
+int sbi_misaligned_store_handler(struct sbi_trap_context *tcntx);
+
+int sbi_load_access_handler(struct sbi_trap_context *tcntx);
+
+int sbi_store_access_handler(struct sbi_trap_context *tcntx);
+
+#endif

include/sbi/sbi_unit_test.h

@@ -0,0 +1,73 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Author: Ivan Orlov <ivan.orlov0322@gmail.com>
+ */
+#ifdef CONFIG_SBIUNIT
+#ifndef __SBI_UNIT_H__
+#define __SBI_UNIT_H__
+
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_string.h>
+
+struct sbiunit_test_case {
+	const char *name;
+	bool failed;
+	void (*test_func)(struct sbiunit_test_case *test);
+};
+
+struct sbiunit_test_suite {
+	const char *name;
+	void (*init)(void);
+	struct sbiunit_test_case *cases;
+};
+
+#define SBIUNIT_TEST_CASE(func)		\
+	{				\
+		.name = #func,		\
+		.failed = false,	\
+		.test_func = (func)	\
+	}
+
+#define SBIUNIT_END_CASE { }
+
+#define SBIUNIT_TEST_SUITE(suite_name, cases_arr)		\
+	struct sbiunit_test_suite suite_name = {		\
+		.name = #suite_name,				\
+		.init = NULL,					\
+		.cases = cases_arr				\
+	}
+
+#define _sbiunit_msg(test, msg) "[SBIUnit] [%s:%d]: %s: %s", __FILE__,	\
+				__LINE__, test->name, msg
+
+#define SBIUNIT_INFO(test, msg) sbi_printf(_sbiunit_msg(test, msg))
+#define SBIUNIT_PANIC(test, msg) sbi_panic(_sbiunit_msg(test, msg))
+
+#define SBIUNIT_EXPECT(test, cond) do {							\
+	if (!(cond)) {									\
+		test->failed = true;							\
+		SBIUNIT_INFO(test, "Condition \"" #cond "\" expected to be true!\n");	\
+	}										\
+} while (0)
+
+#define SBIUNIT_ASSERT(test, cond) do {						\
+	if (!(cond))								\
+		SBIUNIT_PANIC(test, "Condition \"" #cond "\" must be true!\n");	\
+} while (0)
+
+#define SBIUNIT_EXPECT_EQ(test, a, b) SBIUNIT_EXPECT(test, (a) == (b))
+#define SBIUNIT_ASSERT_EQ(test, a, b) SBIUNIT_ASSERT(test, (a) == (b))
+#define SBIUNIT_EXPECT_NE(test, a, b) SBIUNIT_EXPECT(test, (a) != (b))
+#define SBIUNIT_ASSERT_NE(test, a, b) SBIUNIT_ASSERT(test, (a) != (b))
+#define SBIUNIT_EXPECT_MEMEQ(test, a, b, len) SBIUNIT_EXPECT(test, !sbi_memcmp(a, b, len))
+#define SBIUNIT_ASSERT_MEMEQ(test, a, b, len) SBIUNIT_ASSERT(test, !sbi_memcmp(a, b, len))
+#define SBIUNIT_EXPECT_STREQ(test, a, b, len) SBIUNIT_EXPECT(test, !sbi_strncmp(a, b, len))
+#define SBIUNIT_ASSERT_STREQ(test, a, b, len) SBIUNIT_ASSERT(test, !sbi_strncmp(a, b, len))
+
+void run_all_tests(void);
+#endif
+#else
+#define run_all_tests()
+#endif

include/sbi/sbi_version.h

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

lib/sbi/Kconfig

@@ -38,6 +38,10 @@ config SBI_ECALL_CPPC
 	bool "CPPC extension"
 	default y
 
+config SBI_ECALL_FWFT
+	bool "Firmware Feature extension"
+	default y
+
 config SBI_ECALL_LEGACY
 	bool "SBI v0.1 legacy extensions"
 	default y
@@ -46,4 +50,16 @@ config SBI_ECALL_VENDOR
 	bool "Platform-defined vendor extensions"
 	default y
 
+config SBI_ECALL_DBTR
+	bool "Debug Trigger Extension"
+	default y
+
+config SBIUNIT
+	bool "Enable SBIUNIT tests"
+	default n
+
+config SBI_ECALL_SSE
+	bool "SSE extension"
+	default y
+
 endmenu

lib/sbi/objects.mk

@@ -46,18 +46,29 @@ libsbi-objs-$(CONFIG_SBI_ECALL_DBCN) += sbi_ecall_dbcn.o
 carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_CPPC) += ecall_cppc
 libsbi-objs-$(CONFIG_SBI_ECALL_CPPC) += sbi_ecall_cppc.o
 
+carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_FWFT) += ecall_fwft
+libsbi-objs-$(CONFIG_SBI_ECALL_FWFT) += sbi_ecall_fwft.o
+
 carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_LEGACY) += ecall_legacy
 libsbi-objs-$(CONFIG_SBI_ECALL_LEGACY) += sbi_ecall_legacy.o
 
 carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_VENDOR) += ecall_vendor
 libsbi-objs-$(CONFIG_SBI_ECALL_VENDOR) += sbi_ecall_vendor.o
 
+carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_DBTR) += ecall_dbtr
+libsbi-objs-$(CONFIG_SBI_ECALL_DBTR) += sbi_ecall_dbtr.o
+
+carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_SSE) += ecall_sse
+libsbi-objs-$(CONFIG_SBI_ECALL_SSE) += sbi_ecall_sse.o
+
 libsbi-objs-y += sbi_bitmap.o
 libsbi-objs-y += sbi_bitops.o
 libsbi-objs-y += sbi_console.o
+libsbi-objs-y += sbi_domain_context.o
 libsbi-objs-y += sbi_domain.o
 libsbi-objs-y += sbi_emulate_csr.o
 libsbi-objs-y += sbi_fifo.o
+libsbi-objs-y += sbi_fwft.o
 libsbi-objs-y += sbi_hart.o
 libsbi-objs-y += sbi_heap.o
 libsbi-objs-y += sbi_math.o
@@ -67,15 +78,17 @@ libsbi-objs-y += sbi_illegal_insn.o
 libsbi-objs-y += sbi_init.o
 libsbi-objs-y += sbi_ipi.o
 libsbi-objs-y += sbi_irqchip.o
-libsbi-objs-y += sbi_misaligned_ldst.o
 libsbi-objs-y += sbi_platform.o
 libsbi-objs-y += sbi_pmu.o
+libsbi-objs-y += sbi_dbtr.o
 libsbi-objs-y += sbi_scratch.o
+libsbi-objs-y += sbi_sse.o
 libsbi-objs-y += sbi_string.o
 libsbi-objs-y += sbi_system.o
 libsbi-objs-y += sbi_timer.o
 libsbi-objs-y += sbi_tlb.o
 libsbi-objs-y += sbi_trap.o
+libsbi-objs-y += sbi_trap_ldst.o
 libsbi-objs-y += sbi_unpriv.o
 libsbi-objs-y += sbi_expected_trap.o
 libsbi-objs-y += sbi_cppc.o

lib/sbi/riscv_asm.c

@@ -291,7 +291,8 @@ int is_pmp_entry_mapped(unsigned long entry)
 	unsigned long addr;
 	unsigned long log2len;
 
-	pmp_get(entry, &prot, &addr, &log2len);
+	if (pmp_get(entry, &prot, &addr, &log2len) != 0)
+		return false;
 
 	/* If address matching bits are non-zero, the entry is enable */
 	if (prot & PMP_A)

lib/sbi/sbi_console.c

@@ -472,7 +472,7 @@ const struct sbi_console_device *sbi_console_get_device(void)
 
 void sbi_console_set_device(const struct sbi_console_device *dev)
 {
-	if (!dev || console_dev)
+	if (!dev)
 		return;
 
 	console_dev = dev;

lib/sbi/sbi_dbtr.c

@@ -0,0 +1,711 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Ventana Micro Systems, Inc.
+ *
+ * Author(s):
+ *   Himanshu Chauhan <hchauhan@ventanamicro.com>
+ */
+
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_csr_detect.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_byteorder.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_dbtr.h>
+#include <sbi/sbi_heap.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_asm.h>
+
+
+/** Offset of pointer to HART's debug triggers info in scratch space */
+static unsigned long hart_state_ptr_offset;
+
+#define dbtr_get_hart_state_ptr(__scratch)				\
+	sbi_scratch_read_type((__scratch), void *, hart_state_ptr_offset)
+
+#define dbtr_thishart_state_ptr()				\
+	dbtr_get_hart_state_ptr(sbi_scratch_thishart_ptr())
+
+#define dbtr_set_hart_state_ptr(__scratch, __hart_state)		\
+	sbi_scratch_write_type((__scratch), void *, hart_state_ptr_offset, \
+			       (__hart_state))
+
+#define INDEX_TO_TRIGGER(_index)					\
+	({								\
+		struct sbi_dbtr_trigger *__trg = NULL;			\
+		struct sbi_dbtr_hart_triggers_state *__hs = NULL;	\
+		__hs = dbtr_get_hart_state_ptr(sbi_scratch_thishart_ptr()); \
+		__trg = &__hs->triggers[_index];			\
+		(__trg);						\
+	})
+
+#define for_each_trig_entry(_base, _max, _etype, _entry)		\
+	for (int _idx = 0; _entry = ((_etype *)_base + _idx),		\
+	     _idx < _max;						\
+	     _idx++, _entry = ((_etype *)_base + _idx))
+
+#define DBTR_SHMEM_MAKE_PHYS(_p_hi, _p_lo) (_p_lo)
+
+/* must call with hs != NULL */
+static inline bool sbi_dbtr_shmem_disabled(
+	struct sbi_dbtr_hart_triggers_state *hs)
+{
+	return (hs->shmem.phys_lo == SBI_DBTR_SHMEM_INVALID_ADDR &&
+		hs->shmem.phys_hi == SBI_DBTR_SHMEM_INVALID_ADDR
+		? true : false);
+}
+
+/* must call with hs != NULL */
+static inline void sbi_dbtr_disable_shmem(
+	struct sbi_dbtr_hart_triggers_state *hs)
+{
+	hs->shmem.phys_lo = SBI_DBTR_SHMEM_INVALID_ADDR;
+	hs->shmem.phys_hi = SBI_DBTR_SHMEM_INVALID_ADDR;
+}
+
+/* must call with hs which is not disabled */
+static inline void *hart_shmem_base(
+	struct sbi_dbtr_hart_triggers_state *hs)
+{
+	return ((void *)(unsigned long)DBTR_SHMEM_MAKE_PHYS(
+			hs->shmem.phys_hi, hs->shmem.phys_lo));
+}
+
+static void sbi_trigger_init(struct sbi_dbtr_trigger *trig,
+			     unsigned long type_mask, unsigned long idx)
+{
+	trig->type_mask = type_mask;
+	trig->state = 0;
+	trig->tdata1 = 0;
+	trig->tdata2 = 0;
+	trig->tdata3 = 0;
+	trig->index = idx;
+}
+
+static inline struct sbi_dbtr_trigger *sbi_alloc_trigger(void)
+{
+	int i;
+	struct sbi_dbtr_trigger *f_trig = NULL;
+	struct sbi_dbtr_hart_triggers_state *hart_state;
+
+	hart_state = dbtr_thishart_state_ptr();
+	if (!hart_state)
+		return NULL;
+
+	if (hart_state->available_trigs <= 0)
+		return NULL;
+
+	for (i = 0; i < hart_state->total_trigs; i++) {
+		f_trig = INDEX_TO_TRIGGER(i);
+		if (f_trig->state & RV_DBTR_BIT_MASK(TS, MAPPED))
+			continue;
+		hart_state->available_trigs--;
+		break;
+	}
+
+	if (i == hart_state->total_trigs)
+		return NULL;
+
+	__set_bit(RV_DBTR_BIT(TS, MAPPED), &f_trig->state);
+
+	return f_trig;
+}
+
+static inline void sbi_free_trigger(struct sbi_dbtr_trigger *trig)
+{
+	struct sbi_dbtr_hart_triggers_state *hart_state;
+
+	if (trig == NULL)
+		return;
+
+	hart_state = dbtr_thishart_state_ptr();
+	if (!hart_state)
+		return;
+
+	trig->state = 0;
+	trig->tdata1 = 0;
+	trig->tdata2 = 0;
+	trig->tdata3 = 0;
+
+	hart_state->available_trigs++;
+}
+
+int sbi_dbtr_init(struct sbi_scratch *scratch, bool coldboot)
+{
+	struct sbi_trap_info trap = {0};
+	unsigned long tdata1;
+	unsigned long val;
+	int i;
+	struct sbi_dbtr_hart_triggers_state *hart_state = NULL;
+
+	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SDTRIG))
+		return SBI_SUCCESS;
+
+	if (coldboot) {
+		hart_state_ptr_offset = sbi_scratch_alloc_type_offset(void *);
+		if (!hart_state_ptr_offset)
+			return SBI_ENOMEM;
+	}
+
+	hart_state = dbtr_get_hart_state_ptr(scratch);
+	if (!hart_state) {
+		hart_state = sbi_zalloc(sizeof(*hart_state));
+		if (!hart_state)
+			return SBI_ENOMEM;
+		hart_state->hartid = current_hartid();
+		dbtr_set_hart_state_ptr(scratch, hart_state);
+	}
+
+	/* disable the shared memory */
+	sbi_dbtr_disable_shmem(hart_state);
+
+	/* Skip probing triggers if already probed */
+	if (hart_state->probed)
+		goto _probed;
+
+	for (i = 0; i < RV_MAX_TRIGGERS; i++) {
+		csr_write_allowed(CSR_TSELECT, (ulong)&trap, i);
+		if (trap.cause)
+			break;
+
+		val = csr_read_allowed(CSR_TSELECT, (ulong)&trap);
+		if (trap.cause)
+			break;
+
+		/*
+		 * Read back tselect and check that it contains the
+		 * written value
+		 */
+		if (val != i)
+			break;
+
+		val = csr_read_allowed(CSR_TINFO, (ulong)&trap);
+		if (trap.cause) {
+			/*
+			 * If reading tinfo caused an exception, the
+			 * debugger must read tdata1 to discover the
+			 * type.
+			 */
+			tdata1 = csr_read_allowed(CSR_TDATA1,
+						  (ulong)&trap);
+			if (trap.cause)
+				break;
+
+			if (TDATA1_GET_TYPE(tdata1) == 0)
+				break;
+
+			sbi_trigger_init(INDEX_TO_TRIGGER(i),
+					 BIT(TDATA1_GET_TYPE(tdata1)),
+					 i);
+			hart_state->total_trigs++;
+		} else {
+			if (val == 1)
+				break;
+
+			sbi_trigger_init(INDEX_TO_TRIGGER(i), val, i);
+			hart_state->total_trigs++;
+		}
+	}
+
+	hart_state->probed = 1;
+
+ _probed:
+	hart_state->available_trigs = hart_state->total_trigs;
+
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_get_total_triggers(void)
+{
+	struct sbi_dbtr_hart_triggers_state *hs;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+
+	/*
+	 * This function may be used during ecall registration.
+	 * By that time the debug trigger module might not be
+	 * initialized. If the extension is not supported, report
+	 * number of triggers as 0.
+	 */
+	if (!sbi_hart_has_extension(scratch, SBI_HART_EXT_SDTRIG))
+		return 0;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return 0;
+
+	return hs->total_trigs;
+}
+
+int sbi_dbtr_setup_shmem(const struct sbi_domain *dom, unsigned long smode,
+			 unsigned long shmem_phys_lo,
+			 unsigned long shmem_phys_hi)
+{
+	u32 hartid = current_hartid();
+	struct sbi_dbtr_hart_triggers_state *hart_state;
+
+	if (dom && !sbi_domain_is_assigned_hart(dom, hartid)) {
+		sbi_dprintf("%s: calling hart not assigned to this domain\n",
+			   __func__);
+		return SBI_ERR_DENIED;
+	}
+
+	hart_state = dbtr_thishart_state_ptr();
+	if (!hart_state)
+		return SBI_ERR_FAILED;
+
+	/* call is to disable shared memory */
+	if (shmem_phys_lo == SBI_DBTR_SHMEM_INVALID_ADDR
+	    && shmem_phys_hi == SBI_DBTR_SHMEM_INVALID_ADDR) {
+		sbi_dbtr_disable_shmem(hart_state);
+		return SBI_SUCCESS;
+	}
+
+	/* the shared memory must be disabled on this hart */
+	if (!sbi_dbtr_shmem_disabled(hart_state))
+		return SBI_ERR_ALREADY_AVAILABLE;
+
+	/* lower physical address must be XLEN/8 bytes aligned */
+	if (shmem_phys_lo & SBI_DBTR_SHMEM_ALIGN_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_EINVALID_ADDR;
+
+	if (dom && !sbi_domain_check_addr(dom,
+		  DBTR_SHMEM_MAKE_PHYS(shmem_phys_hi, shmem_phys_lo), smode,
+		  SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
+		return SBI_ERR_INVALID_ADDRESS;
+
+	hart_state->shmem.phys_lo = shmem_phys_lo;
+	hart_state->shmem.phys_hi = shmem_phys_hi;
+
+	return SBI_SUCCESS;
+}
+
+static void dbtr_trigger_setup(struct sbi_dbtr_trigger *trig,
+			       struct sbi_dbtr_data_msg *recv)
+{
+	unsigned long tdata1;
+
+	if (!trig)
+		return;
+
+	trig->tdata1 = lle_to_cpu(recv->tdata1);
+	trig->tdata2 = lle_to_cpu(recv->tdata2);
+	trig->tdata3 = lle_to_cpu(recv->tdata3);
+
+	tdata1 = lle_to_cpu(recv->tdata1);
+
+	trig->state = 0;
+
+	__set_bit(RV_DBTR_BIT(TS, MAPPED), &trig->state);
+
+	SET_TRIG_HW_INDEX(trig->state, trig->index);
+
+	switch (TDATA1_GET_TYPE(tdata1)) {
+	case RISCV_DBTR_TRIG_MCONTROL:
+		if (__test_bit(RV_DBTR_BIT(MC, U), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, U), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(MC, S), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, S), &trig->state);
+		break;
+	case RISCV_DBTR_TRIG_MCONTROL6:
+		if (__test_bit(RV_DBTR_BIT(MC6, U), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, U), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(MC6, S), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, S), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(MC6, VU), &tdata1))
+			__set_bit(RV_DBTR_BIT(TS, VU), &trig->state);
+
+		if (__test_bit(RV_DBTR_BIT(MC6, 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));
+		break;
+	}
+}
+
+static inline void update_bit(unsigned long new, int nr, volatile unsigned long *addr)
+{
+	if (new)
+		__set_bit(nr, addr);
+	else
+		__clear_bit(nr, addr);
+}
+
+static void dbtr_trigger_enable(struct sbi_dbtr_trigger *trig)
+{
+	unsigned long state;
+	unsigned long tdata1;
+
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+		return;
+
+	state = trig->state;
+	tdata1 = trig->tdata1;
+
+	switch (TDATA1_GET_TYPE(tdata1)) {
+	case RISCV_DBTR_TRIG_MCONTROL:
+		update_bit(state & RV_DBTR_BIT_MASK(TS, U),
+			   RV_DBTR_BIT(MC, U), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, S),
+			   RV_DBTR_BIT(MC, S), &trig->tdata1);
+		break;
+	case RISCV_DBTR_TRIG_MCONTROL6:
+		update_bit(state & RV_DBTR_BIT_MASK(TS, VU),
+			   RV_DBTR_BIT(MC6, VU), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, VS),
+			   RV_DBTR_BIT(MC6, VS), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, U),
+			   RV_DBTR_BIT(MC6, U), &trig->tdata1);
+		update_bit(state & RV_DBTR_BIT_MASK(TS, S),
+			   RV_DBTR_BIT(MC6, S), &trig->tdata1);
+		break;
+	default:
+		break;
+	}
+
+	/*
+	 * RISC-V Debug Support v1.0.0 section 5.5:
+	 * Debugger cannot simply set a trigger by writing tdata1, then tdata2,
+	 * etc. The current value of tdata2 might not be legal with the new
+	 * value of tdata1. To help with this situation, it is guaranteed that
+	 * writing 0 to tdata1 disables the trigger, and leaves it in a state
+	 * where tdata2 and tdata3 can be written with any value that makes
+	 * sense for any trigger type supported by this trigger.
+	 */
+	csr_write(CSR_TSELECT, trig->index);
+	csr_write(CSR_TDATA1, 0x0);
+	csr_write(CSR_TDATA2, trig->tdata2);
+	csr_write(CSR_TDATA1, trig->tdata1);
+}
+
+static void dbtr_trigger_disable(struct sbi_dbtr_trigger *trig)
+{
+	unsigned long tdata1;
+
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+		return;
+
+	tdata1 = trig->tdata1;
+
+	switch (TDATA1_GET_TYPE(tdata1)) {
+	case RISCV_DBTR_TRIG_MCONTROL:
+		__clear_bit(RV_DBTR_BIT(MC, U), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(MC, S), &trig->tdata1);
+		break;
+	case RISCV_DBTR_TRIG_MCONTROL6:
+		__clear_bit(RV_DBTR_BIT(MC6, VU), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(MC6, VS), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(MC6, U), &trig->tdata1);
+		__clear_bit(RV_DBTR_BIT(MC6, S), &trig->tdata1);
+		break;
+	default:
+		break;
+	}
+
+	csr_write(CSR_TSELECT, trig->index);
+	csr_write(CSR_TDATA1, trig->tdata1);
+}
+
+static void dbtr_trigger_clear(struct sbi_dbtr_trigger *trig)
+{
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+		return;
+
+	csr_write(CSR_TSELECT, trig->index);
+	csr_write(CSR_TDATA1, 0x0);
+	csr_write(CSR_TDATA2, 0x0);
+}
+
+static int dbtr_trigger_supported(unsigned long type)
+{
+	switch (type) {
+	case RISCV_DBTR_TRIG_MCONTROL:
+	case RISCV_DBTR_TRIG_MCONTROL6:
+		return 1;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static int dbtr_trigger_valid(unsigned long type, unsigned long tdata)
+{
+	switch (type) {
+	case RISCV_DBTR_TRIG_MCONTROL:
+		if (!(tdata & RV_DBTR_BIT_MASK(MC, DMODE)) &&
+		    !(tdata & RV_DBTR_BIT_MASK(MC, M)))
+			return 1;
+		break;
+	case RISCV_DBTR_TRIG_MCONTROL6:
+		if (!(tdata & RV_DBTR_BIT_MASK(MC6, DMODE)) &&
+		    !(tdata & RV_DBTR_BIT_MASK(MC6, M)))
+			return 1;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+int sbi_dbtr_num_trig(unsigned long data, unsigned long *out)
+{
+	unsigned long type = TDATA1_GET_TYPE(data);
+	u32 hartid = current_hartid();
+	unsigned long total = 0;
+	struct sbi_dbtr_trigger *trig;
+	int i;
+	struct sbi_dbtr_hart_triggers_state *hs;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	if (data == 0) {
+		*out = hs->total_trigs;
+		return SBI_SUCCESS;
+	}
+
+	for (i = 0; i < hs->total_trigs; i++) {
+		trig = INDEX_TO_TRIGGER(i);
+
+		if (__test_bit(type, &trig->type_mask))
+			total++;
+	}
+
+	sbi_dprintf("%s: hart%d: total triggers of type %lu: %lu\n",
+		    __func__, hartid, type, total);
+
+	*out = total;
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_read_trig(unsigned long smode,
+		       unsigned long trig_idx_base, unsigned long trig_count)
+{
+	struct sbi_dbtr_data_msg *xmit;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_shmem_entry *entry;
+	void *shmem_base = NULL;
+	struct sbi_dbtr_hart_triggers_state *hs = NULL;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	if (trig_idx_base >= hs->total_trigs ||
+	    trig_idx_base + trig_count >= hs->total_trigs)
+		return SBI_ERR_INVALID_PARAM;
+
+	if (sbi_dbtr_shmem_disabled(hs))
+		return SBI_ERR_NO_SHMEM;
+
+	shmem_base = hart_shmem_base(hs);
+
+	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));
+		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();
+	}
+
+	return SBI_SUCCESS;
+}
+
+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;
+	struct sbi_dbtr_data_msg *recv;
+	struct sbi_dbtr_id_msg *xmit;
+	unsigned long ctrl;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_hart_triggers_state *hs = NULL;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	if (sbi_dbtr_shmem_disabled(hs))
+		return SBI_ERR_NO_SHMEM;
+
+	shmem_base = hart_shmem_base(hs);
+
+	/* 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;
+
+		if (!dbtr_trigger_supported(TDATA1_GET_TYPE(ctrl))) {
+			*out = _idx;
+			sbi_hart_unmap_saddr();
+			return SBI_ERR_FAILED;
+		}
+
+		if (!dbtr_trigger_valid(TDATA1_GET_TYPE(ctrl), ctrl)) {
+			*out = _idx;
+			sbi_hart_unmap_saddr();
+			return SBI_ERR_FAILED;
+		}
+		sbi_hart_unmap_saddr();
+	}
+
+	if (hs->available_trigs < trig_count) {
+		*out = hs->available_trigs;
+		return SBI_ERR_FAILED;
+	}
+
+	/* Install triggers */
+	for_each_trig_entry(shmem_base, trig_count, typeof(*entry), entry) {
+		/*
+		 * Since we have already checked if enough triggers are
+		 * available, trigger allocation must succeed.
+		 */
+		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();
+	}
+
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_uninstall_trig(unsigned long trig_idx_base,
+			    unsigned long trig_idx_mask)
+{
+	unsigned long trig_mask = trig_idx_mask << trig_idx_base;
+	unsigned long idx = trig_idx_base;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_hart_triggers_state *hs;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	for_each_set_bit_from(idx, &trig_mask, hs->total_trigs) {
+		trig = INDEX_TO_TRIGGER(idx);
+		if (!(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+			return SBI_ERR_INVALID_PARAM;
+
+		dbtr_trigger_clear(trig);
+
+		sbi_free_trigger(trig);
+	}
+
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_enable_trig(unsigned long trig_idx_base,
+			 unsigned long trig_idx_mask)
+{
+	unsigned long trig_mask = trig_idx_mask << trig_idx_base;
+	unsigned long idx = trig_idx_base;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_hart_triggers_state *hs;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	for_each_set_bit_from(idx, &trig_mask, hs->total_trigs) {
+		trig = INDEX_TO_TRIGGER(idx);
+		sbi_dprintf("%s: enable trigger %lu\n", __func__, idx);
+		dbtr_trigger_enable(trig);
+	}
+
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_update_trig(unsigned long smode,
+			 unsigned long trig_idx_base,
+			 unsigned long trig_idx_mask)
+{
+	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;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_shmem_entry *entry;
+	void *shmem_base = NULL;
+	struct sbi_dbtr_hart_triggers_state *hs = NULL;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	if (sbi_dbtr_shmem_disabled(hs))
+		return SBI_ERR_NO_SHMEM;
+
+	shmem_base = hart_shmem_base(hs);
+
+	for_each_set_bit_from(idx, &trig_mask, hs->total_trigs) {
+		trig = INDEX_TO_TRIGGER(idx);
+
+		if (!(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+			return SBI_ERR_INVALID_PARAM;
+
+		entry = (shmem_base + uidx * sizeof(*entry));
+		recv = &entry->data;
+
+		trig->tdata2 = lle_to_cpu(recv->tdata2);
+		dbtr_trigger_enable(trig);
+		uidx++;
+	}
+
+	return SBI_SUCCESS;
+}
+
+int sbi_dbtr_disable_trig(unsigned long trig_idx_base,
+			  unsigned long trig_idx_mask)
+{
+	unsigned long trig_mask = trig_idx_mask << trig_idx_base;
+	unsigned long idx = trig_idx_base;
+	struct sbi_dbtr_trigger *trig;
+	struct sbi_dbtr_hart_triggers_state *hs;
+
+	hs = dbtr_thishart_state_ptr();
+	if (!hs)
+		return SBI_ERR_FAILED;
+
+	for_each_set_bit_from(idx, &trig_mask, hs->total_trigs) {
+		trig = INDEX_TO_TRIGGER(idx);
+		dbtr_trigger_disable(trig);
+	}
+
+	return SBI_SUCCESS;
+}

lib/sbi/sbi_domain.c

@@ -51,7 +51,7 @@ struct sbi_domain *sbi_hartindex_to_domain(u32 hartindex)
 	return sbi_scratch_read_type(scratch, void *, domain_hart_ptr_offset);
 }
 
-static void update_hartindex_to_domain(u32 hartindex, struct sbi_domain *dom)
+void sbi_update_hartindex_to_domain(u32 hartindex, struct sbi_domain *dom)
 {
 	struct sbi_scratch *scratch;
 
@@ -64,20 +64,34 @@ static void update_hartindex_to_domain(u32 hartindex, struct sbi_domain *dom)
 
 bool sbi_domain_is_assigned_hart(const struct sbi_domain *dom, u32 hartid)
 {
-	if (dom)
-		return sbi_hartmask_test_hartid(hartid, &dom->assigned_harts);
+	bool ret;
+	struct sbi_domain *tdom = (struct sbi_domain *)dom;
 
-	return false;
+	if (!dom)
+		return false;
+
+	spin_lock(&tdom->assigned_harts_lock);
+	ret = sbi_hartmask_test_hartid(hartid, &tdom->assigned_harts);
+	spin_unlock(&tdom->assigned_harts_lock);
+
+	return ret;
 }
 
 ulong sbi_domain_get_assigned_hartmask(const struct sbi_domain *dom,
 				       ulong hbase)
 {
 	ulong ret = 0;
+	struct sbi_domain *tdom = (struct sbi_domain *)dom;
+
+	if (!dom)
+		return 0;
+
+	spin_lock(&tdom->assigned_harts_lock);
 	for (int i = 0; i < 8 * sizeof(ret); i++) {
-		if (sbi_domain_is_assigned_hart(dom, hbase + i))
+		if (sbi_hartmask_test_hartid(hbase + i, &tdom->assigned_harts))
 			ret |= 1UL << i;
 	}
+	spin_unlock(&tdom->assigned_harts_lock);
 
 	return ret;
 }
@@ -555,6 +569,9 @@ int sbi_domain_register(struct sbi_domain *dom,
 	dom->index = domain_count++;
 	domidx_to_domain_table[dom->index] = dom;
 
+	/* Initialize spinlock for dom->assigned_harts */
+	SPIN_LOCK_INIT(dom->assigned_harts_lock);
+
 	/* Clear assigned HARTs of domain */
 	sbi_hartmask_clear_all(&dom->assigned_harts);
 
@@ -567,7 +584,7 @@ int sbi_domain_register(struct sbi_domain *dom,
 		if (tdom)
 			sbi_hartmask_clear_hartindex(i,
 					&tdom->assigned_harts);
-		update_hartindex_to_domain(i, dom);
+		sbi_update_hartindex_to_domain(i, dom);
 		sbi_hartmask_set_hartindex(i, &dom->assigned_harts);
 
 		/*
@@ -701,8 +718,14 @@ int sbi_domain_finalize(struct sbi_scratch *scratch, u32 cold_hartid)
 			continue;
 
 		/* Ignore if boot HART assigned different domain */
-		if (sbi_hartindex_to_domain(dhart) != dom ||
-		    !sbi_hartmask_test_hartindex(dhart, &dom->assigned_harts))
+		if (sbi_hartindex_to_domain(dhart) != dom)
+			continue;
+
+		/* Ignore if boot HART is not part of the assigned HARTs */
+		spin_lock(&dom->assigned_harts_lock);
+		rc = sbi_hartmask_test_hartindex(dhart, &dom->assigned_harts);
+		spin_unlock(&dom->assigned_harts_lock);
+		if (!rc)
 			continue;
 
 		/* Startup boot HART of domain */

lib/sbi/sbi_domain_context.c

@@ -0,0 +1,162 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) IPADS@SJTU 2023. All rights reserved.
+ */
+
+#include <sbi/sbi_error.h>
+#include <sbi/riscv_locks.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_heap.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_domain_context.h>
+
+/**
+ * Switches the HART context from the current domain to the target domain.
+ * This includes changing domain assignments and reconfiguring PMP, as well
+ * as saving and restoring CSRs and trap states.
+ *
+ * @param ctx pointer to the current HART context
+ * @param dom_ctx pointer to the target domain context
+ */
+static void switch_to_next_domain_context(struct sbi_context *ctx,
+					  struct sbi_context *dom_ctx)
+{
+	u32 hartindex = sbi_hartid_to_hartindex(current_hartid());
+	struct sbi_trap_context *trap_ctx;
+	struct sbi_domain *current_dom = ctx->dom;
+	struct sbi_domain *target_dom = dom_ctx->dom;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	unsigned int pmp_count = sbi_hart_pmp_count(scratch);
+
+	/* Assign current hart to target domain */
+	spin_lock(&current_dom->assigned_harts_lock);
+	sbi_hartmask_clear_hartindex(hartindex, &current_dom->assigned_harts);
+	spin_unlock(&current_dom->assigned_harts_lock);
+
+	sbi_update_hartindex_to_domain(hartindex, target_dom);
+
+	spin_lock(&target_dom->assigned_harts_lock);
+	sbi_hartmask_set_hartindex(hartindex, &target_dom->assigned_harts);
+	spin_unlock(&target_dom->assigned_harts_lock);
+
+	/* Reconfigure PMP settings for the new domain */
+	for (int i = 0; i < pmp_count; i++) {
+		pmp_disable(i);
+	}
+	sbi_hart_pmp_configure(scratch);
+
+	/* Save current CSR context and restore target domain's CSR context */
+	ctx->sstatus	= csr_swap(CSR_SSTATUS, dom_ctx->sstatus);
+	ctx->sie	= csr_swap(CSR_SIE, dom_ctx->sie);
+	ctx->stvec	= csr_swap(CSR_STVEC, dom_ctx->stvec);
+	ctx->sscratch	= csr_swap(CSR_SSCRATCH, dom_ctx->sscratch);
+	ctx->sepc	= csr_swap(CSR_SEPC, dom_ctx->sepc);
+	ctx->scause	= csr_swap(CSR_SCAUSE, dom_ctx->scause);
+	ctx->stval	= csr_swap(CSR_STVAL, dom_ctx->stval);
+	ctx->sip	= csr_swap(CSR_SIP, dom_ctx->sip);
+	ctx->satp	= csr_swap(CSR_SATP, dom_ctx->satp);
+	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_10)
+		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);
+
+	/* Save current trap state and restore target domain's trap state */
+	trap_ctx = sbi_trap_get_context(scratch);
+	sbi_memcpy(&ctx->trap_ctx, trap_ctx, sizeof(*trap_ctx));
+	sbi_memcpy(trap_ctx, &dom_ctx->trap_ctx, sizeof(*trap_ctx));
+
+	/* Mark current context structure initialized because context saved */
+	ctx->initialized = true;
+
+	/* If target domain context is not initialized or runnable */
+	if (!dom_ctx->initialized) {
+		/* Startup boot HART of target domain */
+		if (current_hartid() == target_dom->boot_hartid)
+			sbi_hart_switch_mode(target_dom->boot_hartid,
+					     target_dom->next_arg1,
+					     target_dom->next_addr,
+					     target_dom->next_mode,
+					     false);
+		else
+			sbi_hsm_hart_stop(scratch, true);
+	}
+}
+
+int sbi_domain_context_enter(struct sbi_domain *dom)
+{
+	struct sbi_context *ctx = sbi_domain_context_thishart_ptr();
+	struct sbi_context *dom_ctx = sbi_hartindex_to_domain_context(
+		sbi_hartid_to_hartindex(current_hartid()), dom);
+
+	/* Validate the domain context existence */
+	if (!dom_ctx)
+		return SBI_EINVAL;
+
+	/* 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;
+}
+
+int sbi_domain_context_exit(void)
+{
+	u32 i, hartindex = sbi_hartid_to_hartindex(current_hartid());
+	struct sbi_domain *dom;
+	struct sbi_context *ctx = sbi_domain_context_thishart_ptr();
+	struct sbi_context *dom_ctx, *tmp;
+
+	/*
+	 * If it's first time to call `exit` on the current hart, no
+	 * context allocated before. Loop through each domain to allocate
+	 * its context on the current hart if valid.
+	 */
+	if (!ctx) {
+		sbi_domain_for_each(i, dom) {
+			if (!sbi_hartmask_test_hartindex(hartindex,
+							 dom->possible_harts))
+				continue;
+
+			dom_ctx = sbi_zalloc(sizeof(struct sbi_context));
+			if (!dom_ctx)
+				return SBI_ENOMEM;
+
+			/* Bind context and domain */
+			dom_ctx->dom				   = dom;
+			dom->hartindex_to_context_table[hartindex] = dom_ctx;
+		}
+
+		ctx = sbi_domain_context_thishart_ptr();
+	}
+
+	dom_ctx = ctx->prev_ctx;
+
+	/* If no previous caller context */
+	if (!dom_ctx) {
+		/* Try to find next uninitialized user-defined domain's context */
+		sbi_domain_for_each(i, dom) {
+			if (dom == &root || dom == sbi_domain_thishart_ptr())
+				continue;
+
+			tmp = sbi_hartindex_to_domain_context(hartindex, dom);
+			if (tmp && !tmp->initialized) {
+				dom_ctx = tmp;
+				break;
+			}
+		}
+	}
+
+	/* Take the root domain context if fail to find */
+	if (!dom_ctx)
+		dom_ctx = sbi_hartindex_to_domain_context(hartindex, &root);
+
+	switch_to_next_domain_context(ctx, dom_ctx);
+
+	return 0;
+}

lib/sbi/sbi_ecall.c

@@ -95,9 +95,10 @@ void sbi_ecall_unregister_extension(struct sbi_ecall_extension *ext)
 		sbi_list_del_init(&ext->head);
 }
 
-int sbi_ecall_handler(struct sbi_trap_regs *regs)
+int sbi_ecall_handler(struct sbi_trap_context *tcntx)
 {
 	int ret = 0;
+	struct sbi_trap_regs *regs = &tcntx->regs;
 	struct sbi_ecall_extension *ext;
 	unsigned long extension_id = regs->a7;
 	unsigned long func_id = regs->a6;

lib/sbi/sbi_ecall_dbtr.c

@@ -0,0 +1,73 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Ventana Micro Systems Inc.
+ *
+ * Author(s):
+ *   Himanshu Chauhan <hchauhan@ventanamicro.com>
+ */
+
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_domain.h>
+#include <sbi/sbi_dbtr.h>
+
+static int sbi_ecall_dbtr_handler(unsigned long extid, unsigned long funcid,
+				  struct sbi_trap_regs *regs,
+				  struct sbi_ecall_return *out)
+{
+	unsigned long smode = (csr_read(CSR_MSTATUS) & MSTATUS_MPP) >>
+			MSTATUS_MPP_SHIFT;
+	int ret = 0;
+
+	switch (funcid) {
+	case SBI_EXT_DBTR_NUM_TRIGGERS:
+		ret = sbi_dbtr_num_trig(regs->a0, &out->value);
+		break;
+	case SBI_EXT_DBTR_SETUP_SHMEM:
+		ret = sbi_dbtr_setup_shmem(sbi_domain_thishart_ptr(), smode,
+					   regs->a0, regs->a1);
+		break;
+	case SBI_EXT_DBTR_TRIGGER_READ:
+		ret = sbi_dbtr_read_trig(smode, regs->a0, regs->a1);
+		break;
+	case SBI_EXT_DBTR_TRIGGER_INSTALL:
+		ret = sbi_dbtr_install_trig(smode, regs->a0, &out->value);
+		break;
+	case SBI_EXT_DBTR_TRIGGER_UNINSTALL:
+		ret = sbi_dbtr_uninstall_trig(regs->a0, regs->a1);
+		break;
+	case SBI_EXT_DBTR_TRIGGER_ENABLE:
+		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);
+		break;
+	case SBI_EXT_DBTR_TRIGGER_DISABLE:
+		ret = sbi_dbtr_disable_trig(regs->a0, regs->a1);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	};
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_dbtr;
+
+static int sbi_ecall_dbtr_register_extensions(void)
+{
+	if (sbi_dbtr_get_total_triggers() == 0)
+		return 0;
+
+	return sbi_ecall_register_extension(&ecall_dbtr);
+}
+
+struct sbi_ecall_extension ecall_dbtr = {
+	.extid_start = SBI_EXT_DBTR,
+	.extid_end = SBI_EXT_DBTR,
+	.handle = sbi_ecall_dbtr_handler,
+	.register_extensions = sbi_ecall_dbtr_register_extensions,
+};

lib/sbi/sbi_ecall_fwft.c

@@ -0,0 +1,49 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2024 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <cleger@rivosinc.com>
+ */
+
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_fwft.h>
+#include <sbi/sbi_trap.h>
+
+static int sbi_ecall_fwft_handler(unsigned long extid, unsigned long funcid,
+				 struct sbi_trap_regs *regs,
+				 struct sbi_ecall_return *out)
+{
+	int ret = 0;
+
+	switch (funcid) {
+	case SBI_EXT_FWFT_SET:
+		ret = sbi_fwft_set(regs->a0, regs->a1, regs->a2);
+		break;
+	case SBI_EXT_FWFT_GET:
+		ret = sbi_fwft_get(regs->a0, &out->value);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+		break;
+	}
+
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_fwft;
+
+static int sbi_ecall_fwft_register_extensions(void)
+{
+	return sbi_ecall_register_extension(&ecall_fwft);
+}
+
+struct sbi_ecall_extension ecall_fwft = {
+	.extid_start = SBI_EXT_FWFT,
+	.extid_end = SBI_EXT_FWFT,
+	.register_extensions = sbi_ecall_fwft_register_extensions,
+	.handle = sbi_ecall_fwft_handler,
+};

lib/sbi/sbi_ecall_legacy.c

@@ -74,7 +74,6 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 						&hmask, &trap)) {
 			ret = sbi_ipi_send_smode(hmask, 0);
 		} else {
-			trap.epc = regs->mepc;
 			sbi_trap_redirect(regs, &trap);
 			out->skip_regs_update = true;
 		}
@@ -86,7 +85,6 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 					  SBI_TLB_FENCE_I, source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		} else {
-			trap.epc = regs->mepc;
 			sbi_trap_redirect(regs, &trap);
 			out->skip_regs_update = true;
 		}
@@ -98,7 +96,6 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 					  SBI_TLB_SFENCE_VMA, source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		} else {
-			trap.epc = regs->mepc;
 			sbi_trap_redirect(regs, &trap);
 			out->skip_regs_update = true;
 		}
@@ -112,7 +109,6 @@ static int sbi_ecall_legacy_handler(unsigned long extid, unsigned long funcid,
 					  source_hart);
 			ret = sbi_tlb_request(hmask, 0, &tlb_info);
 		} else {
-			trap.epc = regs->mepc;
 			sbi_trap_redirect(regs, &trap);
 			out->skip_regs_update = true;
 		}

lib/sbi/sbi_ecall_sse.c

@@ -0,0 +1,57 @@
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_ecall.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_sse.h>
+
+static int sbi_ecall_sse_handler(unsigned long extid, unsigned long funcid,
+				  struct sbi_trap_regs *regs,
+				  struct sbi_ecall_return *out)
+{
+	int ret;
+
+	switch (funcid) {
+	case SBI_EXT_SSE_READ_ATTR:
+		ret = sbi_sse_read_attrs(regs->a0, regs->a1, regs->a2,
+					 regs->a3, regs->a4);
+		break;
+	case SBI_EXT_SSE_WRITE_ATTR:
+		ret = sbi_sse_write_attrs(regs->a0, regs->a1, regs->a2,
+					 regs->a3, regs->a4);
+		break;
+	case SBI_EXT_SSE_REGISTER:
+		ret = sbi_sse_register(regs->a0, regs->a1, regs->a2);
+		break;
+	case SBI_EXT_SSE_UNREGISTER:
+		ret = sbi_sse_unregister(regs->a0);
+		break;
+	case SBI_EXT_SSE_ENABLE:
+		ret = sbi_sse_enable(regs->a0);
+		break;
+	case SBI_EXT_SSE_DISABLE:
+		ret = sbi_sse_disable(regs->a0);
+		break;
+	case SBI_EXT_SSE_COMPLETE:
+		ret = sbi_sse_complete(regs, out);
+		break;
+	case SBI_EXT_SSE_INJECT:
+		ret = sbi_sse_inject_from_ecall(regs->a0, regs->a1, out);
+		break;
+	default:
+		ret = SBI_ENOTSUPP;
+	}
+	return ret;
+}
+
+struct sbi_ecall_extension ecall_sse;
+
+static int sbi_ecall_sse_register_extensions(void)
+{
+	return sbi_ecall_register_extension(&ecall_sse);
+}
+
+struct sbi_ecall_extension ecall_sse = {
+	.extid_start		= SBI_EXT_SSE,
+	.extid_end		= SBI_EXT_SSE,
+	.register_extensions	= sbi_ecall_sse_register_extensions,
+	.handle			= sbi_ecall_sse_handler,
+};

lib/sbi/sbi_expected_trap.S

@@ -23,8 +23,6 @@
 	.global __sbi_expected_trap
 __sbi_expected_trap:
 	/* Without H-extension so, MTVAL2 and MTINST CSRs and GVA not available */
-	csrr	a4, CSR_MEPC
-	REG_S	a4, SBI_TRAP_INFO_OFFSET(epc)(a3)
 	csrr	a4, CSR_MCAUSE
 	REG_S	a4, SBI_TRAP_INFO_OFFSET(cause)(a3)
 	csrr	a4, CSR_MTVAL
@@ -41,8 +39,6 @@ __sbi_expected_trap:
 	.global __sbi_expected_trap_hext
 __sbi_expected_trap_hext:
 	/* With H-extension so, MTVAL2 and MTINST CSRs and GVA available */
-	csrr	a4, CSR_MEPC
-	REG_S	a4, SBI_TRAP_INFO_OFFSET(epc)(a3)
 	csrr	a4, CSR_MCAUSE
 	REG_S	a4, SBI_TRAP_INFO_OFFSET(cause)(a3)
 	csrr	a4, CSR_MTVAL

lib/sbi/sbi_fwft.c

@@ -0,0 +1,266 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2024 Rivos Inc.
+ *
+ * Authors:
+ *   Clément Léger <cleger@rivosinc.com>
+ */
+
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_bitmap.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_heap.h>
+#include <sbi/sbi_scratch.h>
+#include <sbi/sbi_string.h>
+#include <sbi/sbi_types.h>
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+
+/** Offset of pointer to FWFT HART state in scratch space */
+static unsigned long fwft_ptr_offset;
+
+#define fwft_get_hart_state_ptr(__scratch)				\
+	sbi_scratch_read_type((__scratch), void *, fwft_ptr_offset)
+
+#define fwft_thishart_state_ptr()					\
+	fwft_get_hart_state_ptr(sbi_scratch_thishart_ptr())
+
+#define fwft_set_hart_state_ptr(__scratch, __phs)			\
+	sbi_scratch_write_type((__scratch), void *, fwft_ptr_offset, (__phs))
+
+#define MIS_DELEG (1UL << CAUSE_MISALIGNED_LOAD | 1UL << CAUSE_MISALIGNED_STORE)
+
+struct fwft_config;
+
+struct fwft_feature {
+	enum sbi_fwft_feature_t id;
+	int (*supported)(struct fwft_config *conf);
+	int (*set)(struct fwft_config *conf, unsigned long value);
+	int (*get)(struct fwft_config *conf, unsigned long *value);
+};
+
+struct fwft_config {
+	const struct fwft_feature *feature;
+	unsigned long flags;
+};
+
+struct fwft_hart_state {
+	unsigned int config_count;
+	struct fwft_config configs[];
+};
+
+static const unsigned long fwft_defined_features[] = {
+	SBI_FWFT_MISALIGNED_EXC_DELEG,
+	SBI_FWFT_LANDING_PAD,
+	SBI_FWFT_SHADOW_STACK,
+	SBI_FWFT_DOUBLE_TRAP,
+	SBI_FWFT_PTE_AD_HW_UPDATING,
+};
+
+static bool fwft_is_defined_feature(enum sbi_fwft_feature_t feature)
+{
+	int i;
+
+	for (i = 0; i < array_size(fwft_defined_features); i++) {
+		if (fwft_defined_features[i] == feature)
+			return true;
+	}
+
+	return false;
+}
+
+static int fwft_misaligned_delegation_supported(struct fwft_config *conf)
+{
+	if (!misa_extension('S'))
+		return SBI_ENOTSUPP;
+
+	return SBI_OK;
+}
+
+static int fwft_set_misaligned_delegation(struct fwft_config *conf,
+					 unsigned long value)
+{
+	if (value == 1)
+		csr_set(CSR_MEDELEG, MIS_DELEG);
+	else if (value == 0)
+		csr_clear(CSR_MEDELEG, MIS_DELEG);
+	else
+		return SBI_EINVAL;
+
+	return SBI_OK;
+}
+
+static int fwft_get_misaligned_delegation(struct fwft_config *conf,
+					 unsigned long *value)
+{
+	*value = (csr_read(CSR_MEDELEG) & MIS_DELEG) != 0;
+
+	return SBI_OK;
+}
+
+static int fwft_adue_supported(struct fwft_config *conf)
+{
+	if (!sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
+				    SBI_HART_EXT_SVADU))
+		return SBI_ENOTSUPP;
+
+	return SBI_OK;
+}
+
+static int fwft_set_adue(struct fwft_config *conf, unsigned long value)
+{
+	if (value == 1)
+#if __riscv_xlen == 32
+		csr_set(CSR_MENVCFGH, ENVCFG_ADUE >> 32);
+#else
+		csr_set(CSR_MENVCFG, ENVCFG_ADUE);
+#endif
+	else if (value == 0)
+#if __riscv_xlen == 32
+		csr_clear(CSR_MENVCFGH, ENVCFG_ADUE >> 32);
+#else
+		csr_clear(CSR_MENVCFG, ENVCFG_ADUE);
+#endif
+	else
+		return SBI_EINVAL;
+
+	return SBI_OK;
+}
+
+static int fwft_get_adue(struct fwft_config *conf, unsigned long *value)
+{
+	unsigned long cfg;
+
+#if __riscv_xlen == 32
+	cfg = csr_read(CSR_MENVCFGH) & (ENVCFG_ADUE >> 32);
+#else
+	cfg = csr_read(CSR_MENVCFG) & ENVCFG_ADUE;
+#endif
+	*value = cfg != 0;
+
+	return SBI_OK;
+}
+
+static struct fwft_config* get_feature_config(enum sbi_fwft_feature_t feature)
+{
+	int i;
+	struct fwft_hart_state *fhs = fwft_thishart_state_ptr();
+
+	if (feature & SBI_FWFT_GLOBAL_FEATURE_BIT)
+		return NULL;
+
+	for (i = 0; i < fhs->config_count; i++){
+		if (feature == fhs->configs[i].feature->id)
+			return &fhs->configs[i];
+	}
+
+	return NULL;
+}
+
+static int fwft_get_feature(enum sbi_fwft_feature_t feature,
+			    struct fwft_config **conf)
+{
+	int ret;
+	struct fwft_config *tconf;
+
+	tconf = get_feature_config(feature);
+	if (!tconf) {
+		if (fwft_is_defined_feature(feature))
+			return SBI_ENOTSUPP;
+
+		return SBI_EDENIED;
+	}
+
+	if (tconf->feature->supported) {
+		ret = tconf->feature->supported(tconf);
+		if (ret)
+			return ret;
+	}
+	*conf = tconf;
+
+	return SBI_SUCCESS;
+}
+
+int sbi_fwft_set(enum sbi_fwft_feature_t feature, unsigned long value,
+		 unsigned long flags)
+{
+	int ret;
+	struct fwft_config *conf;
+
+	ret = fwft_get_feature(feature, &conf);
+	if (ret)
+		return ret;
+
+	if ((flags & ~SBI_FWFT_SET_FLAG_LOCK) != 0)
+		return SBI_EINVAL;
+
+	if (conf->flags & SBI_FWFT_SET_FLAG_LOCK)
+		return SBI_EDENIED;
+
+	ret = conf->feature->set(conf, value);
+	if (ret)
+		return ret;
+
+	conf->flags = flags;
+
+	return SBI_OK;
+}
+
+int sbi_fwft_get(enum sbi_fwft_feature_t feature, unsigned long *out_val)
+{
+	int ret;
+	struct fwft_config *conf;
+
+	ret = fwft_get_feature(feature, &conf);
+	if (ret)
+		return ret;
+
+	return conf->feature->get(conf, out_val);
+}
+
+static const struct fwft_feature features[] =
+{
+	{
+		.id = SBI_FWFT_MISALIGNED_EXC_DELEG,
+		.supported = fwft_misaligned_delegation_supported,
+		.set = fwft_set_misaligned_delegation,
+		.get = fwft_get_misaligned_delegation,
+	},
+	{
+		.id = SBI_FWFT_PTE_AD_HW_UPDATING,
+		.supported = fwft_adue_supported,
+		.set = fwft_set_adue,
+		.get = fwft_get_adue,
+	},
+};
+
+int sbi_fwft_init(struct sbi_scratch *scratch, bool cold_boot)
+{
+	int i;
+	struct fwft_hart_state *fhs;
+
+	if (cold_boot) {
+		fwft_ptr_offset = sbi_scratch_alloc_type_offset(void *);
+		if (!fwft_ptr_offset)
+			return SBI_ENOMEM;
+	}
+
+	fhs = fwft_get_hart_state_ptr(scratch);
+	if (!fhs) {
+		fhs = sbi_zalloc(sizeof(*fhs) + array_size(features) * sizeof(struct fwft_config));
+		if (!fhs)
+			return SBI_ENOMEM;
+
+		fhs->config_count = array_size(features);
+		for (i = 0; i < array_size(features); i++)
+			fhs->configs[i].feature = &features[i];
+
+		fwft_set_hart_state_ptr(scratch, fhs);
+	}
+
+	return 0;
+}

lib/sbi/sbi_hart.c

@@ -95,11 +95,16 @@ static void mstatus_init(struct sbi_scratch *scratch)
 		mstateen_val |= SMSTATEEN0_HSENVCFG;
 
 		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SMAIA))
-			mstateen_val |= (SMSTATEEN0_AIA | SMSTATEEN0_SVSLCT |
-					SMSTATEEN0_IMSIC);
+			mstateen_val |= (SMSTATEEN0_AIA | SMSTATEEN0_IMSIC);
 		else
-			mstateen_val &= ~(SMSTATEEN0_AIA | SMSTATEEN0_SVSLCT |
-					SMSTATEEN0_IMSIC);
+			mstateen_val &= ~(SMSTATEEN0_AIA | SMSTATEEN0_IMSIC);
+
+		if (sbi_hart_has_extension(scratch, SBI_HART_EXT_SMAIA) ||
+		    sbi_hart_has_extension(scratch, SBI_HART_EXT_SMCSRIND))
+			mstateen_val |= (SMSTATEEN0_SVSLCT);
+		else
+			mstateen_val &= ~(SMSTATEEN0_SVSLCT);
+
 		csr_write(CSR_MSTATEEN0, mstateen_val);
 #if __riscv_xlen == 32
 		csr_write(CSR_MSTATEEN0H, mstateen_val >> 32);
@@ -129,9 +134,20 @@ static void mstatus_init(struct sbi_scratch *scratch)
 		__set_menvcfg_ext(SBI_HART_EXT_SVPBMT, ENVCFG_PBMTE)
 #endif
 		__set_menvcfg_ext(SBI_HART_EXT_SSTC, ENVCFG_STCE)
+		__set_menvcfg_ext(SBI_HART_EXT_SMCDELEG, ENVCFG_CDE);
+		__set_menvcfg_ext(SBI_HART_EXT_SVADU, ENVCFG_ADUE);
 
 #undef __set_menvcfg_ext
 
+		/*
+		 * When both Svade and Svadu are present in DT, the default scheme for managing
+		 * the PTE A/D bits should use Svade. Check Svadu before Svade extension to ensure
+		 * that the ADUE bit is cleared when the Svade support are specified.
+		 */
+
+		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);
@@ -657,8 +673,18 @@ const struct sbi_hart_ext_data sbi_hart_ext[] = {
 	__SBI_HART_EXT_DATA(zicboz, SBI_HART_EXT_ZICBOZ),
 	__SBI_HART_EXT_DATA(zicbom, SBI_HART_EXT_ZICBOM),
 	__SBI_HART_EXT_DATA(svpbmt, SBI_HART_EXT_SVPBMT),
+	__SBI_HART_EXT_DATA(sdtrig, SBI_HART_EXT_SDTRIG),
+	__SBI_HART_EXT_DATA(smcsrind, SBI_HART_EXT_SMCSRIND),
+	__SBI_HART_EXT_DATA(smcdeleg, SBI_HART_EXT_SMCDELEG),
+	__SBI_HART_EXT_DATA(sscsrind, SBI_HART_EXT_SSCSRIND),
+	__SBI_HART_EXT_DATA(ssccfg, SBI_HART_EXT_SSCCFG),
+	__SBI_HART_EXT_DATA(svade, SBI_HART_EXT_SVADE),
+	__SBI_HART_EXT_DATA(svadu, SBI_HART_EXT_SVADU),
 };
 
+_Static_assert(SBI_HART_EXT_MAX == array_size(sbi_hart_ext),
+	       "sbi_hart_ext[]: wrong number of entries");
+
 /**
  * Get the hart extensions in string format
  *
@@ -898,6 +924,9 @@ __pmp_skip:
 	/* Detect if hart supports smcntrpmf */
 	__check_ext_csr(SBI_HART_PRIV_VER_1_12,
 			CSR_MCYCLECFG, SBI_HART_EXT_SMCNTRPMF);
+	/* Detect if hart support sdtrig (debug triggers) */
+	__check_ext_csr(SBI_HART_PRIV_VER_UNKNOWN,
+			CSR_TSELECT, SBI_HART_EXT_SDTRIG);
 
 #undef __check_ext_csr
 
@@ -1029,10 +1058,17 @@ sbi_hart_switch_mode(unsigned long arg0, unsigned long arg1,
 	csr_write(CSR_MEPC, next_addr);
 
 	if (next_mode == PRV_S) {
-		csr_write(CSR_STVEC, next_addr);
-		csr_write(CSR_SSCRATCH, 0);
-		csr_write(CSR_SIE, 0);
-		csr_write(CSR_SATP, 0);
+		if (next_virt) {
+			csr_write(CSR_VSTVEC, next_addr);
+			csr_write(CSR_VSSCRATCH, 0);
+			csr_write(CSR_VSIE, 0);
+			csr_write(CSR_VSATP, 0);
+		} else {
+			csr_write(CSR_STVEC, next_addr);
+			csr_write(CSR_SSCRATCH, 0);
+			csr_write(CSR_SIE, 0);
+			csr_write(CSR_SATP, 0);
+		}
 	} else if (next_mode == PRV_U) {
 		if (misa_extension('N')) {
 			csr_write(CSR_UTVEC, next_addr);

lib/sbi/sbi_hsm.c

@@ -44,6 +44,11 @@ struct sbi_hsm_data {
 	unsigned long suspend_type;
 	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
 	atomic_t start_ticket;
 };
 
@@ -360,6 +365,10 @@ int sbi_hsm_hart_start(struct sbi_scratch *scratch,
 
 	if (!rc)
 		return 0;
+
+	/* If it fails to start, change hart state back to stop */
+	__sbi_hsm_hart_change_state(hdata, SBI_HSM_STATE_START_PENDING,
+				    SBI_HSM_STATE_STOPPED);
 err:
 	hsm_start_ticket_release(hdata);
 	return rc;
@@ -413,6 +422,13 @@ 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);
+	}
 }
 
 static void __sbi_hsm_suspend_non_ret_restore(struct sbi_scratch *scratch)
@@ -420,6 +436,13 @@ 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
+	}
+	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)));
 }

lib/sbi/sbi_illegal_insn.c

@@ -25,7 +25,6 @@ static int truly_illegal_insn(ulong insn, struct sbi_trap_regs *regs)
 {
 	struct sbi_trap_info trap;
 
-	trap.epc = regs->mepc;
 	trap.cause = CAUSE_ILLEGAL_INSTRUCTION;
 	trap.tval = insn;
 	trap.tval2 = 0;
@@ -137,8 +136,10 @@ static const illegal_insn_func illegal_insn_table[32] = {
 	truly_illegal_insn  /* 31 */
 };
 
-int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs)
+int sbi_illegal_insn_handler(struct sbi_trap_context *tcntx)
 {
+	struct sbi_trap_regs *regs = &tcntx->regs;
+	ulong insn = tcntx->trap.tval;
 	struct sbi_trap_info uptrap;
 
 	/*
@@ -155,10 +156,8 @@ int sbi_illegal_insn_handler(ulong insn, struct sbi_trap_regs *regs)
 	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ILLEGAL_INSN);
 	if (unlikely((insn & 3) != 3)) {
 		insn = sbi_get_insn(regs->mepc, &uptrap);
-		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
+		if (uptrap.cause)
 			return sbi_trap_redirect(regs, &uptrap);
-		}
 		if ((insn & 3) != 3)
 			return truly_illegal_insn(insn, regs);
 	}

lib/sbi/sbi_init.c

@@ -10,11 +10,11 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_atomic.h>
 #include <sbi/riscv_barrier.h>
-#include <sbi/riscv_locks.h>
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_cppc.h>
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_ecall.h>
+#include <sbi/sbi_fwft.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_hartmask.h>
 #include <sbi/sbi_heap.h>
@@ -23,11 +23,14 @@
 #include <sbi/sbi_irqchip.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_pmu.h>
+#include <sbi/sbi_dbtr.h>
+#include <sbi/sbi_sse.h>
 #include <sbi/sbi_system.h>
 #include <sbi/sbi_string.h>
 #include <sbi/sbi_timer.h>
 #include <sbi/sbi_tlb.h>
 #include <sbi/sbi_version.h>
+#include <sbi/sbi_unit_test.h>
 
 #define BANNER                                              \
 	"   ____                    _____ ____ _____\n"     \
@@ -183,85 +186,24 @@ static void sbi_boot_print_hart(struct sbi_scratch *scratch, u32 hartid)
 	sbi_printf("Boot HART MHPM Info       : %lu (0x%08x)\n",
 		   sbi_popcount(sbi_hart_mhpm_mask(scratch)),
 		   sbi_hart_mhpm_mask(scratch));
+	sbi_printf("Boot HART Debug Triggers  : %d triggers\n",
+		   sbi_dbtr_get_total_triggers());
 	sbi_hart_delegation_dump(scratch, "Boot HART ", "         ");
 }
 
-static spinlock_t coldboot_lock = SPIN_LOCK_INITIALIZER;
-static struct sbi_hartmask coldboot_wait_hmask = { 0 };
-
 static unsigned long coldboot_done;
 
 static void wait_for_coldboot(struct sbi_scratch *scratch, u32 hartid)
 {
-	unsigned long saved_mie, cmip;
-
-	if (__smp_load_acquire(&coldboot_done))
-		return;
-
-	/* Save MIE CSR */
-	saved_mie = csr_read(CSR_MIE);
-
-	/* Set MSIE and MEIE bits to receive IPI */
-	csr_set(CSR_MIE, MIP_MSIP | MIP_MEIP);
-
-	/* Acquire coldboot lock */
-	spin_lock(&coldboot_lock);
-
-	/* Mark current HART as waiting */
-	sbi_hartmask_set_hartid(hartid, &coldboot_wait_hmask);
-
-	/* Release coldboot lock */
-	spin_unlock(&coldboot_lock);
-
-	/* Wait for coldboot to finish using WFI */
-	while (!__smp_load_acquire(&coldboot_done)) {
-		do {
-			wfi();
-			cmip = csr_read(CSR_MIP);
-		 } while (!(cmip & (MIP_MSIP | MIP_MEIP)));
-	}
-
-	/* Acquire coldboot lock */
-	spin_lock(&coldboot_lock);
-
-	/* Unmark current HART as waiting */
-	sbi_hartmask_clear_hartid(hartid, &coldboot_wait_hmask);
-
-	/* Release coldboot lock */
-	spin_unlock(&coldboot_lock);
-
-	/* Restore MIE CSR */
-	csr_write(CSR_MIE, saved_mie);
-
-	/*
-	 * The wait for coldboot is common for both warm startup and
-	 * warm resume path so clearing IPI here would result in losing
-	 * an IPI in warm resume path.
-	 *
-	 * Also, the sbi_platform_ipi_init() called from sbi_ipi_init()
-	 * will automatically clear IPI for current HART.
-	 */
+	/* Wait for coldboot to finish */
+	while (!__smp_load_acquire(&coldboot_done))
+		cpu_relax();
 }
 
 static void wake_coldboot_harts(struct sbi_scratch *scratch, u32 hartid)
 {
-	u32 i, hartindex = sbi_hartid_to_hartindex(hartid);
-
 	/* Mark coldboot done */
 	__smp_store_release(&coldboot_done, 1);
-
-	/* Acquire coldboot lock */
-	spin_lock(&coldboot_lock);
-
-	/* Send an IPI to all HARTs waiting for coldboot */
-	sbi_hartmask_for_each_hartindex(i, &coldboot_wait_hmask) {
-		if (i == hartindex)
-			continue;
-		sbi_ipi_raw_send(i);
-	}
-
-	/* Release coldboot lock */
-	spin_unlock(&coldboot_lock);
 }
 
 static unsigned long entry_count_offset;
@@ -303,6 +245,14 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	if (rc)
 		sbi_hart_hang();
 
+	/*
+	 * All non-coldboot HARTs do HSM initialization (i.e. enter HSM state
+	 * machine) at the start of the warmboot path so it is wasteful to
+	 * have these HARTs busy spin in wait_for_coldboot() until coldboot
+	 * path is completed.
+	 */
+	wake_coldboot_harts(scratch, hartid);
+
 	rc = sbi_platform_early_init(plat, true);
 	if (rc)
 		sbi_hart_hang();
@@ -315,6 +265,12 @@ 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",
@@ -322,6 +278,10 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 		sbi_hart_hang();
 	}
 
+	rc = sbi_dbtr_init(scratch, true);
+	if (rc)
+		sbi_hart_hang();
+
 	sbi_boot_print_banner(scratch);
 
 	rc = sbi_irqchip_init(scratch, true);
@@ -349,6 +309,12 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 		sbi_hart_hang();
 	}
 
+	rc = sbi_fwft_init(scratch, true);
+	if (rc) {
+		sbi_printf("%s: fwft init failed (error %d)\n", __func__, rc);
+		sbi_hart_hang();
+	}
+
 	/*
 	 * Note: Finalize domains after HSM initialization so that we
 	 * can startup non-root domains.
@@ -391,6 +357,8 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 
 	sbi_boot_print_hart(scratch, hartid);
 
+	run_all_tests();
+
 	/*
 	 * Configure PMP at last because if SMEPMP is detected,
 	 * M-mode access to the S/U space will be rescinded.
@@ -402,8 +370,6 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 		sbi_hart_hang();
 	}
 
-	wake_coldboot_harts(scratch, hartid);
-
 	count = sbi_scratch_offset_ptr(scratch, init_count_offset);
 	(*count)++;
 
@@ -423,6 +389,7 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	count = sbi_scratch_offset_ptr(scratch, entry_count_offset);
 	(*count)++;
 
+	/* Note: This has to be first thing in warmboot init sequence */
 	rc = sbi_hsm_init(scratch, hartid, false);
 	if (rc)
 		sbi_hart_hang();
@@ -435,10 +402,18 @@ 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();
 
+	rc = sbi_dbtr_init(scratch, false);
+	if (rc)
+		sbi_hart_hang();
+
 	rc = sbi_irqchip_init(scratch, false);
 	if (rc)
 		sbi_hart_hang();
@@ -455,6 +430,10 @@ static void __noreturn init_warm_startup(struct sbi_scratch *scratch,
 	if (rc)
 		sbi_hart_hang();
 
+	rc = sbi_fwft_init(scratch, false);
+	if (rc)
+		sbi_hart_hang();
+
 	rc = sbi_platform_final_init(plat, false);
 	if (rc)
 		sbi_hart_hang();
@@ -639,6 +618,8 @@ void __noreturn sbi_exit(struct sbi_scratch *scratch)
 
 	sbi_platform_early_exit(plat);
 
+	sbi_sse_exit(scratch);
+
 	sbi_pmu_exit(scratch);
 
 	sbi_timer_exit(scratch);

lib/sbi/sbi_irqchip.c

@@ -10,22 +10,22 @@
 #include <sbi/sbi_irqchip.h>
 #include <sbi/sbi_platform.h>
 
-static int default_irqfn(struct sbi_trap_regs *regs)
+static int default_irqfn(void)
 {
 	return SBI_ENODEV;
 }
 
-static int (*ext_irqfn)(struct sbi_trap_regs *regs) = default_irqfn;
+static int (*ext_irqfn)(void) = default_irqfn;
 
-void sbi_irqchip_set_irqfn(int (*fn)(struct sbi_trap_regs *regs))
+void sbi_irqchip_set_irqfn(int (*fn)(void))
 {
 	if (fn)
 		ext_irqfn = fn;
 }
 
-int sbi_irqchip_process(struct sbi_trap_regs *regs)
+int sbi_irqchip_process(void)
 {
-	return ext_irqfn(regs);
+	return ext_irqfn();
 }
 
 int sbi_irqchip_init(struct sbi_scratch *scratch, bool cold_boot)

lib/sbi/sbi_pmu.c

@@ -17,6 +17,7 @@
 #include <sbi/sbi_pmu.h>
 #include <sbi/sbi_scratch.h>
 #include <sbi/sbi_string.h>
+#include <sbi/sbi_sse.h>
 
 /** Information about hardware counters */
 struct sbi_pmu_hw_event {
@@ -62,6 +63,8 @@ struct sbi_pmu_hart_state {
 	uint32_t active_events[SBI_PMU_HW_CTR_MAX + SBI_PMU_FW_CTR_MAX];
 	/* Bitmap of firmware counters started */
 	unsigned long fw_counters_started;
+	/* if true, SSE is enabled */
+	bool sse_enabled;
 	/*
 	 * Counter values for SBI firmware events and event codes
 	 * for platform firmware events. Both are mutually exclusive
@@ -74,7 +77,7 @@ struct sbi_pmu_hart_state {
 static unsigned long phs_ptr_offset;
 
 #define pmu_get_hart_state_ptr(__scratch)				\
-	sbi_scratch_read_type((__scratch), void *, phs_ptr_offset)
+	phs_ptr_offset ? sbi_scratch_read_type((__scratch), void *, phs_ptr_offset) : NULL
 
 #define pmu_thishart_state_ptr()					\
 	pmu_get_hart_state_ptr(sbi_scratch_thishart_ptr())
@@ -207,6 +210,9 @@ int sbi_pmu_ctr_fw_read(uint32_t cidx, uint64_t *cval)
 	uint32_t event_code;
 	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
 
+	if (unlikely(!phs))
+		return SBI_EINVAL;
+
 	event_idx_type = pmu_ctr_validate(phs, cidx, &event_code);
 	if (event_idx_type != SBI_PMU_EVENT_TYPE_FW)
 		return SBI_EINVAL;
@@ -297,6 +303,16 @@ int sbi_pmu_add_raw_event_counter_map(uint64_t select, uint64_t select_mask, u32
 				    SBI_PMU_EVENT_RAW_IDX, cmap, select, select_mask);
 }
 
+void sbi_pmu_ovf_irq()
+{
+	/*
+	 * We need to disable LCOFIP before returning to S-mode or we will loop
+	 * on LCOFIP being triggered
+	 */
+	csr_clear(CSR_MIE, MIP_LCOFIP);
+	sbi_sse_inject_event(SBI_SSE_EVENT_LOCAL_PMU);
+}
+
 static int pmu_ctr_enable_irq_hw(int ctr_idx)
 {
 	unsigned long mhpmevent_csr;
@@ -432,6 +448,10 @@ int sbi_pmu_ctr_start(unsigned long cbase, unsigned long cmask,
 		      unsigned long flags, uint64_t ival)
 {
 	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	if (unlikely(!phs))
+		return SBI_EINVAL;
+
 	int event_idx_type;
 	uint32_t event_code;
 	int ret = SBI_EINVAL;
@@ -535,6 +555,10 @@ int sbi_pmu_ctr_stop(unsigned long cbase, unsigned long cmask,
 		     unsigned long flag)
 {
 	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	if (unlikely(!phs))
+		return SBI_EINVAL;
+
 	int ret = SBI_EINVAL;
 	int event_idx_type;
 	uint32_t event_code;
@@ -564,6 +588,10 @@ int sbi_pmu_ctr_stop(unsigned long cbase, unsigned long cmask,
 		}
 	}
 
+	/* Clear MIP_LCOFIP to avoid spurious interrupts */
+	if (phs->sse_enabled)
+		csr_clear(CSR_MIP, MIP_LCOFIP);
+
 	return ret;
 }
 
@@ -794,6 +822,10 @@ int sbi_pmu_ctr_cfg_match(unsigned long cidx_base, unsigned long cidx_mask,
 			  uint64_t event_data)
 {
 	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	if (unlikely(!phs))
+		return SBI_EINVAL;
+
 	int ret, event_type, ctr_idx = SBI_ENOTSUPP;
 	u32 event_code;
 
@@ -869,6 +901,9 @@ int sbi_pmu_ctr_incr_fw(enum sbi_pmu_fw_event_code_id fw_id)
 	uint64_t *fcounter = NULL;
 	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
 
+	if (unlikely(!phs))
+		return 0;
+
 	if (likely(!phs->fw_counters_started))
 		return 0;
 
@@ -944,6 +979,7 @@ static void pmu_reset_event_map(struct sbi_pmu_hart_state *phs)
 	for (j = 0; j < SBI_PMU_FW_CTR_MAX; j++)
 		phs->fw_counters_data[j] = 0;
 	phs->fw_counters_started = 0;
+	phs->sse_enabled = 0;
 }
 
 const struct sbi_pmu_device *sbi_pmu_get_device(void)
@@ -961,15 +997,51 @@ void sbi_pmu_set_device(const struct sbi_pmu_device *dev)
 
 void sbi_pmu_exit(struct sbi_scratch *scratch)
 {
+	struct sbi_pmu_hart_state *phs = pmu_get_hart_state_ptr(scratch);
+
 	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_11)
 		csr_write(CSR_MCOUNTINHIBIT, 0xFFFFFFF8);
 
 	if (sbi_hart_priv_version(scratch) >= SBI_HART_PRIV_VER_1_10)
 		csr_write(CSR_MCOUNTEREN, -1);
 
-	pmu_reset_event_map(pmu_get_hart_state_ptr(scratch));
+	if (unlikely(!phs))
+		return;
+
+	pmu_reset_event_map(phs);
 }
 
+static void pmu_sse_enable(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_bit());
+	csr_clear(CSR_MIP, MIP_LCOFIP);
+	csr_set(CSR_MIE, MIP_LCOFIP);
+}
+
+static void pmu_sse_disable(uint32_t event_id)
+{
+	struct sbi_pmu_hart_state *phs = pmu_thishart_state_ptr();
+
+	csr_clear(CSR_MIE, MIP_LCOFIP);
+	csr_clear(CSR_MIP, MIP_LCOFIP);
+	csr_set(CSR_MIDELEG, sbi_pmu_irq_bit());
+	phs->sse_enabled = false;
+}
+
+static void pmu_sse_complete(uint32_t event_id)
+{
+	csr_set(CSR_MIE, MIP_LCOFIP);
+}
+
+static const struct sbi_sse_cb_ops pmu_sse_cb_ops = {
+	.enable_cb = pmu_sse_enable,
+	.disable_cb = pmu_sse_disable,
+	.complete_cb = pmu_sse_complete,
+};
+
 int sbi_pmu_init(struct sbi_scratch *scratch, bool cold_boot)
 {
 	int hpm_count = sbi_fls(sbi_hart_mhpm_mask(scratch));
@@ -1009,6 +1081,8 @@ int sbi_pmu_init(struct sbi_scratch *scratch, bool cold_boot)
 		total_ctrs = num_hw_ctrs + SBI_PMU_FW_CTR_MAX;
 	}
 
+	sbi_sse_set_cb_ops(SBI_SSE_EVENT_LOCAL_PMU, &pmu_sse_cb_ops);
+
 	phs = pmu_get_hart_state_ptr(scratch);
 	if (!phs) {
 		phs = sbi_zalloc(sizeof(*phs));

lib/sbi/sbi_system.c

@@ -148,7 +148,7 @@ bool sbi_system_suspend_supported(u32 sleep_type)
 
 int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 {
-	const struct sbi_domain *dom = sbi_domain_thishart_ptr();
+	struct sbi_domain *dom = sbi_domain_thishart_ptr();
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	void (*jump_warmboot)(void) = (void (*)(void))scratch->warmboot_addr;
 	unsigned int hartid = current_hartid();
@@ -171,13 +171,17 @@ int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 	if (prev_mode != PRV_S && prev_mode != PRV_U)
 		return SBI_EFAIL;
 
+	spin_lock(&dom->assigned_harts_lock);
 	sbi_hartmask_for_each_hartindex(j, &dom->assigned_harts) {
 		i = sbi_hartindex_to_hartid(j);
 		if (i == hartid)
 			continue;
-		if (__sbi_hsm_hart_get_state(i) != SBI_HSM_STATE_STOPPED)
+		if (__sbi_hsm_hart_get_state(i) != SBI_HSM_STATE_STOPPED) {
+			spin_unlock(&dom->assigned_harts_lock);
 			return SBI_ERR_DENIED;
+		}
 	}
+	spin_unlock(&dom->assigned_harts_lock);
 
 	if (!sbi_domain_check_addr(dom, resume_addr, prev_mode,
 				   SBI_DOMAIN_EXECUTE))

lib/sbi/sbi_trap.c

@@ -17,60 +17,76 @@
 #include <sbi/sbi_illegal_insn.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_irqchip.h>
-#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_trap_ldst.h>
 #include <sbi/sbi_pmu.h>
 #include <sbi/sbi_scratch.h>
+#include <sbi/sbi_sse.h>
 #include <sbi/sbi_timer.h>
 #include <sbi/sbi_trap.h>
 
-static void __noreturn sbi_trap_error(const char *msg, int rc,
-				      ulong mcause, ulong mtval, ulong mtval2,
-				      ulong mtinst, struct sbi_trap_regs *regs)
+static void sbi_trap_error_one(const struct sbi_trap_context *tcntx,
+			       const char *prefix, u32 hartid, u32 depth)
 {
-	u32 hartid = current_hartid();
+	const struct sbi_trap_info *trap = &tcntx->trap;
+	const struct sbi_trap_regs *regs = &tcntx->regs;
 
-	sbi_printf("%s: hart%d: %s (error %d)\n", __func__, hartid, msg, rc);
-	sbi_printf("%s: hart%d: mcause=0x%" PRILX " mtval=0x%" PRILX "\n",
-		   __func__, hartid, mcause, mtval);
+	sbi_printf("\n");
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "mcause", trap->cause, "mtval", trap->tval);
 	if (misa_extension('H')) {
-		sbi_printf("%s: hart%d: mtval2=0x%" PRILX
-			   " mtinst=0x%" PRILX "\n",
-			   __func__, hartid, mtval2, mtinst);
+		sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+			   hartid, depth, "mtval2", trap->tval2, "mtinst", trap->tinst);
 	}
-	sbi_printf("%s: hart%d: mepc=0x%" PRILX " mstatus=0x%" PRILX "\n",
-		   __func__, hartid, regs->mepc, regs->mstatus);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "ra", regs->ra, "sp", regs->sp);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "gp", regs->gp, "tp", regs->tp);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s0", regs->s0, "s1", regs->s1);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "a0", regs->a0, "a1", regs->a1);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "a2", regs->a2, "a3", regs->a3);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "a4", regs->a4, "a5", regs->a5);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "a6", regs->a6, "a7", regs->a7);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s2", regs->s2, "s3", regs->s3);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s4", regs->s4, "s5", regs->s5);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s6", regs->s6, "s7", regs->s7);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s8", regs->s8, "s9", regs->s9);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "s10", regs->s10, "s11", regs->s11);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "t0", regs->t0, "t1", regs->t1);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "t2", regs->t2, "t3", regs->t3);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", __func__,
-		   hartid, "t4", regs->t4, "t5", regs->t5);
-	sbi_printf("%s: hart%d: %s=0x%" PRILX "\n", __func__, hartid, "t6",
-		   regs->t6);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "mepc", regs->mepc, "mstatus", regs->mstatus);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "ra", regs->ra, "sp", regs->sp);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "gp", regs->gp, "tp", regs->tp);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s0", regs->s0, "s1", regs->s1);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "a0", regs->a0, "a1", regs->a1);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "a2", regs->a2, "a3", regs->a3);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "a4", regs->a4, "a5", regs->a5);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "a6", regs->a6, "a7", regs->a7);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s2", regs->s2, "s3", regs->s3);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s4", regs->s4, "s5", regs->s5);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s6", regs->s6, "s7", regs->s7);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s8", regs->s8, "s9", regs->s9);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "s10", regs->s10, "s11", regs->s11);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "t0", regs->t0, "t1", regs->t1);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "t2", regs->t2, "t3", regs->t3);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX " %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "t4", regs->t4, "t5", regs->t5);
+	sbi_printf("%s: hart%d: trap%d: %s=0x%" PRILX "\n", prefix,
+		   hartid, depth, "t6", regs->t6);
+}
+
+static void __noreturn sbi_trap_error(const char *msg, int rc,
+				      const struct sbi_trap_context *tcntx)
+{
+	u32 depth = 0, hartid = current_hartid();
+	const struct sbi_trap_context *tc;
+
+	for (tc = tcntx; tc; tc = tc->prev_context)
+		depth++;
+
+	sbi_printf("\n");
+	sbi_printf("%s: hart%d: trap%d: %s (error %d)\n", __func__,
+		   hartid, depth - 1, msg, rc);
+	for (tc = tcntx; tc; tc = tc->prev_context)
+		sbi_trap_error_one(tc, __func__, hartid, --depth);
 
 	sbi_hart_hang();
 }
@@ -84,7 +100,7 @@ static void __noreturn sbi_trap_error(const char *msg, int rc,
  * @return 0 on success and negative error code on failure
  */
 int sbi_trap_redirect(struct sbi_trap_regs *regs,
-		      struct sbi_trap_info *trap)
+		      const struct sbi_trap_info *trap)
 {
 	ulong hstatus, vsstatus, prev_mode;
 #if __riscv_xlen == 32
@@ -140,7 +156,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 	if (next_virt) {
 		/* Update VS-mode exception info */
 		csr_write(CSR_VSTVAL, trap->tval);
-		csr_write(CSR_VSEPC, trap->epc);
+		csr_write(CSR_VSEPC, regs->mepc);
 		csr_write(CSR_VSCAUSE, trap->cause);
 
 		/* Set MEPC to VS-mode exception vector base */
@@ -171,7 +187,7 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 	} else {
 		/* Update S-mode exception info */
 		csr_write(CSR_STVAL, trap->tval);
-		csr_write(CSR_SEPC, trap->epc);
+		csr_write(CSR_SEPC, regs->mepc);
 		csr_write(CSR_SCAUSE, trap->cause);
 
 		/* Set MEPC to S-mode exception vector base */
@@ -198,18 +214,20 @@ int sbi_trap_redirect(struct sbi_trap_regs *regs,
 	return 0;
 }
 
-static int sbi_trap_nonaia_irq(struct sbi_trap_regs *regs, ulong mcause)
+static int sbi_trap_nonaia_irq(unsigned long irq)
 {
-	mcause &= ~(1UL << (__riscv_xlen - 1));
-	switch (mcause) {
+	switch (irq) {
 	case IRQ_M_TIMER:
 		sbi_timer_process();
 		break;
 	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(regs);
+		return sbi_irqchip_process();
 	default:
 		return SBI_ENOENT;
 	}
@@ -217,7 +235,7 @@ static int sbi_trap_nonaia_irq(struct sbi_trap_regs *regs, ulong mcause)
 	return 0;
 }
 
-static int sbi_trap_aia_irq(struct sbi_trap_regs *regs, ulong mcause)
+static int sbi_trap_aia_irq(void)
 {
 	int rc;
 	unsigned long mtopi;
@@ -231,8 +249,11 @@ static int sbi_trap_aia_irq(struct sbi_trap_regs *regs, ulong mcause)
 		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(regs);
+			rc = sbi_irqchip_process();
 			if (rc)
 				return rc;
 			break;
@@ -258,91 +279,75 @@ static int sbi_trap_aia_irq(struct sbi_trap_regs *regs, ulong mcause)
  * 6. Stack pointer (SP) is setup for current HART
  * 7. Interrupts are disabled in MSTATUS CSR
  *
- * @param regs pointer to register state
+ * @param tcntx pointer to trap context
  */
-struct sbi_trap_regs *sbi_trap_handler(struct sbi_trap_regs *regs)
+struct sbi_trap_context *sbi_trap_handler(struct sbi_trap_context *tcntx)
 {
 	int rc = SBI_ENOTSUPP;
 	const char *msg = "trap handler failed";
-	ulong mcause = csr_read(CSR_MCAUSE);
-	ulong mtval = csr_read(CSR_MTVAL), mtval2 = 0, mtinst = 0;
-	struct sbi_trap_info trap;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
+	const struct sbi_trap_info *trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
+	ulong mcause = tcntx->trap.cause;
 
-	if (misa_extension('H')) {
-		mtval2 = csr_read(CSR_MTVAL2);
-		mtinst = csr_read(CSR_MTINST);
-	}
+	/* Update trap context pointer */
+	tcntx->prev_context = sbi_trap_get_context(scratch);
+	sbi_trap_set_context(scratch, tcntx);
 
-	if (mcause & (1UL << (__riscv_xlen - 1))) {
+	if (mcause & MCAUSE_IRQ_MASK) {
 		if (sbi_hart_has_extension(sbi_scratch_thishart_ptr(),
 					   SBI_HART_EXT_SMAIA))
-			rc = sbi_trap_aia_irq(regs, mcause);
+			rc = sbi_trap_aia_irq();
 		else
-			rc = sbi_trap_nonaia_irq(regs, mcause);
-		if (rc) {
-			msg = "unhandled local interrupt";
-			goto trap_error;
-		}
-		return regs;
+			rc = sbi_trap_nonaia_irq(mcause & ~MCAUSE_IRQ_MASK);
+		msg = "unhandled local interrupt";
+		goto trap_done;
 	}
 
 	switch (mcause) {
 	case CAUSE_ILLEGAL_INSTRUCTION:
-		rc  = sbi_illegal_insn_handler(mtval, regs);
+		rc  = sbi_illegal_insn_handler(tcntx);
 		msg = "illegal instruction handler failed";
 		break;
 	case CAUSE_MISALIGNED_LOAD:
-		rc = sbi_misaligned_load_handler(mtval, mtval2, mtinst, regs);
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_LOAD);
+		rc  = sbi_misaligned_load_handler(tcntx);
 		msg = "misaligned load handler failed";
 		break;
 	case CAUSE_MISALIGNED_STORE:
-		rc  = sbi_misaligned_store_handler(mtval, mtval2, mtinst, regs);
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_STORE);
+		rc  = sbi_misaligned_store_handler(tcntx);
 		msg = "misaligned store handler failed";
 		break;
 	case CAUSE_SUPERVISOR_ECALL:
 	case CAUSE_MACHINE_ECALL:
-		rc  = sbi_ecall_handler(regs);
+		rc  = sbi_ecall_handler(tcntx);
 		msg = "ecall handler failed";
 		break;
 	case CAUSE_LOAD_ACCESS:
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ACCESS_LOAD);
+		rc  = sbi_load_access_handler(tcntx);
+		msg = "load fault handler failed";
+		break;
 	case CAUSE_STORE_ACCESS:
-		sbi_pmu_ctr_incr_fw(mcause == CAUSE_LOAD_ACCESS ?
-			SBI_PMU_FW_ACCESS_LOAD : SBI_PMU_FW_ACCESS_STORE);
-		/* fallthrough */
+		sbi_pmu_ctr_incr_fw(SBI_PMU_FW_ACCESS_STORE);
+		rc  = sbi_store_access_handler(tcntx);
+		msg = "store fault handler failed";
+		break;
 	default:
 		/* If the trap came from S or U mode, redirect it there */
-		trap.epc = regs->mepc;
-		trap.cause = mcause;
-		trap.tval = mtval;
-		trap.tval2 = mtval2;
-		trap.tinst = mtinst;
-		trap.gva   = sbi_regs_gva(regs);
-
-		rc = sbi_trap_redirect(regs, &trap);
+		msg = "trap redirect failed";
+		rc  = sbi_trap_redirect(regs, trap);
 		break;
 	}
 
-trap_error:
+trap_done:
 	if (rc)
-		sbi_trap_error(msg, rc, mcause, mtval, mtval2, mtinst, regs);
-	return regs;
-}
-
-typedef void (*trap_exit_t)(const struct sbi_trap_regs *regs);
-
-/**
- * Exit trap/interrupt handling
- *
- * This function is called by non-firmware code to abruptly exit
- * trap/interrupt handling and resume execution at context pointed
- * by given register state.
- *
- * @param regs pointer to register state
- */
-void __noreturn sbi_trap_exit(const struct sbi_trap_regs *regs)
-{
-	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
-
-	((trap_exit_t)scratch->trap_exit)(regs);
-	__builtin_unreachable();
+		sbi_trap_error(msg, rc, tcntx);
+
+	if (((regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT) != PRV_M)
+		sbi_sse_process_pending_events(regs);
+
+	sbi_trap_set_context(scratch, tcntx->prev_context);
+	return tcntx;
 }

lib/sbi/sbi_trap_ldst.c

@@ -11,16 +11,26 @@
 #include <sbi/riscv_encoding.h>
 #include <sbi/riscv_fp.h>
 #include <sbi/sbi_error.h>
-#include <sbi/sbi_misaligned_ldst.h>
-#include <sbi/sbi_pmu.h>
+#include <sbi/sbi_trap_ldst.h>
 #include <sbi/sbi_trap.h>
 #include <sbi/sbi_unpriv.h>
+#include <sbi/sbi_platform.h>
 
-union reg_data {
-	u8 data_bytes[8];
-	ulong data_ulong;
-	u64 data_u64;
-};
+/**
+ * Load emulator callback:
+ *
+ * @return rlen=success, 0=success w/o regs modification, or negative error
+ */
+typedef int (*sbi_trap_ld_emulator)(int rlen, union sbi_ldst_data *out_val,
+				    struct sbi_trap_context *tcntx);
+
+/**
+ * Store emulator callback:
+ *
+ * @return wlen=success, 0=success w/o regs modification, or negative error
+ */
+typedef int (*sbi_trap_st_emulator)(int wlen, union sbi_ldst_data in_val,
+				    struct sbi_trap_context *tcntx);
 
 static ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
 					ulong addr_offset)
@@ -34,23 +44,23 @@ static ulong sbi_misaligned_tinst_fixup(ulong orig_tinst, ulong new_tinst,
 		return orig_tinst | (addr_offset << SH_RS1);
 }
 
-int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
-				struct sbi_trap_regs *regs)
+static int sbi_trap_emulate_load(struct sbi_trap_context *tcntx,
+				 sbi_trap_ld_emulator emu)
 {
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
 	ulong insn, insn_len;
-	union reg_data val;
+	union sbi_ldst_data val = { 0 };
 	struct sbi_trap_info uptrap;
-	int i, fp = 0, shift = 0, len = 0;
+	int rc, fp = 0, shift = 0, len = 0;
 
-	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_LOAD);
-
-	if (tinst & 0x1) {
+	if (orig_trap->tinst & 0x1) {
 		/*
 		 * Bit[0] == 1 implies trapped instruction value is
 		 * transformed instruction or custom instruction.
 		 */
-		insn = tinst | INSN_16BIT_MASK;
-		insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+		insn	 = orig_trap->tinst | INSN_16BIT_MASK;
+		insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
 	} else {
 		/*
 		 * Bit[0] == 0 implies trapped instruction value is
@@ -58,13 +68,17 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
 		 */
 		insn = sbi_get_insn(regs->mepc, &uptrap);
 		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
 			return sbi_trap_redirect(regs, &uptrap);
 		}
 		insn_len = INSN_LEN(insn);
 	}
 
-	if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
+	if ((insn & INSN_MASK_LB) == INSN_MATCH_LB) {
+		len   = 1;
+		shift = 8 * (sizeof(ulong) - len);
+	} else if ((insn & INSN_MASK_LBU) == INSN_MATCH_LBU) {
+		len = 1;
+	} else if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
 		len   = 4;
 		shift = 8 * (sizeof(ulong) - len);
 #if __riscv_xlen == 64
@@ -123,27 +137,20 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
 		len = 4;
 #endif
 #endif
+	} else if ((insn & INSN_MASK_C_LHU) == INSN_MATCH_C_LHU) {
+		len = 2;
+		insn = RVC_RS2S(insn) << SH_RD;
+	} else if ((insn & INSN_MASK_C_LH) == INSN_MATCH_C_LH) {
+		len = 2;
+		shift = 8 * (sizeof(ulong) - len);
+		insn = RVC_RS2S(insn) << SH_RD;
 	} else {
-		uptrap.epc = regs->mepc;
-		uptrap.cause = CAUSE_MISALIGNED_LOAD;
-		uptrap.tval = addr;
-		uptrap.tval2 = tval2;
-		uptrap.tinst = tinst;
-		uptrap.gva   = sbi_regs_gva(regs);
-		return sbi_trap_redirect(regs, &uptrap);
+		return sbi_trap_redirect(regs, orig_trap);
 	}
 
-	val.data_u64 = 0;
-	for (i = 0; i < len; i++) {
-		val.data_bytes[i] = sbi_load_u8((void *)(addr + i),
-						&uptrap);
-		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				tinst, uptrap.tinst, i);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
-	}
+	rc = emu(len, &val, tcntx);
+	if (rc <= 0)
+		return rc;
 
 	if (!fp)
 		SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
@@ -159,23 +166,23 @@ int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
 	return 0;
 }
 
-int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
-				 struct sbi_trap_regs *regs)
+static int sbi_trap_emulate_store(struct sbi_trap_context *tcntx,
+				  sbi_trap_st_emulator emu)
 {
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
 	ulong insn, insn_len;
-	union reg_data val;
+	union sbi_ldst_data val;
 	struct sbi_trap_info uptrap;
-	int i, len = 0;
+	int rc, len = 0;
 
-	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_MISALIGNED_STORE);
-
-	if (tinst & 0x1) {
+	if (orig_trap->tinst & 0x1) {
 		/*
 		 * Bit[0] == 1 implies trapped instruction value is
 		 * transformed instruction or custom instruction.
 		 */
-		insn = tinst | INSN_16BIT_MASK;
-		insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+		insn	 = orig_trap->tinst | INSN_16BIT_MASK;
+		insn_len = (orig_trap->tinst & 0x2) ? INSN_LEN(insn) : 2;
 	} else {
 		/*
 		 * Bit[0] == 0 implies trapped instruction value is
@@ -183,7 +190,6 @@ int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
 		 */
 		insn = sbi_get_insn(regs->mepc, &uptrap);
 		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
 			return sbi_trap_redirect(regs, &uptrap);
 		}
 		insn_len = INSN_LEN(insn);
@@ -191,7 +197,9 @@ int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
 
 	val.data_ulong = GET_RS2(insn, regs);
 
-	if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
+	if ((insn & INSN_MASK_SB) == INSN_MATCH_SB) {
+		len = 1;
+	} else if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
 		len = 4;
 #if __riscv_xlen == 64
 	} else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
@@ -237,28 +245,114 @@ int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
 		val.data_ulong = GET_F32_RS2C(insn, regs);
 #endif
 #endif
+	} else if ((insn & INSN_MASK_C_SH) == INSN_MATCH_C_SH) {
+		len		= 2;
+		val.data_ulong = GET_RS2S(insn, regs);
 	} else {
-		uptrap.epc = regs->mepc;
-		uptrap.cause = CAUSE_MISALIGNED_STORE;
-		uptrap.tval = addr;
-		uptrap.tval2 = tval2;
-		uptrap.tinst = tinst;
-		uptrap.gva   = sbi_regs_gva(regs);
-		return sbi_trap_redirect(regs, &uptrap);
+		return sbi_trap_redirect(regs, orig_trap);
 	}
 
-	for (i = 0; i < len; i++) {
-		sbi_store_u8((void *)(addr + i), val.data_bytes[i],
-			     &uptrap);
-		if (uptrap.cause) {
-			uptrap.epc = regs->mepc;
-			uptrap.tinst = sbi_misaligned_tinst_fixup(
-				tinst, uptrap.tinst, i);
-			return sbi_trap_redirect(regs, &uptrap);
-		}
-	}
+	rc = emu(len, val, tcntx);
+	if (rc <= 0)
+		return rc;
 
 	regs->mepc += insn_len;
 
 	return 0;
 }
+
+static int sbi_misaligned_ld_emulator(int rlen, union sbi_ldst_data *out_val,
+				      struct sbi_trap_context *tcntx)
+{
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
+	struct sbi_trap_info uptrap;
+	int i;
+
+	for (i = 0; i < rlen; i++) {
+		out_val->data_bytes[i] =
+			sbi_load_u8((void *)(orig_trap->tval + i), &uptrap);
+		if (uptrap.cause) {
+			uptrap.tinst = sbi_misaligned_tinst_fixup(
+				orig_trap->tinst, uptrap.tinst, i);
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+	return rlen;
+}
+
+int sbi_misaligned_load_handler(struct sbi_trap_context *tcntx)
+{
+	return sbi_trap_emulate_load(tcntx, sbi_misaligned_ld_emulator);
+}
+
+static int sbi_misaligned_st_emulator(int wlen, union sbi_ldst_data in_val,
+				      struct sbi_trap_context *tcntx)
+{
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
+	struct sbi_trap_info uptrap;
+	int i;
+
+	for (i = 0; i < wlen; i++) {
+		sbi_store_u8((void *)(orig_trap->tval + i),
+			     in_val.data_bytes[i], &uptrap);
+		if (uptrap.cause) {
+			uptrap.tinst = sbi_misaligned_tinst_fixup(
+				orig_trap->tinst, uptrap.tinst, i);
+			return sbi_trap_redirect(regs, &uptrap);
+		}
+	}
+	return wlen;
+}
+
+int sbi_misaligned_store_handler(struct sbi_trap_context *tcntx)
+{
+	return sbi_trap_emulate_store(tcntx, sbi_misaligned_st_emulator);
+}
+
+static int sbi_ld_access_emulator(int rlen, union sbi_ldst_data *out_val,
+				  struct sbi_trap_context *tcntx)
+{
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
+
+	/* If fault came from M mode, just fail */
+	if (((regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT) == PRV_M)
+		return SBI_EINVAL;
+
+	/* If platform emulator failed, we redirect instead of fail */
+	if (sbi_platform_emulate_load(sbi_platform_thishart_ptr(), rlen,
+				      orig_trap->tval, out_val))
+		return sbi_trap_redirect(regs, orig_trap);
+
+	return rlen;
+}
+
+int sbi_load_access_handler(struct sbi_trap_context *tcntx)
+{
+	return sbi_trap_emulate_load(tcntx, sbi_ld_access_emulator);
+}
+
+static int sbi_st_access_emulator(int wlen, union sbi_ldst_data in_val,
+				  struct sbi_trap_context *tcntx)
+{
+	const struct sbi_trap_info *orig_trap = &tcntx->trap;
+	struct sbi_trap_regs *regs = &tcntx->regs;
+
+	/* If fault came from M mode, just fail */
+	if (((regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT) == PRV_M)
+		return SBI_EINVAL;
+
+	/* If platform emulator failed, we redirect instead of fail */
+	if (sbi_platform_emulate_store(sbi_platform_thishart_ptr(), wlen,
+				       orig_trap->tval, in_val))
+		return sbi_trap_redirect(regs, orig_trap);
+
+	return wlen;
+}
+
+int sbi_store_access_handler(struct sbi_trap_context *tcntx)
+{
+	return sbi_trap_emulate_store(tcntx, sbi_st_access_emulator);
+}

lib/sbi/tests/objects.mk

@@ -0,0 +1,14 @@
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_unit_test.o
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_unit_tests.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += bitmap_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_bitmap_test.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += console_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_console_test.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += atomic_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/riscv_atomic_test.o
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += locks_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/riscv_locks_test.o

lib/sbi/tests/riscv_atomic_test.c

@@ -0,0 +1,143 @@
+#include <sbi/sbi_unit_test.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/sbi_bitops.h>
+
+#define ATOMIC_TEST_VAL1 239l
+#define ATOMIC_TEST_VAL2 30l
+#define ATOMIC_TEST_VAL3 2024l
+
+#define ATOMIC_TEST_BIT_NUM 3
+
+#define ATOMIC_TEST_RAW_BIT_CELL 1
+#define ATOMIC_TEST_RAW_BIT_NUM 15
+
+static atomic_t test_atomic;
+
+static void atomic_test_suite_init(void)
+{
+	ATOMIC_INIT(&test_atomic, 0);
+}
+
+static void atomic_rw_test(struct sbiunit_test_case *test)
+{
+	/* We should read the same value as we've written */
+	atomic_write(&test_atomic, ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL1);
+	/* Negative value should also work */
+	atomic_write(&test_atomic, -ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), -ATOMIC_TEST_VAL1);
+}
+
+static void add_return_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_add_return(&test_atomic, ATOMIC_TEST_VAL2),
+			  ATOMIC_TEST_VAL1 + ATOMIC_TEST_VAL2);
+	/* The atomic value should be updated as well */
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL1 + ATOMIC_TEST_VAL2);
+}
+
+static void sub_return_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_sub_return(&test_atomic, ATOMIC_TEST_VAL2),
+			  ATOMIC_TEST_VAL1 - ATOMIC_TEST_VAL2);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL1 - ATOMIC_TEST_VAL2);
+}
+
+static void cmpxchg_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, ATOMIC_TEST_VAL1);
+	/* if current value != expected, it stays the same */
+	SBIUNIT_EXPECT_EQ(test, atomic_cmpxchg(&test_atomic, ATOMIC_TEST_VAL2, ATOMIC_TEST_VAL3),
+			  ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL1);
+	/* if current value == expected, it gets updated */
+	SBIUNIT_EXPECT_EQ(test, atomic_cmpxchg(&test_atomic, ATOMIC_TEST_VAL1, ATOMIC_TEST_VAL2),
+			  ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL2);
+}
+
+static void atomic_xchg_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_xchg(&test_atomic, ATOMIC_TEST_VAL2), ATOMIC_TEST_VAL1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), ATOMIC_TEST_VAL2);
+}
+
+static void atomic_raw_set_bit_test(struct sbiunit_test_case *test)
+{
+	unsigned long data[] = {0, 0, 0};
+	/* the bitpos points to the bit of one of the elements of the `data` array */
+	size_t bitpos = ATOMIC_TEST_RAW_BIT_CELL * BITS_PER_LONG + ATOMIC_TEST_RAW_BIT_NUM;
+
+	/* check if the bit we set actually gets set */
+	SBIUNIT_EXPECT_EQ(test, atomic_raw_set_bit(bitpos, data), 0);
+	SBIUNIT_EXPECT_EQ(test, data[ATOMIC_TEST_RAW_BIT_CELL], 1 << ATOMIC_TEST_RAW_BIT_NUM);
+
+	/* Other elements of the `data` array should stay untouched */
+	SBIUNIT_EXPECT_EQ(test, data[0], 0);
+	SBIUNIT_EXPECT_EQ(test, data[2], 0);
+
+	/* check that if we set the bit twice it stays set */
+	SBIUNIT_EXPECT_EQ(test, atomic_raw_set_bit(bitpos, data), 1);
+	SBIUNIT_EXPECT_EQ(test, data[ATOMIC_TEST_RAW_BIT_CELL], 1 << ATOMIC_TEST_RAW_BIT_NUM);
+}
+
+static void atomic_raw_clear_bit_test(struct sbiunit_test_case *test)
+{
+	unsigned long data[] = {~1UL, 1 << ATOMIC_TEST_RAW_BIT_NUM, ~1UL};
+	/* the bitpos points to the bit of one of the elements of the `data` array */
+	size_t bitpos = ATOMIC_TEST_RAW_BIT_CELL * BITS_PER_LONG + ATOMIC_TEST_RAW_BIT_NUM;
+
+	/* check if the bit we clear actually gets cleared */
+	SBIUNIT_EXPECT_EQ(test, atomic_raw_clear_bit(bitpos, data), 1);
+	SBIUNIT_EXPECT_EQ(test, data[ATOMIC_TEST_RAW_BIT_CELL], 0);
+
+	/* Other elements of the `data` array should stay untouched */
+	SBIUNIT_EXPECT_EQ(test, data[0], ~1UL);
+	SBIUNIT_EXPECT_EQ(test, data[2], ~1UL);
+
+	/* check that if we clear the bit twice it stays cleared */
+	SBIUNIT_EXPECT_EQ(test, atomic_raw_clear_bit(bitpos, data), 0);
+	SBIUNIT_EXPECT_EQ(test, data[ATOMIC_TEST_RAW_BIT_CELL], 0);
+}
+
+static void atomic_set_bit_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, 0);
+	SBIUNIT_EXPECT_EQ(test, atomic_set_bit(ATOMIC_TEST_BIT_NUM, &test_atomic), 0);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), 1 << ATOMIC_TEST_BIT_NUM);
+	/* If we set the bit twice, it stays 1 */
+	SBIUNIT_EXPECT_EQ(test, atomic_set_bit(ATOMIC_TEST_BIT_NUM, &test_atomic), 1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), 1 << ATOMIC_TEST_BIT_NUM);
+}
+
+static void atomic_clear_bit_test(struct sbiunit_test_case *test)
+{
+	atomic_write(&test_atomic, 1 << ATOMIC_TEST_BIT_NUM);
+	SBIUNIT_EXPECT_EQ(test, atomic_clear_bit(ATOMIC_TEST_BIT_NUM, &test_atomic), 1);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), 0);
+	/* if we clear the bit twice, it stays 0 */
+	SBIUNIT_EXPECT_EQ(test, atomic_clear_bit(ATOMIC_TEST_BIT_NUM, &test_atomic), 0);
+	SBIUNIT_EXPECT_EQ(test, atomic_read(&test_atomic), 0);
+}
+
+static struct sbiunit_test_case atomic_test_cases[] = {
+	SBIUNIT_TEST_CASE(atomic_rw_test),
+	SBIUNIT_TEST_CASE(add_return_test),
+	SBIUNIT_TEST_CASE(sub_return_test),
+	SBIUNIT_TEST_CASE(cmpxchg_test),
+	SBIUNIT_TEST_CASE(atomic_xchg_test),
+	SBIUNIT_TEST_CASE(atomic_raw_set_bit_test),
+	SBIUNIT_TEST_CASE(atomic_raw_clear_bit_test),
+	SBIUNIT_TEST_CASE(atomic_set_bit_test),
+	SBIUNIT_TEST_CASE(atomic_clear_bit_test),
+	SBIUNIT_END_CASE,
+};
+
+const struct sbiunit_test_suite atomic_test_suite = {
+	.name = "atomic_test_suite",
+	.cases = atomic_test_cases,
+	.init = atomic_test_suite_init
+};

lib/sbi/tests/riscv_locks_test.c

@@ -0,0 +1,41 @@
+#include <sbi/sbi_unit_test.h>
+#include <sbi/riscv_locks.h>
+
+static spinlock_t test_lock = SPIN_LOCK_INITIALIZER;
+
+static void spin_lock_test(struct sbiunit_test_case *test)
+{
+	/* We don't want to accidentally get locked */
+	SBIUNIT_ASSERT(test, !spin_lock_check(&test_lock));
+
+	spin_lock(&test_lock);
+	SBIUNIT_EXPECT(test, spin_lock_check(&test_lock));
+	spin_unlock(&test_lock);
+
+	SBIUNIT_ASSERT(test, !spin_lock_check(&test_lock));
+}
+
+static void spin_trylock_fail(struct sbiunit_test_case *test)
+{
+	/* We don't want to accidentally get locked */
+	SBIUNIT_ASSERT(test, !spin_lock_check(&test_lock));
+
+	spin_lock(&test_lock);
+	SBIUNIT_EXPECT(test, !spin_trylock(&test_lock));
+	spin_unlock(&test_lock);
+}
+
+static void spin_trylock_success(struct sbiunit_test_case *test)
+{
+	SBIUNIT_EXPECT(test, spin_trylock(&test_lock));
+	spin_unlock(&test_lock);
+}
+
+static struct sbiunit_test_case locks_test_cases[] = {
+	SBIUNIT_TEST_CASE(spin_lock_test),
+	SBIUNIT_TEST_CASE(spin_trylock_fail),
+	SBIUNIT_TEST_CASE(spin_trylock_success),
+	SBIUNIT_END_CASE,
+};
+
+SBIUNIT_TEST_SUITE(locks_test_suite, locks_test_cases);

lib/sbi/tests/sbi_bitmap_test.c

@@ -0,0 +1,102 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Author: Ivan Orlov <ivan.orlov0322@gmail.com>
+ */
+#include <sbi/sbi_bitmap.h>
+#include <sbi/sbi_unit_test.h>
+
+#define DATA_SIZE sizeof(data_zero)
+#define DATA_BIT_SIZE (DATA_SIZE * 8)
+
+static unsigned long data_a[] = { 0xDEADBEEF, 0x00BAB10C, 0x1BADB002, 0xABADBABE };
+static unsigned long data_b[] = { 0xC00010FF, 0x00BAB10C, 0xBAAAAAAD, 0xBADDCAFE };
+static unsigned long data_zero[] = { 0, 0, 0, 0 };
+
+static void bitmap_and_test(struct sbiunit_test_case *test)
+{
+	unsigned long res[DATA_SIZE];
+	unsigned long a_and_b[] = { data_a[0] & data_b[0], data_a[1] & data_b[1],
+				    data_a[2] & data_b[2], data_a[3] & data_b[3] };
+
+	__bitmap_and(res, data_a, data_b, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, a_and_b, DATA_SIZE);
+
+	/* a & a = a */
+	__bitmap_and(res, data_a, data_a, DATA_BIT_SIZE);
+	SBIUNIT_ASSERT_MEMEQ(test, res, data_a, DATA_SIZE);
+
+	/* a & 0 = 0 */
+	__bitmap_and(res, data_a, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+
+	/* 0 & 0 = 0 */
+	__bitmap_and(res, data_zero, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+
+	sbi_memcpy(res, data_zero, DATA_SIZE);
+	/* Cover zero 'bits' argument */
+	__bitmap_and(res, data_a, data_b, 0);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+}
+
+static void bitmap_or_test(struct sbiunit_test_case *test)
+{
+	unsigned long res[DATA_SIZE];
+	unsigned long a_or_b[] = { data_a[0] | data_b[0], data_a[1] | data_b[1],
+				   data_a[2] | data_b[2], data_a[3] | data_b[3] };
+
+	__bitmap_or(res, data_a, data_b, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, a_or_b, DATA_SIZE);
+
+	/* a | a = a */
+	__bitmap_or(res, data_a, data_a, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_a, DATA_SIZE);
+
+	/* a | 0 = a */
+	__bitmap_or(res, data_a, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_a, DATA_SIZE);
+
+	/* 0 | 0 = 0 */
+	__bitmap_or(res, data_zero, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+
+	sbi_memcpy(res, data_zero, DATA_SIZE);
+	__bitmap_or(res, data_a, data_b, 0);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+}
+
+static void bitmap_xor_test(struct sbiunit_test_case *test)
+{
+	unsigned long res[DATA_SIZE];
+	unsigned long a_xor_b[] = { data_a[0] ^ data_b[0], data_a[1] ^ data_b[1],
+				    data_a[2] ^ data_b[2], data_a[3] ^ data_b[3] };
+
+	__bitmap_xor(res, data_a, data_b, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, a_xor_b, DATA_SIZE);
+
+	/* a ^ 0 = a */
+	__bitmap_xor(res, data_a, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_a, DATA_SIZE);
+
+	/* a ^ a = 0 */
+	__bitmap_xor(res, data_a, data_a, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+
+	/* 0 ^ 0 = 0 */
+	__bitmap_xor(res, data_zero, data_zero, DATA_BIT_SIZE);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+
+	sbi_memcpy(res, data_zero, DATA_SIZE);
+	__bitmap_xor(res, data_a, data_b, 0);
+	SBIUNIT_EXPECT_MEMEQ(test, res, data_zero, DATA_SIZE);
+}
+
+static struct sbiunit_test_case bitmap_test_cases[] = {
+	SBIUNIT_TEST_CASE(bitmap_and_test),
+	SBIUNIT_TEST_CASE(bitmap_or_test),
+	SBIUNIT_TEST_CASE(bitmap_xor_test),
+	SBIUNIT_END_CASE,
+};
+
+SBIUNIT_TEST_SUITE(bitmap_test_suite, bitmap_test_cases);

lib/sbi/tests/sbi_console_test.c

@@ -0,0 +1,107 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Author: Ivan Orlov <ivan.orlov0322@gmail.com>
+ */
+#include <sbi/riscv_locks.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_unit_test.h>
+
+#define TEST_CONSOLE_BUF_LEN 1024
+
+static const struct sbi_console_device *old_dev;
+static char test_console_buf[TEST_CONSOLE_BUF_LEN];
+static u32 test_console_buf_pos;
+static spinlock_t test_console_lock = SPIN_LOCK_INITIALIZER;
+
+static void test_console_putc(char c)
+{
+	test_console_buf[test_console_buf_pos] = c;
+	test_console_buf_pos = (test_console_buf_pos + 1) % TEST_CONSOLE_BUF_LEN;
+}
+
+static void clear_test_console_buf(void)
+{
+	test_console_buf_pos = 0;
+	test_console_buf[0] = '\0';
+}
+
+static const struct sbi_console_device test_console_dev = {
+	.name = "Test console device",
+	.console_putc = test_console_putc,
+};
+
+/* Mock the console device */
+static inline void test_console_begin(const struct sbi_console_device *device)
+{
+	old_dev = sbi_console_get_device();
+	sbi_console_set_device(device);
+}
+
+static inline void test_console_end(void)
+{
+	sbi_console_set_device(old_dev);
+}
+
+static void putc_test(struct sbiunit_test_case *test)
+{
+	clear_test_console_buf();
+	test_console_begin(&test_console_dev);
+	sbi_putc('a');
+	test_console_end();
+	SBIUNIT_ASSERT_EQ(test, test_console_buf[0], 'a');
+}
+
+#define PUTS_TEST(test, expected, str) do {			\
+	spin_lock(&test_console_lock);				\
+	clear_test_console_buf();				\
+	test_console_begin(&test_console_dev);			\
+	sbi_puts(str);						\
+	test_console_end();					\
+	SBIUNIT_ASSERT_STREQ(test, test_console_buf, expected,	\
+			     sbi_strlen(expected));		\
+	spin_unlock(&test_console_lock);			\
+} while (0)
+
+static void puts_test(struct sbiunit_test_case *test)
+{
+	PUTS_TEST(test, "Hello, OpenSBI!", "Hello, OpenSBI!");
+	PUTS_TEST(test, "Hello,\r\nOpenSBI!", "Hello,\nOpenSBI!");
+}
+
+#define PRINTF_TEST(test, expected, format, ...) do {		\
+	spin_lock(&test_console_lock);				\
+	clear_test_console_buf();				\
+	test_console_begin(&test_console_dev);			\
+	size_t __res = sbi_printf(format, ##__VA_ARGS__);	\
+	test_console_end();					\
+	SBIUNIT_ASSERT_EQ(test, __res, sbi_strlen(expected));	\
+	SBIUNIT_ASSERT_STREQ(test, test_console_buf, expected,	\
+			     sbi_strlen(expected));		\
+	spin_unlock(&test_console_lock);			\
+} while (0)
+
+static void printf_test(struct sbiunit_test_case *test)
+{
+	PRINTF_TEST(test, "Hello", "Hello");
+	PRINTF_TEST(test, "3 5 7", "%d %d %d", 3, 5, 7);
+	PRINTF_TEST(test, "Hello", "%s", "Hello");
+	PRINTF_TEST(test, "-1", "%d", -1);
+	PRINTF_TEST(test, "FF", "%X", 255);
+	PRINTF_TEST(test, "ff", "%x", 255);
+	PRINTF_TEST(test, "A", "%c", 'A');
+	PRINTF_TEST(test, "1fe", "%p", (void *)0x1fe);
+	PRINTF_TEST(test, "4294967295", "%u", 4294967295U);
+	PRINTF_TEST(test, "-2147483647", "%ld", -2147483647l);
+	PRINTF_TEST(test, "-9223372036854775807", "%lld", -9223372036854775807LL);
+	PRINTF_TEST(test, "18446744073709551615", "%llu", 18446744073709551615ULL);
+}
+
+static struct sbiunit_test_case console_test_cases[] = {
+	SBIUNIT_TEST_CASE(putc_test),
+	SBIUNIT_TEST_CASE(puts_test),
+	SBIUNIT_TEST_CASE(printf_test),
+	SBIUNIT_END_CASE,
+};
+
+SBIUNIT_TEST_SUITE(console_test_suite, console_test_cases);

lib/sbi/tests/sbi_unit_test.c

@@ -0,0 +1,46 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Author: Ivan Orlov <ivan.orlov0322@gmail.com>
+ */
+#include <sbi/sbi_unit_test.h>
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_console.h>
+
+extern struct sbiunit_test_suite *sbi_unit_tests[];
+extern unsigned long sbi_unit_tests_size;
+
+static void run_test_suite(struct sbiunit_test_suite *suite)
+{
+	struct sbiunit_test_case *s_case;
+	u32 count_pass = 0, count_fail = 0;
+
+	sbi_printf("## Running test suite: %s\n", suite->name);
+
+	if (suite->init)
+		suite->init();
+
+	s_case = suite->cases;
+	while (s_case->test_func) {
+		s_case->test_func(s_case);
+		if (s_case->failed)
+			count_fail++;
+		else
+			count_pass++;
+		sbi_printf("[%s] %s\n", s_case->failed ? "FAILED" : "PASSED",
+			   s_case->name);
+		s_case++;
+	}
+	sbi_printf("%u PASSED / %u FAILED / %u TOTAL\n", count_pass, count_fail,
+		   count_pass + count_fail);
+}
+
+void run_all_tests(void)
+{
+	u32 i;
+
+	sbi_printf("\n# Running SBIUNIT tests #\n");
+
+	for (i = 0; i < sbi_unit_tests_size; i++)
+		run_test_suite(sbi_unit_tests[i]);
+}

lib/sbi/tests/sbi_unit_tests.carray

@@ -0,0 +1,3 @@
+HEADER: sbi/sbi_unit_test.h
+TYPE: struct sbiunit_test_suite
+NAME: sbi_unit_tests

lib/utils/fdt/fdt_fixup.c

@@ -385,6 +385,21 @@ int fdt_reserved_memory_fixup(void *fdt)
 	return 0;
 }
 
+void fdt_config_fixup(void *fdt)
+{
+	int chosen_offset, config_offset;
+
+	chosen_offset = fdt_path_offset(fdt, "/chosen");
+	if (chosen_offset < 0)
+		return;
+
+	config_offset = fdt_node_offset_by_compatible(fdt, chosen_offset, "opensbi,config");
+	if (config_offset < 0)
+		return;
+
+	fdt_nop_node(fdt, config_offset);
+}
+
 void fdt_fixups(void *fdt)
 {
 	fdt_aplic_fixup(fdt);
@@ -398,4 +413,6 @@ void fdt_fixups(void *fdt)
 #ifndef CONFIG_FDT_FIXUPS_PRESERVE_PMU_NODE
 	fdt_pmu_fixup(fdt);
 #endif
+
+	fdt_config_fixup(fdt);
 }

lib/utils/fdt/fdt_helper.c

@@ -720,7 +720,9 @@ aplic_msi_parent_done:
 		deleg->child_index = 0;
 	}
 
-	del = fdt_getprop(fdt, nodeoff, "riscv,delegate", &len);
+	del = fdt_getprop(fdt, nodeoff, "riscv,delegation", &len);
+	if (!del)
+		del = fdt_getprop(fdt, nodeoff, "riscv,delegate", &len);
 	if (!del || len < (3 * sizeof(fdt32_t)))
 		goto skip_delegate_parse;
 	d = 0;

lib/utils/gpio/gpio.c

@@ -73,17 +73,26 @@ int gpio_direction_output(struct gpio_pin *gp, int value)
 	if (!gp->chip->direction_output)
 		return SBI_ENOSYS;
 
+	if (gp->flags & GPIO_FLAG_ACTIVE_LOW)
+		value = value == 0 ? 1 : 0;
+
 	return gp->chip->direction_output(gp, value);
 }
 
 int gpio_get(struct gpio_pin *gp)
 {
+	int value;
+
 	if (!gp || !gp->chip || (gp->chip->ngpio <= gp->offset))
 		return SBI_EINVAL;
 	if (!gp->chip->get)
 		return SBI_ENOSYS;
 
-	return gp->chip->get(gp);
+	value = gp->chip->get(gp);
+
+	if (gp->flags & GPIO_FLAG_ACTIVE_LOW)
+		value = value == 0 ? 1 : 0;
+	return value;
 }
 
 int gpio_set(struct gpio_pin *gp, int value)
@@ -93,6 +102,9 @@ int gpio_set(struct gpio_pin *gp, int value)
 	if (!gp->chip->set)
 		return SBI_ENOSYS;
 
+	if (gp->flags & GPIO_FLAG_ACTIVE_LOW)
+		value = value == 0 ? 1 : 0;
+
 	gp->chip->set(gp, value);
 	return 0;
 }

lib/utils/ipi/fdt_ipi.c

@@ -44,6 +44,9 @@ static int fdt_ipi_cold_init(void)
 		noff = -1;
 		while ((noff = fdt_find_match(fdt, noff,
 					drv->match_table, &match)) >= 0) {
+			if (!fdt_node_is_enabled(fdt, noff))
+				continue;
+
 			/* drv->cold_init must not be NULL */
 			if (drv->cold_init == NULL)
 				return SBI_EFAIL;

lib/utils/irqchip/fdt_irqchip.c

@@ -62,6 +62,9 @@ static int fdt_irqchip_cold_init(void)
 		drv_added = false;
 		while ((noff = fdt_find_match(fdt, noff,
 					drv->match_table, &match)) >= 0) {
+			if (!fdt_node_is_enabled(fdt,noff))
+				continue;
+
 			if (drv->cold_init) {
 				rc = drv->cold_init(fdt, noff, match);
 				if (rc == SBI_ENODEV)

lib/utils/irqchip/imsic.c

@@ -12,6 +12,7 @@
 #include <sbi/riscv_io.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_console.h>
+#include <sbi/sbi_csr_detect.h>
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_irqchip.h>
@@ -122,6 +123,9 @@ struct imsic_data *imsic_get_data(u32 hartid)
 {
 	struct sbi_scratch *scratch;
 
+	if (!imsic_ptr_offset)
+		return NULL;
+
 	scratch = sbi_hartid_to_scratch(hartid);
 	if (!scratch)
 		return NULL;
@@ -133,6 +137,9 @@ int imsic_get_target_file(u32 hartid)
 {
 	struct sbi_scratch *scratch;
 
+	if (!imsic_file_offset)
+		return SBI_ENOENT;
+
 	scratch = sbi_hartid_to_scratch(hartid);
 	if (!scratch)
 		return SBI_ENOENT;
@@ -140,7 +147,7 @@ int imsic_get_target_file(u32 hartid)
 	return imsic_get_hart_file(scratch);
 }
 
-static int imsic_external_irqfn(struct sbi_trap_regs *regs)
+static int imsic_external_irqfn(void)
 {
 	ulong mirq;
 
@@ -222,6 +229,8 @@ static void imsic_local_eix_update(unsigned long base_id,
 
 void imsic_local_irqchip_init(void)
 {
+	struct sbi_trap_info trap = { 0 };
+
 	/*
 	 * This function is expected to be called from:
 	 * 1) nascent_init() platform callback which is called
@@ -231,6 +240,11 @@ void imsic_local_irqchip_init(void)
 	 *    in boot-up path.
 	 */
 
+	/* If Smaia not available then do nothing */
+	csr_read_allowed(CSR_MTOPI, (ulong)&trap);
+	if (trap.cause)
+		return;
+
 	/* Setup threshold to allow all enabled interrupts */
 	imsic_csr_write(IMSIC_EITHRESHOLD, IMSIC_ENABLE_EITHRESHOLD);
 

lib/utils/reset/Kconfig

@@ -24,6 +24,10 @@ config FDT_RESET_HTIF
 	select SYS_HTIF
 	default n
 
+config FDT_RESET_SG2042_HWMON_MCU
+	bool "Sophgo SG2042 hwmon MCU FDT reset driver"
+	default n
+
 config FDT_RESET_SUNXI_WDT
 	bool "Sunxi WDT FDT reset driver"
 	default n

lib/utils/reset/fdt_reset.c

@@ -19,22 +19,27 @@ extern unsigned long fdt_reset_drivers_size;
 
 int fdt_reset_driver_init(void *fdt, struct fdt_reset *drv)
 {
-	int noff, rc = SBI_ENODEV;
+	int noff, rc, cnt = 0;
 	const struct fdt_match *match;
 
-	noff = fdt_find_match(fdt, -1, drv->match_table, &match);
-	if (noff < 0)
-		return SBI_ENODEV;
+	noff = -1;
+	while ((noff = fdt_find_match(fdt, noff,
+				drv->match_table, &match)) >= 0) {
+		if (!fdt_node_is_enabled(fdt, noff))
+			continue;
 
-	if (drv->init) {
-		rc = drv->init(fdt, noff, match);
-		if (rc && rc != SBI_ENODEV) {
-			sbi_printf("%s: %s init failed, %d\n",
-				   __func__, match->compatible, rc);
+		if (drv->init) {
+			rc = drv->init(fdt, noff, match);
+			if (!rc)
+				cnt++;
+			else if (rc != SBI_ENODEV) {
+				sbi_printf("%s: %s init failed, %d\n",
+					__func__, match->compatible, rc);
+			}
 		}
 	}
 
-	return rc;
+	return cnt > 0 ? 0 : SBI_ENODEV;
 }
 
 void fdt_reset_init(void)

lib/utils/reset/fdt_reset_sg2042_hwmon_mcu.c

@@ -0,0 +1,114 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2023 Lin Chunzhi <chunzhi.lin@sophgo.com>
+ */
+
+#include <libfdt.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_types.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_console.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+#include <sbi_utils/reset/fdt_reset.h>
+#include <sbi_utils/i2c/fdt_i2c.h>
+
+#define MANGO_BOARD_TYPE_MASK		0x80
+
+#define REG_BOARD_TYPE			0x00
+#define REG_CMD				0x03
+
+#define CMD_POWEROFF			0x02
+#define CMD_RESET			0x03
+#define CMD_REBOOT			0x07
+
+static struct i2c_adapter *mcu_adapter = NULL;
+static uint32_t mcu_reg = 0;
+
+static int sg2042_mcu_reset_check(u32 type, u32 reason)
+{
+	switch (type) {
+	case SBI_SRST_RESET_TYPE_SHUTDOWN:
+		return 1;
+	case SBI_SRST_RESET_TYPE_COLD_REBOOT:
+	case SBI_SRST_RESET_TYPE_WARM_REBOOT:
+		return 255;
+	}
+
+	return 0;
+}
+
+static void sg2042_mcu_reset(u32 type, u32 reason)
+{
+	switch (type) {
+	case SBI_SRST_RESET_TYPE_SHUTDOWN:
+		i2c_adapter_reg_write(mcu_adapter, mcu_reg,
+				      REG_CMD, CMD_POWEROFF);
+		break;
+	case SBI_SRST_RESET_TYPE_COLD_REBOOT:
+	case SBI_SRST_RESET_TYPE_WARM_REBOOT:
+		i2c_adapter_reg_write(mcu_adapter, mcu_reg,
+				      REG_CMD, CMD_REBOOT);
+		break;
+	}
+}
+
+static struct sbi_system_reset_device sg2042_mcu_reset_device = {
+	.name = "sg2042-mcu-reset",
+	.system_reset_check = sg2042_mcu_reset_check,
+	.system_reset = sg2042_mcu_reset
+};
+
+static int sg2042_mcu_reset_check_board(struct i2c_adapter *adap, uint32_t reg)
+{
+	static uint8_t val;
+	int ret;
+
+	/* check board type */
+	ret = i2c_adapter_reg_read(adap, reg, REG_BOARD_TYPE, &val);
+	if (ret)
+		return ret;
+
+	if (!(val & MANGO_BOARD_TYPE_MASK))
+		return SBI_ENODEV;
+
+	return 0;
+}
+
+static int sg2042_mcu_reset_init(void *fdt, int nodeoff,
+				 const struct fdt_match *match)
+{
+	int ret, i2c_bus;
+	uint64_t addr;
+
+	ret = fdt_get_node_addr_size(fdt, nodeoff, 0, &addr, NULL);
+	if (ret)
+		return ret;
+
+	mcu_reg = addr;
+
+	i2c_bus = fdt_parent_offset(fdt, nodeoff);
+	if (i2c_bus < 0)
+		return i2c_bus;
+
+	ret = fdt_i2c_adapter_get(fdt, i2c_bus, &mcu_adapter);
+	if (ret)
+		return ret;
+
+	ret = sg2042_mcu_reset_check_board(mcu_adapter, mcu_reg);
+
+	sbi_system_reset_add_device(&sg2042_mcu_reset_device);
+
+	return 0;
+}
+
+static const struct fdt_match sg2042_mcu_reset_match[] = {
+	{ .compatible = "sophgo,sg2042-hwmon-mcu", .data = (void *)true},
+	{ },
+};
+
+struct fdt_reset fdt_reset_sg2042_mcu = {
+	.match_table = sg2042_mcu_reset_match,
+	.init = sg2042_mcu_reset_init,
+};

lib/utils/reset/objects.mk

@@ -20,6 +20,9 @@ libsbiutils-objs-$(CONFIG_FDT_RESET_GPIO) += reset/fdt_reset_gpio.o
 carray-fdt_reset_drivers-$(CONFIG_FDT_RESET_HTIF) += fdt_reset_htif
 libsbiutils-objs-$(CONFIG_FDT_RESET_HTIF) += reset/fdt_reset_htif.o
 
+carray-fdt_reset_drivers-$(CONFIG_FDT_RESET_SG2042_HWMON_MCU) += fdt_reset_sg2042_mcu
+libsbiutils-objs-$(CONFIG_FDT_RESET_SG2042_HWMON_MCU) += reset/fdt_reset_sg2042_hwmon_mcu.o
+
 carray-fdt_reset_drivers-$(CONFIG_FDT_RESET_SUNXI_WDT) += fdt_reset_sunxi_wdt
 libsbiutils-objs-$(CONFIG_FDT_RESET_SUNXI_WDT) += reset/fdt_reset_sunxi_wdt.o
 

lib/utils/serial/fdt_serial.c

@@ -40,6 +40,10 @@ int fdt_serial_init(void)
 			else
 				noff = fdt_path_offset(fdt, prop);
 		}
+		if (-1 < noff) {
+			if (!fdt_node_is_enabled(fdt, noff))
+				noff = -1;
+		}
 	}
 
 	/* First check DT node pointed by stdout-path */
@@ -64,18 +68,21 @@ int fdt_serial_init(void)
 	for (pos = 0; pos < fdt_serial_drivers_size; pos++) {
 		drv = fdt_serial_drivers[pos];
 
-		noff = fdt_find_match(fdt, -1, drv->match_table, &match);
-		if (noff < 0)
-			continue;
+		noff = -1;
+		while ((noff = fdt_find_match(fdt, noff,
+					drv->match_table, &match)) >= 0) {
+			if (!fdt_node_is_enabled(fdt, noff))
+				continue;
 
-		/* drv->init must not be NULL */
-		if (drv->init == NULL)
-			return SBI_EFAIL;
+			/* drv->init must not be NULL */
+			if (drv->init == NULL)
+				return SBI_EFAIL;
 
-		rc = drv->init(fdt, noff, match);
-		if (rc == SBI_ENODEV)
-			continue;
-		return rc;
+			rc = drv->init(fdt, noff, match);
+			if (rc == SBI_ENODEV)
+				continue;
+			return rc;
+		}
 	}
 
 	return SBI_ENODEV;

lib/utils/serial/litex-uart.c

@@ -20,6 +20,8 @@
 #define UART_REG_EV_PENDING	4
 #define UART_REG_EV_ENABLE	5
 
+#define UART_EV_RX		0x2
+
 /* clang-format on */
 
 static volatile u32 *uart_base;
@@ -42,10 +44,14 @@ static void litex_uart_putc(char ch)
 
 static int litex_uart_getc(void)
 {
+	int ret;
+
 	if (get_reg(UART_REG_RXEMPTY))
 		return -1;
-	else
-		return get_reg(UART_REG_RXTX);
+
+	ret = get_reg(UART_REG_RXTX);
+	set_reg(UART_REG_EV_PENDING, UART_EV_RX); /* ack. char read */
+	return ret;
 }
 
 static struct sbi_console_device litex_console = {

lib/utils/timer/aclint_mtimer.c

@@ -72,14 +72,10 @@ static u64 mtimer_value(void)
 static void mtimer_event_stop(void)
 {
 	u32 target_hart = current_hartid();
-	struct sbi_scratch *scratch;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	struct aclint_mtimer_data *mt;
 	u64 *time_cmp;
 
-	scratch = sbi_hartid_to_scratch(target_hart);
-	if (!scratch)
-		return;
-
 	mt = mtimer_get_hart_data_ptr(scratch);
 	if (!mt)
 		return;
@@ -92,14 +88,10 @@ static void mtimer_event_stop(void)
 static void mtimer_event_start(u64 next_event)
 {
 	u32 target_hart = current_hartid();
-	struct sbi_scratch *scratch;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	struct aclint_mtimer_data *mt;
 	u64 *time_cmp;
 
-	scratch = sbi_hartid_to_scratch(target_hart);
-	if (!scratch)
-		return;
-
 	mt = mtimer_get_hart_data_ptr(scratch);
 	if (!mt)
 		return;
@@ -155,13 +147,9 @@ int aclint_mtimer_warm_init(void)
 {
 	u64 *mt_time_cmp;
 	u32 target_hart = current_hartid();
-	struct sbi_scratch *scratch;
+	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
 	struct aclint_mtimer_data *mt;
 
-	scratch = sbi_hartid_to_scratch(target_hart);
-	if (!scratch)
-		return SBI_ENOENT;
-
 	mt = mtimer_get_hart_data_ptr(scratch);
 	if (!mt)
 		return SBI_ENODEV;

lib/utils/timer/fdt_timer.c

@@ -44,6 +44,9 @@ static int fdt_timer_cold_init(void)
 		noff = -1;
 		while ((noff = fdt_find_match(fdt, noff,
 					drv->match_table, &match)) >= 0) {
+			if (!fdt_node_is_enabled(fdt, noff))
+				continue;
+
 			/* drv->cold_init must not be NULL */
 			if (drv->cold_init == NULL)
 				return SBI_EFAIL;

platform/fpga/ariane/objects.mk

@@ -17,7 +17,6 @@ platform-objs-y += platform.o
 PLATFORM_RISCV_XLEN = 64
 
 # Blobs to build
-FW_TEXT_START=0x80000000
 FW_JUMP=n
 
 ifeq ($(PLATFORM_RISCV_XLEN), 32)

platform/fpga/openpiton/objects.mk

@@ -16,7 +16,6 @@ platform-objs-y += platform.o
 PLATFORM_RISCV_XLEN = 64
 
 # Blobs to build
-FW_TEXT_START=0x80000000
 FW_JUMP=n
 
 ifeq ($(PLATFORM_RISCV_XLEN), 32)

platform/generic/Kconfig

@@ -37,7 +37,7 @@ config PLATFORM_ANDES_AE350
 
 config PLATFORM_RENESAS_RZFIVE
 	bool "Renesas RZ/Five support"
-	select ANDES45_PMA
+	select ANDES_PMA
 	select ANDES_SBI
 	select ANDES_PMU
 	default n

platform/generic/allwinner/sun20i-d1.c

@@ -33,15 +33,6 @@
 #define RISCV_CFG_BGR_REG		0xd0c
 #define PPU_BGR_REG			0x1ac
 
-/*
- * CSRs
- */
-
-#define CSR_MXSTATUS			0x7c0
-#define CSR_MHCR			0x7c1
-#define CSR_MCOR			0x7c2
-#define CSR_MHINT			0x7c5
-
 static unsigned long csr_mxstatus;
 static unsigned long csr_mhcr;
 static unsigned long csr_mhint;
@@ -49,24 +40,24 @@ static unsigned long csr_mhint;
 static void sun20i_d1_csr_save(void)
 {
 	/* Save custom CSRs. */
-	csr_mxstatus	= csr_read(CSR_MXSTATUS);
-	csr_mhcr	= csr_read(CSR_MHCR);
-	csr_mhint	= csr_read(CSR_MHINT);
+	csr_mxstatus	= csr_read(THEAD_C9XX_CSR_MXSTATUS);
+	csr_mhcr	= csr_read(THEAD_C9XX_CSR_MHCR);
+	csr_mhint	= csr_read(THEAD_C9XX_CSR_MHINT);
 
 	/* Flush and disable caches. */
-	csr_write(CSR_MCOR, 0x22);
-	csr_write(CSR_MHCR, 0x0);
+	csr_write(THEAD_C9XX_CSR_MCOR, 0x22);
+	csr_write(THEAD_C9XX_CSR_MHCR, 0x0);
 }
 
 static void sun20i_d1_csr_restore(void)
 {
 	/* Invalidate caches and the branch predictor. */
-	csr_write(CSR_MCOR, 0x70013);
+	csr_write(THEAD_C9XX_CSR_MCOR, 0x70013);
 
 	/* Restore custom CSRs, including the cache state. */
-	csr_write(CSR_MXSTATUS,	csr_mxstatus);
-	csr_write(CSR_MHCR,	csr_mhcr);
-	csr_write(CSR_MHINT,	csr_mhint);
+	csr_write(THEAD_C9XX_CSR_MXSTATUS, csr_mxstatus);
+	csr_write(THEAD_C9XX_CSR_MHCR, csr_mhcr);
+	csr_write(THEAD_C9XX_CSR_MHINT, csr_mhint);
 }
 
 /*
@@ -151,7 +142,7 @@ static void sun20i_d1_riscv_cfg_restore(void)
 
 static void sun20i_d1_riscv_cfg_init(void)
 {
-	u64 entry = sbi_hartid_to_scratch(0)->warmboot_addr;
+	u64 entry = sbi_scratch_thishart_ptr()->warmboot_addr;
 
 	/* Enable MMIO access. */
 	writel_relaxed(CCU_BGR_ENABLE, SUN20I_D1_CCU_BASE + RISCV_CFG_BGR_REG);

platform/generic/andes/Kconfig

@@ -1,6 +1,6 @@
 # SPDX-License-Identifier: BSD-2-Clause
 
-config ANDES45_PMA
+config ANDES_PMA
 	bool "Andes PMA support"
 	default n
 

platform/generic/andes/ae350.c

@@ -18,7 +18,7 @@
 #include <sbi/sbi_hsm.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_init.h>
-#include <andes/andes45.h>
+#include <andes/andes.h>
 
 static struct smu_data smu = { 0 };
 extern void __ae350_enable_coherency_warmboot(void);
@@ -65,8 +65,9 @@ static int ae350_hart_stop(void)
 	smu_set_wakeup_events(&smu, 0x0, hartid);
 	smu_set_command(&smu, DEEP_SLEEP_CMD, hartid);
 
-	rc = smu_set_reset_vector(&smu, (ulong)__ae350_enable_coherency_warmboot,
-			       hartid);
+	rc = smu_set_reset_vector(&smu,
+				  (ulong)__ae350_enable_coherency_warmboot,
+				  hartid);
 	if (rc)
 		goto fail;
 

platform/generic/andes/andes45-pma.c

@@ -1,350 +0,0 @@
-// SPDX-License-Identifier: GPL-2.0
-/*
- * Copyright (C) 2023 Renesas Electronics Corp.
- *
- * Copyright (c) 2020 Andes Technology Corporation
- *
- * Authors:
- *      Nick Hu <nickhu@andestech.com>
- *      Nylon Chen <nylon7@andestech.com>
- *      Lad Prabhakar <prabhakar.mahadev-lad.rj@bp.renesas.com>
- */
-
-#include <andes/andes45_pma.h>
-#include <libfdt.h>
-#include <sbi/riscv_asm.h>
-#include <sbi/riscv_io.h>
-#include <sbi/sbi_console.h>
-#include <sbi/sbi_error.h>
-#include <sbi_utils/fdt/fdt_helper.h>
-
-/* Configuration Registers */
-#define ANDES45_CSR_MMSC_CFG		0xFC2
-#define ANDES45_CSR_MMSC_PPMA_OFFSET	(1 << 30)
-
-#define ANDES45_PMAADDR_0		0xBD0
-
-#define ANDES45_PMACFG_0		0xBC0
-
-static inline unsigned long andes45_pma_read_cfg(unsigned int pma_cfg_off)
-{
-#define switchcase_pma_cfg_read(__pma_cfg_off, __val)		\
-	case __pma_cfg_off:					\
-		__val = csr_read(__pma_cfg_off);		\
-		break;
-#define switchcase_pma_cfg_read_2(__pma_cfg_off, __val)		\
-	switchcase_pma_cfg_read(__pma_cfg_off + 0, __val)	\
-	switchcase_pma_cfg_read(__pma_cfg_off + 2, __val)
-
-	unsigned long ret = 0;
-
-	switch (pma_cfg_off) {
-	switchcase_pma_cfg_read_2(ANDES45_PMACFG_0, ret)
-
-	default:
-		sbi_panic("%s: Unknown PMA CFG offset %#x", __func__, pma_cfg_off);
-		break;
-	}
-
-	return ret;
-
-#undef switchcase_pma_cfg_read_2
-#undef switchcase_pma_cfg_read
-}
-
-static inline void andes45_pma_write_cfg(unsigned int pma_cfg_off, unsigned long val)
-{
-#define switchcase_pma_cfg_write(__pma_cfg_off, __val)		\
-	case __pma_cfg_off:					\
-		csr_write(__pma_cfg_off, __val);		\
-		break;
-#define switchcase_pma_cfg_write_2(__pma_cfg_off, __val)	\
-	switchcase_pma_cfg_write(__pma_cfg_off + 0, __val)	\
-	switchcase_pma_cfg_write(__pma_cfg_off + 2, __val)
-
-	switch (pma_cfg_off) {
-	switchcase_pma_cfg_write_2(ANDES45_PMACFG_0, val)
-
-	default:
-		sbi_panic("%s: Unknown PMA CFG offset %#x", __func__, pma_cfg_off);
-		break;
-	}
-
-#undef switchcase_pma_cfg_write_2
-#undef switchcase_pma_cfg_write
-}
-
-static inline void andes45_pma_write_addr(unsigned int pma_addr_off, unsigned long val)
-{
-#define switchcase_pma_write(__pma_addr_off, __val)		\
-	case __pma_addr_off:					\
-		csr_write(__pma_addr_off, __val);		\
-		break;
-#define switchcase_pma_write_2(__pma_addr_off, __val)		\
-	switchcase_pma_write(__pma_addr_off + 0, __val)		\
-	switchcase_pma_write(__pma_addr_off + 1, __val)
-#define switchcase_pma_write_4(__pma_addr_off, __val)		\
-	switchcase_pma_write_2(__pma_addr_off + 0, __val)	\
-	switchcase_pma_write_2(__pma_addr_off + 2, __val)
-#define switchcase_pma_write_8(__pma_addr_off, __val)		\
-	switchcase_pma_write_4(__pma_addr_off + 0, __val)	\
-	switchcase_pma_write_4(__pma_addr_off + 4, __val)
-#define switchcase_pma_write_16(__pma_addr_off, __val)		\
-	switchcase_pma_write_8(__pma_addr_off + 0, __val)	\
-	switchcase_pma_write_8(__pma_addr_off + 8, __val)
-
-	switch (pma_addr_off) {
-	switchcase_pma_write_16(ANDES45_PMAADDR_0, val)
-
-	default:
-		sbi_panic("%s: Unknown PMA ADDR offset %#x", __func__, pma_addr_off);
-		break;
-	}
-
-#undef switchcase_pma_write_16
-#undef switchcase_pma_write_8
-#undef switchcase_pma_write_4
-#undef switchcase_pma_write_2
-#undef switchcase_pma_write
-}
-
-static inline unsigned long andes45_pma_read_addr(unsigned int pma_addr_off)
-{
-#define switchcase_pma_read(__pma_addr_off, __val)		\
-	case __pma_addr_off:					\
-		__val = csr_read(__pma_addr_off);		\
-		break;
-#define switchcase_pma_read_2(__pma_addr_off, __val)		\
-	switchcase_pma_read(__pma_addr_off + 0, __val)		\
-	switchcase_pma_read(__pma_addr_off + 1, __val)
-#define switchcase_pma_read_4(__pma_addr_off, __val)		\
-	switchcase_pma_read_2(__pma_addr_off + 0, __val)	\
-	switchcase_pma_read_2(__pma_addr_off + 2, __val)
-#define switchcase_pma_read_8(__pma_addr_off, __val)		\
-	switchcase_pma_read_4(__pma_addr_off + 0, __val)	\
-	switchcase_pma_read_4(__pma_addr_off + 4, __val)
-#define switchcase_pma_read_16(__pma_addr_off, __val)		\
-	switchcase_pma_read_8(__pma_addr_off + 0, __val)	\
-	switchcase_pma_read_8(__pma_addr_off + 8, __val)
-
-	unsigned long ret = 0;
-
-	switch (pma_addr_off) {
-	switchcase_pma_read_16(ANDES45_PMAADDR_0, ret)
-
-	default:
-		sbi_panic("%s: Unknown PMA ADDR offset %#x", __func__, pma_addr_off);
-		break;
-	}
-
-	return ret;
-
-#undef switchcase_pma_read_16
-#undef switchcase_pma_read_8
-#undef switchcase_pma_read_4
-#undef switchcase_pma_read_2
-#undef switchcase_pma_read
-}
-
-static unsigned long
-andes45_pma_setup(const struct andes45_pma_region *pma_region,
-		  unsigned int entry_id)
-{
-	unsigned long size = pma_region->size;
-	unsigned long addr = pma_region->pa;
-	unsigned int pma_cfg_addr;
-	unsigned long pmacfg_val;
-	unsigned long pmaaddr;
-	char *pmaxcfg;
-
-	/* Check for 4KiB granularity */
-	if (size < (1 << 12))
-		return SBI_EINVAL;
-
-	/* Check size is power of 2 */
-	if (size & (size - 1))
-		return SBI_EINVAL;
-
-	if (entry_id > 15)
-		return SBI_EINVAL;
-
-	if (!(pma_region->flags & ANDES45_PMACFG_ETYP_NAPOT))
-		return SBI_EINVAL;
-
-	if ((addr & (size - 1)) != 0)
-		return SBI_EINVAL;
-
-	pma_cfg_addr = entry_id / 8 ? ANDES45_PMACFG_0 + 2 : ANDES45_PMACFG_0;
-	pmacfg_val = andes45_pma_read_cfg(pma_cfg_addr);
-	pmaxcfg = (char *)&pmacfg_val + (entry_id % 8);
-	*pmaxcfg = 0;
-	*pmaxcfg = pma_region->flags;
-
-	andes45_pma_write_cfg(pma_cfg_addr, pmacfg_val);
-
-	pmaaddr = (addr >> 2) + (size >> 3) - 1;
-
-	andes45_pma_write_addr(ANDES45_PMAADDR_0 + entry_id, pmaaddr);
-
-	return andes45_pma_read_addr(ANDES45_PMAADDR_0 + entry_id) == pmaaddr ?
-			pmaaddr : SBI_EINVAL;
-}
-
-static int andes45_fdt_pma_resv(void *fdt, const struct andes45_pma_region *pma,
-				unsigned int index, int parent)
-{
-	int na = fdt_address_cells(fdt, 0);
-	int ns = fdt_size_cells(fdt, 0);
-	static bool dma_default = false;
-	fdt32_t addr_high, addr_low;
-	fdt32_t size_high, size_low;
-	int subnode, err;
-	fdt32_t reg[4];
-	fdt32_t *val;
-	char name[32];
-
-	addr_high = (u64)pma->pa >> 32;
-	addr_low = pma->pa;
-	size_high = (u64)pma->size >> 32;
-	size_low = pma->size;
-
-	if (na > 1 && addr_high)
-		sbi_snprintf(name, sizeof(name),
-			     "pma_resv%d@%x,%x", index,
-			     addr_high, addr_low);
-	else
-		sbi_snprintf(name, sizeof(name),
-			     "pma_resv%d@%x", index,
-			     addr_low);
-
-	subnode = fdt_add_subnode(fdt, parent, name);
-	if (subnode < 0)
-		return subnode;
-
-	if (pma->shared_dma) {
-		err = fdt_setprop_string(fdt, subnode, "compatible", "shared-dma-pool");
-		if (err < 0)
-			return err;
-	}
-
-	if (pma->no_map) {
-		err = fdt_setprop_empty(fdt, subnode, "no-map");
-		if (err < 0)
-			return err;
-	}
-
-	/* Linux allows single linux,dma-default region. */
-	if (pma->dma_default) {
-		if (dma_default)
-			return SBI_EINVAL;
-
-		err = fdt_setprop_empty(fdt, subnode, "linux,dma-default");
-		if (err < 0)
-			return err;
-		dma_default = true;
-	}
-
-	/* encode the <reg> property value */
-	val = reg;
-	if (na > 1)
-		*val++ = cpu_to_fdt32(addr_high);
-	*val++ = cpu_to_fdt32(addr_low);
-	if (ns > 1)
-		*val++ = cpu_to_fdt32(size_high);
-	*val++ = cpu_to_fdt32(size_low);
-
-	err = fdt_setprop(fdt, subnode, "reg", reg,
-			  (na + ns) * sizeof(fdt32_t));
-	if (err < 0)
-		return err;
-
-	return 0;
-}
-
-static int andes45_fdt_reserved_memory_fixup(void *fdt,
-					     const struct andes45_pma_region *pma,
-					     unsigned int entry)
-{
-	int parent;
-
-	/* try to locate the reserved memory node */
-	parent = fdt_path_offset(fdt, "/reserved-memory");
-	if (parent < 0) {
-		int na = fdt_address_cells(fdt, 0);
-		int ns = fdt_size_cells(fdt, 0);
-		int err;
-
-		/* if such node does not exist, create one */
-		parent = fdt_add_subnode(fdt, 0, "reserved-memory");
-		if (parent < 0)
-			return parent;
-
-		err = fdt_setprop_empty(fdt, parent, "ranges");
-		if (err < 0)
-			return err;
-
-		err = fdt_setprop_u32(fdt, parent, "#size-cells", ns);
-		if (err < 0)
-			return err;
-
-		err = fdt_setprop_u32(fdt, parent, "#address-cells", na);
-		if (err < 0)
-			return err;
-	}
-
-	return andes45_fdt_pma_resv(fdt, pma, entry, parent);
-}
-
-int andes45_pma_setup_regions(const struct andes45_pma_region *pma_regions,
-			      unsigned int pma_regions_count)
-{
-	unsigned long mmsc = csr_read(ANDES45_CSR_MMSC_CFG);
-	unsigned int dt_populate_cnt;
-	unsigned int i, j;
-	unsigned long pa;
-	void *fdt;
-	int ret;
-
-	if (!pma_regions || !pma_regions_count)
-		return 0;
-
-	if (pma_regions_count > ANDES45_MAX_PMA_REGIONS)
-		return SBI_EINVAL;
-
-	if ((mmsc & ANDES45_CSR_MMSC_PPMA_OFFSET) == 0)
-		return SBI_ENOTSUPP;
-
-	/* Configure the PMA regions */
-	for (i = 0; i < pma_regions_count; i++) {
-		pa = andes45_pma_setup(&pma_regions[i], i);
-		if (pa == SBI_EINVAL)
-			return SBI_EINVAL;
-	}
-
-	dt_populate_cnt = 0;
-	for (i = 0; i < pma_regions_count; i++) {
-		if (!pma_regions[i].dt_populate)
-			continue;
-		dt_populate_cnt++;
-	}
-
-	if (!dt_populate_cnt)
-		return 0;
-
-	fdt = fdt_get_address();
-
-	ret = fdt_open_into(fdt, fdt, fdt_totalsize(fdt) + (64 * dt_populate_cnt));
-	if (ret < 0)
-		return ret;
-
-	for (i = 0, j = 0; i < pma_regions_count; i++) {
-		if (!pma_regions[i].dt_populate)
-			continue;
-
-		ret = andes45_fdt_reserved_memory_fixup(fdt, &pma_regions[i], j++);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}

platform/generic/andes/andes_pma.c

@@ -0,0 +1,292 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2023 Renesas Electronics Corp.
+ * Copyright (c) 2024 Andes Technology Corporation
+ *
+ * Authors:
+ *      Ben Zong-You Xie <ben717@andestech.com>
+ *      Lad Prabhakar <prabhakar.mahadev-lad.rj@bp.renesas.com>
+ */
+
+#include <andes/andes.h>
+#include <andes/andes_pma.h>
+#include <libfdt.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_io.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_error.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+
+static unsigned long andes_pma_read_num(unsigned int csr_num)
+{
+#define switchcase_csr_read(__csr_num, __val)		\
+	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)
+
+	unsigned long ret = 0;
+
+	switch (csr_num) {
+	switchcase_csr_read_4(CSR_PMACFG0, ret)
+	switchcase_csr_read_16(CSR_PMAADDR0, ret)
+	default:
+		sbi_panic("%s: Unknown Andes PMA CSR %#x", __func__, csr_num);
+		break;
+	}
+
+	return ret;
+
+#undef switchcase_csr_read_16
+#undef switchcase_csr_read_8
+#undef switchcase_csr_read_4
+#undef switchcase_csr_read_2
+#undef switchcase_csr_read
+}
+
+static void andes_pma_write_num(unsigned int csr_num, unsigned long val)
+{
+#define switchcase_csr_write(__csr_num, __val)		\
+	case __csr_num:					\
+		csr_write(__csr_num, __val);		\
+		break;
+#define switchcase_csr_write_2(__csr_num, __val)	\
+	switchcase_csr_write(__csr_num + 0, __val)	\
+	switchcase_csr_write(__csr_num + 1, __val)
+#define switchcase_csr_write_4(__csr_num, __val)	\
+	switchcase_csr_write_2(__csr_num + 0, __val)	\
+	switchcase_csr_write_2(__csr_num + 2, __val)
+#define switchcase_csr_write_8(__csr_num, __val)	\
+	switchcase_csr_write_4(__csr_num + 0, __val)	\
+	switchcase_csr_write_4(__csr_num + 4, __val)
+#define switchcase_csr_write_16(__csr_num, __val)	\
+	switchcase_csr_write_8(__csr_num + 0, __val)	\
+	switchcase_csr_write_8(__csr_num + 8, __val)
+
+	switch (csr_num) {
+	switchcase_csr_write_4(CSR_PMACFG0, val)
+	switchcase_csr_write_16(CSR_PMAADDR0, val)
+	default:
+		sbi_panic("%s: Unknown Andes PMA CSR %#x", __func__, csr_num);
+		break;
+	}
+
+#undef switchcase_csr_write_16
+#undef switchcase_csr_write_8
+#undef switchcase_csr_write_4
+#undef switchcase_csr_write_2
+#undef switchcase_csr_write
+}
+
+static inline bool not_napot(unsigned long addr, unsigned long size)
+{
+	return ((size & (size - 1)) || (addr & (size - 1)));
+}
+
+static unsigned long andes_pma_setup(const struct andes_pma_region *pma_region,
+				     unsigned int entry_id)
+{
+	unsigned long size = pma_region->size;
+	unsigned long addr = pma_region->pa;
+	unsigned int pma_cfg_addr;
+	unsigned long pmacfg_val;
+	unsigned long pmaaddr;
+	char *pmaxcfg;
+
+	/* Check for a 4KiB granularity NAPOT region*/
+	if (size < ANDES_PMA_GRANULARITY || not_napot(addr, size) ||
+	    !(pma_region->flags & ANDES_PMACFG_ETYP_NAPOT))
+		return SBI_EINVAL;
+
+#if __riscv_xlen == 64
+	pma_cfg_addr = CSR_PMACFG0 + ((entry_id / 8) ? 2 : 0);
+	pmacfg_val = andes_pma_read_num(pma_cfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 8);
+#elif __riscv_xlen == 32
+	pma_cfg_addr = CSR_PMACFG0 + (entry_id / 4);
+	pmacfg_val = andes_pma_read_num(pma_cfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 4);
+#else
+#error "Unexpected __riscv_xlen"
+#endif
+	*pmaxcfg = pma_region->flags;
+
+	andes_pma_write_num(pma_cfg_addr, pmacfg_val);
+
+	pmaaddr = (addr >> 2) + (size >> 3) - 1;
+
+	andes_pma_write_num(CSR_PMAADDR0 + entry_id, pmaaddr);
+
+	return andes_pma_read_num(CSR_PMAADDR0 + entry_id) == pmaaddr ?
+	       pmaaddr : SBI_EINVAL;
+}
+
+static int andes_fdt_pma_resv(void *fdt, const struct andes_pma_region *pma,
+			      unsigned int index, int parent)
+{
+	int na = fdt_address_cells(fdt, 0);
+	int ns = fdt_size_cells(fdt, 0);
+	static bool dma_default = false;
+	fdt32_t addr_high, addr_low;
+	fdt32_t size_high, size_low;
+	int subnode, err;
+	fdt32_t reg[4];
+	fdt32_t *val;
+	char name[32];
+
+	addr_high = (u64)pma->pa >> 32;
+	addr_low = pma->pa;
+	size_high = (u64)pma->size >> 32;
+	size_low = pma->size;
+
+	if (na > 1 && addr_high) {
+		sbi_snprintf(name, sizeof(name),
+			     "pma_resv%d@%x,%x",
+			     index, addr_high, addr_low);
+	} else {
+		sbi_snprintf(name, sizeof(name),
+			     "pma_resv%d@%x",
+			     index, addr_low);
+	}
+	subnode = fdt_add_subnode(fdt, parent, name);
+	if (subnode < 0)
+		return subnode;
+
+	if (pma->shared_dma) {
+		err = fdt_setprop_string(fdt, subnode, "compatible",
+					 "shared-dma-pool");
+		if (err < 0)
+			return err;
+	}
+
+	if (pma->no_map) {
+		err = fdt_setprop_empty(fdt, subnode, "no-map");
+		if (err < 0)
+			return err;
+	}
+
+	/* Linux allows single linux,dma-default region. */
+	if (pma->dma_default) {
+		if (dma_default)
+			return SBI_EINVAL;
+
+		err = fdt_setprop_empty(fdt, subnode, "linux,dma-default");
+		if (err < 0)
+			return err;
+		dma_default = true;
+	}
+
+	/* Encode the <reg> property value */
+	val = reg;
+	if (na > 1)
+		*val++ = cpu_to_fdt32(addr_high);
+	*val++ = cpu_to_fdt32(addr_low);
+	if (ns > 1)
+		*val++ = cpu_to_fdt32(size_high);
+	*val++ = cpu_to_fdt32(size_low);
+
+	err = fdt_setprop(fdt, subnode, "reg", reg,
+			  (na + ns) * sizeof(fdt32_t));
+	if (err < 0)
+		return err;
+
+	return 0;
+}
+
+static int andes_fdt_reserved_memory_fixup(void *fdt,
+					   const struct andes_pma_region *pma,
+					   unsigned int entry)
+{
+	int parent;
+
+	/* Try to locate the reserved memory node */
+	parent = fdt_path_offset(fdt, "/reserved-memory");
+	if (parent < 0) {
+		int na = fdt_address_cells(fdt, 0);
+		int ns = fdt_size_cells(fdt, 0);
+		int err;
+
+		/* If such node does not exist, create one */
+		parent = fdt_add_subnode(fdt, 0, "reserved-memory");
+		if (parent < 0)
+			return parent;
+
+		err = fdt_setprop_empty(fdt, parent, "ranges");
+		if (err < 0)
+			return err;
+
+		err = fdt_setprop_u32(fdt, parent, "#size-cells", ns);
+		if (err < 0)
+			return err;
+
+		err = fdt_setprop_u32(fdt, parent, "#address-cells", na);
+		if (err < 0)
+			return err;
+	}
+
+	return andes_fdt_pma_resv(fdt, pma, entry, parent);
+}
+
+int andes_pma_setup_regions(const struct andes_pma_region *pma_regions,
+			    unsigned int pma_regions_count)
+{
+	unsigned long mmsc = csr_read(CSR_MMSC_CFG);
+	unsigned int dt_populate_cnt;
+	unsigned int i, j;
+	unsigned long pa;
+	void *fdt;
+	int ret;
+
+	if (!pma_regions || !pma_regions_count)
+		return 0;
+
+	if (pma_regions_count > ANDES_MAX_PMA_REGIONS)
+		return SBI_EINVAL;
+
+	if ((mmsc & MMSC_CFG_PPMA_MASK) == 0)
+		return SBI_ENOTSUPP;
+
+	/* Configure the PMA regions */
+	dt_populate_cnt = 0;
+	for (i = 0; i < pma_regions_count; i++) {
+		pa = andes_pma_setup(&pma_regions[i], i);
+		if (pa == SBI_EINVAL)
+			return SBI_EINVAL;
+		else if (pma_regions[i].dt_populate)
+			dt_populate_cnt++;
+	}
+
+	if (!dt_populate_cnt)
+		return 0;
+
+	fdt = fdt_get_address();
+
+	ret = fdt_open_into(fdt, fdt,
+			    fdt_totalsize(fdt) + (64 * dt_populate_cnt));
+	if (ret < 0)
+		return ret;
+
+	for (i = 0, j = 0; i < pma_regions_count; i++) {
+		if (!pma_regions[i].dt_populate)
+			continue;
+
+		ret = andes_fdt_reserved_memory_fixup(fdt,
+						      &pma_regions[i],
+						      j++);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}

platform/generic/andes/andes_pmu.c

@@ -5,8 +5,7 @@
  * Copyright (C) 2023 Andes Technology Corporation
  */
 
-#include <andes/andes45.h>
-#include <andes/andes_hpm.h>
+#include <andes/andes.h>
 #include <andes/andes_pmu.h>
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_error.h>
@@ -86,20 +85,9 @@ int andes_pmu_extensions_init(const struct fdt_match *match,
 int andes_pmu_init(const struct fdt_match *match)
 {
 	struct sbi_scratch *scratch = sbi_scratch_thishart_ptr();
-	void *fdt = fdt_get_address();
-	int pmu_offset;
 
 	if (sbi_hart_has_extension(scratch, SBI_HART_EXT_XANDESPMU))
 		sbi_pmu_set_device(&andes_pmu);
 
-	/*
-	 * Populate default mappings if device-tree doesn't
-	 * provide a valid pmu node.
-	 */
-	pmu_offset = fdt_node_offset_by_compatible(fdt, -1, "riscv,pmu");
-	if (pmu_offset < 0)
-		return (pmu_offset == -FDT_ERR_NOTFOUND) ? andes_pmu_setup()
-							 : SBI_EFAIL;
-
 	return 0;
 }

platform/generic/andes/andes_sbi.c

@@ -3,7 +3,7 @@
  * Copyright (C) 2023 Renesas Electronics Corp.
  *
  */
-#include <andes/andes45.h>
+#include <andes/andes.h>
 #include <andes/andes_sbi.h>
 #include <sbi/riscv_asm.h>
 #include <sbi/sbi_error.h>
@@ -13,7 +13,7 @@ enum sbi_ext_andes_fid {
 	SBI_EXT_ANDES_IOCP_SW_WORKAROUND,
 };
 
-static bool andes45_cache_controllable(void)
+static bool andes_cache_controllable(void)
 {
 	return (((csr_read(CSR_MICM_CFG) & MICM_CFG_ISZ_MASK) ||
 		 (csr_read(CSR_MDCM_CFG) & MDCM_CFG_DSZ_MASK)) &&
@@ -22,14 +22,14 @@ static bool andes45_cache_controllable(void)
 		misa_extension('U'));
 }
 
-static bool andes45_iocp_disabled(void)
+static bool andes_iocp_disabled(void)
 {
 	return (csr_read(CSR_MMSC_CFG) & MMSC_IOCP_MASK) ? false : true;
 }
 
-static bool andes45_apply_iocp_sw_workaround(void)
+static bool andes_apply_iocp_sw_workaround(void)
 {
-	return andes45_cache_controllable() & andes45_iocp_disabled();
+	return andes_cache_controllable() & andes_iocp_disabled();
 }
 
 int andes_sbi_vendor_ext_provider(long funcid,
@@ -39,7 +39,7 @@ int andes_sbi_vendor_ext_provider(long funcid,
 {
 	switch (funcid) {
 	case SBI_EXT_ANDES_IOCP_SW_WORKAROUND:
-		out->value = andes45_apply_iocp_sw_workaround();
+		out->value = andes_apply_iocp_sw_workaround();
 		break;
 
 	default:

platform/generic/andes/objects.mk

@@ -5,6 +5,6 @@
 carray-platform_override_modules-$(CONFIG_PLATFORM_ANDES_AE350) += andes_ae350
 platform-objs-$(CONFIG_PLATFORM_ANDES_AE350) += andes/ae350.o andes/sleep.o
 
-platform-objs-$(CONFIG_ANDES45_PMA) += andes/andes45-pma.o
+platform-objs-$(CONFIG_ANDES_PMA) += andes/andes_pma.o
 platform-objs-$(CONFIG_ANDES_SBI) += andes/andes_sbi.o
 platform-objs-$(CONFIG_ANDES_PMU) += andes/andes_pmu.o

platform/generic/andes/sleep.S

@@ -9,7 +9,7 @@
 
 #include <sbi/riscv_encoding.h>
 #include <sbi/riscv_asm.h>
-#include <andes/andes45.h>
+#include <andes/andes.h>
 
 	.section .text, "ax", %progbits
 	.align 3

platform/generic/configs/defconfig

@@ -27,6 +27,7 @@ CONFIG_FDT_RESET_ATCWDT200=y
 CONFIG_FDT_RESET_GPIO=y
 CONFIG_FDT_RESET_HTIF=y
 CONFIG_FDT_RESET_SUNXI_WDT=y
+CONFIG_FDT_RESET_SG2042_HWMON_MCU=y
 CONFIG_FDT_RESET_SYSCON=y
 CONFIG_FDT_SERIAL=y
 CONFIG_FDT_SERIAL_CADENCE=y

platform/generic/include/andes/andes.h

@@ -1,7 +1,10 @@
-#ifndef _RISCV_ANDES45_H
-#define _RISCV_ANDES45_H
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2024 Andes Technology Corporation
+ */
 
-#define CSR_MARCHID_MICROID 0xfff
+#ifndef _RISCV_ANDES_H
+#define _RISCV_ANDES_H
 
 /* Memory and Miscellaneous Registers */
 #define CSR_MCACHE_CTL		0x7ca
@@ -23,19 +26,25 @@
 #define CSR_MCOUNTERMASK_U	0x7d3
 #define CSR_MCOUNTEROVF		0x7d4
 
+/* PMA Related Registers */
+#define CSR_PMACFG0		0xbc0
+#define CSR_PMAADDR0		0xbd0
+
 #define MICM_CFG_ISZ_OFFSET		6
-#define MICM_CFG_ISZ_MASK		(0x7  << MICM_CFG_ISZ_OFFSET)
+#define MICM_CFG_ISZ_MASK		(7 << MICM_CFG_ISZ_OFFSET)
 
 #define MDCM_CFG_DSZ_OFFSET		6
-#define MDCM_CFG_DSZ_MASK		(0x7  << MDCM_CFG_DSZ_OFFSET)
+#define MDCM_CFG_DSZ_MASK		(7 << MDCM_CFG_DSZ_OFFSET)
 
 #define MMSC_CFG_CCTLCSR_OFFSET		16
-#define MMSC_CFG_CCTLCSR_MASK		(0x1 << MMSC_CFG_CCTLCSR_OFFSET)
-#define MMSC_IOCP_OFFSET			47
-#define MMSC_IOCP_MASK			(0x1ULL << MMSC_IOCP_OFFSET)
+#define MMSC_CFG_CCTLCSR_MASK		(1 << MMSC_CFG_CCTLCSR_OFFSET)
+#define MMSC_CFG_PPMA_OFFSET		30
+#define MMSC_CFG_PPMA_MASK		(1 << MMSC_CFG_PPMA_OFFSET)
+#define MMSC_IOCP_OFFSET		47
+#define MMSC_IOCP_MASK			(1ULL << MMSC_IOCP_OFFSET)
 
 #define MCACHE_CTL_CCTL_SUEN_OFFSET	8
-#define MCACHE_CTL_CCTL_SUEN_MASK	(0x1 << MCACHE_CTL_CCTL_SUEN_OFFSET)
+#define MCACHE_CTL_CCTL_SUEN_MASK	(1 << MCACHE_CTL_CCTL_SUEN_OFFSET)
 
 /* Performance monitor */
 #define MMSC_CFG_PMNDS_MASK		(1 << 15)
@@ -58,4 +67,4 @@
 
 #endif /* __ASSEMBLER__ */
 
-#endif /* _RISCV_ANDES45_H */
+#endif /* _RISCV_ANDES_H */

platform/generic/include/andes/andes_hpm.h

@@ -1,12 +0,0 @@
-/*
- * SPDX-License-Identifier: BSD-2-Clause
- *
- * Copyright (c) 2023 Andes Technology Corporation
- */
-
-#ifndef _ANDES_HPM_H_
-#define _ANDES_HPM_H_
-
-static inline int andes_pmu_setup(void) { return 0; }
-
-#endif /* _ANDES_HPM_H_ */