firmware: fw_base.S: fix multi-core boot bug.

In a multi-core startup scenario, if both _try_lottery and
_wait_for_boot_hart use the data in the _boot_status address, when
a CPU enters OpenSBI later than boot hart set the _boot_status to
BOOT_STATUS_BOOT_HART_DONE, the CPU will modify _boot_status to 1
by amoswap.w and will never be awakened in _wait_for_boot_hart.
So let _try_lottery and _boot_status use data from two addresses.

Signed-off-by: dong.yang <dong.yang@sophgo.com>

.github/workflows/repo-lockdown.yml

@@ -0,0 +1,26 @@
+#name: 'Repo Lockdown'
+#
+#on:
+#  pull_request_target:
+#    types: opened
+#
+#permissions:
+#  pull-requests: write
+#
+#jobs:
+#  action:
+#    runs-on: ubuntu-latest
+#    steps:
+#      - uses: dessant/repo-lockdown@v4
+#        with:
+#          pr-comment: |
+#            We have mailing list based patch review so it would be great if you can send these patchs to OpenSBI mailing list.
+#
+#            You need to join OpenSBI mailing list using following link
+#            http://lists.infradead.org/mailman/listinfo/opensbi
+#
+#            Make sure you use "git send-email" to send the patches.
+#
+#            Thanks for your contribution to OpenSBI project.
+#          lock-pr: true
+#          close-pr: true

.gitignore

@@ -4,6 +4,7 @@
 # exceptions we need even begin with dot
 !.clang-format
 !.gitignore
+!.github
 
 # Object files
 *.o

Makefile

@@ -489,7 +489,7 @@ compile_d2c = $(CMD_PREFIX)mkdir -p `dirname $(1)`; \
 	     $(src_dir)/scripts/d2c.sh -i $(6) -a $(D2C_ALIGN_BYTES) -p $(D2C_NAME_PREFIX) -t $(D2C_PADDING_BYTES) > $(1)
 compile_carray = $(CMD_PREFIX)mkdir -p `dirname $(1)`; \
 	     echo " CARRAY    $(subst $(build_dir)/,,$(1))"; \
-	     $(eval CARRAY_VAR_LIST := $(carray-$(subst .c,,$(shell basename $(1)))-y)) \
+	     $(eval CARRAY_VAR_LIST := $(carray-$(subst .carray.c,,$(shell basename $(1)))-y)) \
 	     $(src_dir)/scripts/carray.sh -i $(2) -l "$(CARRAY_VAR_LIST)" > $(1)
 compile_gen_dep = $(CMD_PREFIX)mkdir -p `dirname $(1)`; \
 	     echo " GEN-DEP   $(subst $(build_dir)/,,$(1))"; \
@@ -519,7 +519,7 @@ $(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
+$(build_dir)/%.carray.c: $(src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
 $(build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_AUTOHEADER)
@@ -547,7 +547,7 @@ $(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
+$(platform_build_dir)/%.carray.c: $(platform_src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
 $(platform_build_dir)/%.dep: $(platform_src_dir)/%.c $(KCONFIG_AUTOHEADER)
@@ -590,7 +590,7 @@ $(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
+$(platform_build_dir)/%.carray.c: $(src_dir)/%.carray
 	$(call compile_carray,$@,$<)
 
 $(platform_build_dir)/%.dep: $(src_dir)/%.c $(KCONFIG_AUTOHEADER)
@@ -687,6 +687,8 @@ clean:
 	$(CMD_PREFIX)mkdir -p $(build_dir)
 	$(if $(V), @echo " RM        $(build_dir)/*.o")
 	$(CMD_PREFIX)find $(build_dir) -type f -name "*.o" -exec rm -rf {} +
+	$(if $(V), @echo " RM        $(build_dir)/*.carray.c")
+	$(CMD_PREFIX)find $(build_dir) -type f -name "*.carray.c" -exec rm -rf {} +
 	$(if $(V), @echo " RM        $(build_dir)/*.a")
 	$(CMD_PREFIX)find $(build_dir) -type f -name "*.a" -exec rm -rf {} +
 	$(if $(V), @echo " RM        $(build_dir)/*.elf")

docs/domain_support.md

@@ -180,9 +180,7 @@ The DT properties of a domain instance DT node are as follows:
   boot HART of the domain instance.
 * **next-arg1** (Optional) - The 64 bit next booting stage arg1 for the
   domain instance. If this DT property is not available and coldboot HART
-  is not assigned to the domain instance then **0x0** is used as default
-  value. If this DT property is not available and coldboot HART is assigned
-  to the domain instance then **next booting stage arg1 of coldboot HART**
+  is not assigned to the domain instance then **next booting stage arg1 of coldboot HART**
   is used as default value.
 * **next-addr** (Optional) - The 64 bit next booting stage address for the
   domain instance. If this DT property is not available and coldboot HART

docs/writing_tests.md

@@ -57,9 +57,7 @@ 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/`.
+Now, run `make clean` in order to regenerate the carray-related files.
 
 Recompile OpenSBI with the CONFIG_SBIUNIT option enabled and run it in QEMU.
 You will see something like this:

firmware/fw_base.S

@@ -47,7 +47,7 @@ _start:
 	bne	a0, a6, _wait_for_boot_hart
 _try_lottery:
 	/* Jump to relocation wait loop if we don't get relocation lottery */
-	lla	a6, _boot_status
+	lla	a6, _relocate_lottery
 	li	a7, BOOT_STATUS_LOTTERY_DONE
 	amoswap.w a6, a7, (a6)
 	bnez	a6, _wait_for_boot_hart
@@ -371,6 +371,8 @@ _skip_trap_handler_hyp:
 
 	.data
 	.align 3
+_relocate_lottery:
+	RISCV_PTR   0
 _boot_status:
 	RISCV_PTR	0
 

include/sbi/riscv_encoding.h

@@ -207,7 +207,7 @@
 
 #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)
@@ -947,7 +947,10 @@
 #define REG_PTR(insn, pos, regs)	\
 	(ulong *)((ulong)(regs) + REG_OFFSET(insn, pos))
 
-#define GET_RM(insn)			(((insn) >> 12) & 7)
+#define GET_RM(insn)			((insn & MASK_FUNCT3) >> SHIFT_FUNCT3)
+
+#define GET_RS1_NUM(insn)		((insn & MASK_RS1) >> 15)
+#define GET_CSR_NUM(insn)		((insn & MASK_CSR) >> SHIFT_CSR)
 
 #define GET_RS1(insn, regs)		(*REG_PTR(insn, SH_RS1, regs))
 #define GET_RS2(insn, regs)		(*REG_PTR(insn, SH_RS2, regs))
@@ -959,7 +962,20 @@
 #define IMM_I(insn)			((s32)(insn) >> 20)
 #define IMM_S(insn)			(((s32)(insn) >> 25 << 5) | \
 					 (s32)(((insn) >> 7) & 0x1f))
+
 #define MASK_FUNCT3			0x7000
+#define MASK_RS1			0xf8000
+#define MASK_CSR			0xfff00000
+
+#define SHIFT_FUNCT3			12
+#define SHIFT_CSR			20
+
+#define CSRRW 1
+#define CSRRS 2
+#define CSRRC 3
+#define CSRRWI 5
+#define CSRRSI 6
+#define CSRRCI 7
 
 /* clang-format on */
 

include/sbi/riscv_fp.h

@@ -15,7 +15,6 @@
 #include <sbi/sbi_types.h>
 
 #define GET_PRECISION(insn) (((insn) >> 25) & 3)
-#define GET_RM(insn) (((insn) >> 12) & 7)
 #define PRECISION_S 0
 #define PRECISION_D 1
 

include/sbi/sbi_console.h

@@ -58,8 +58,6 @@ void sbi_console_set_device(const struct sbi_console_device *dev);
 
 struct sbi_scratch;
 
-int sbi_console_init(struct sbi_scratch *scratch);
-
 #define SBI_ASSERT(cond, args) do { \
 	if (unlikely(!(cond))) \
 		sbi_panic args; \

include/sbi/sbi_fifo.h

@@ -23,6 +23,18 @@ struct sbi_fifo {
 	u16 tail;
 };
 
+#define SBI_FIFO_INITIALIZER(__queue_mem, __entries, __entry_size)	\
+{	.queue = __queue_mem,						\
+	.qlock = SPIN_LOCK_INITIALIZER,					\
+	.num_entries = __entries,					\
+	.entry_size = __entry_size,					\
+	.avail = 0,							\
+	.tail = 0,							\
+}
+
+#define SBI_FIFO_DEFINE(__name, __queue_mem, __entries, __entry_size)	\
+struct sbi_fifo __name = SBI_FIFO_INITIALIZER(__queue_mem, __entries, __entry_size)
+
 enum sbi_fifo_inplace_update_types {
 	SBI_FIFO_SKIP,
 	SBI_FIFO_UPDATED,
@@ -30,7 +42,7 @@ enum sbi_fifo_inplace_update_types {
 };
 
 int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data);
-int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data);
+int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data, bool force);
 void sbi_fifo_init(struct sbi_fifo *fifo, void *queue_mem, u16 entries,
 		   u16 entry_size);
 int sbi_fifo_is_empty(struct sbi_fifo *fifo);

include/sbi/sbi_heap.h

@@ -12,16 +12,41 @@
 
 #include <sbi/sbi_types.h>
 
+/* Opaque declaration of heap control struct */
+struct sbi_heap_control;
+
+/* Global heap control structure */
+extern struct sbi_heap_control global_hpctrl;
+
 /* Alignment of heap base address and size */
 #define HEAP_BASE_ALIGN			1024
 
 struct sbi_scratch;
 
 /** Allocate from heap area */
-void *sbi_malloc(size_t size);
+void *sbi_malloc_from(struct sbi_heap_control *hpctrl, size_t size);
+
+static inline void *sbi_malloc(size_t size)
+{
+	return sbi_malloc_from(&global_hpctrl, size);
+}
+
+/** Allocate aligned from heap area */
+void *sbi_aligned_alloc_from(struct sbi_heap_control *hpctrl,
+			     size_t alignment,size_t size);
+
+static inline void *sbi_aligned_alloc(size_t alignment, size_t size)
+{
+	return sbi_aligned_alloc_from(&global_hpctrl, alignment, size);
+}
 
 /** Zero allocate from heap area */
-void *sbi_zalloc(size_t size);
+void *sbi_zalloc_from(struct sbi_heap_control *hpctrl, size_t size);
+
+static inline void *sbi_zalloc(size_t size)
+{
+	return sbi_zalloc_from(&global_hpctrl, size);
+}
 
 /** Allocate array from heap area */
 static inline void *sbi_calloc(size_t nitems, size_t size)
@@ -29,19 +54,48 @@ static inline void *sbi_calloc(size_t nitems, size_t size)
 	return sbi_zalloc(nitems * size);
 }
 
+static inline void *sbi_calloc_from(struct sbi_heap_control *hpctrl,
+				    size_t nitems, size_t size)
+{
+	return sbi_zalloc_from(hpctrl, nitems * size);
+}
+
 /** Free-up to heap area */
-void sbi_free(void *ptr);
+void sbi_free_from(struct sbi_heap_control *hpctrl, void *ptr);
+
+static inline void sbi_free(void *ptr)
+{
+	return sbi_free_from(&global_hpctrl, ptr);
+}
 
 /** Amount (in bytes) of free space in the heap area */
-unsigned long sbi_heap_free_space(void);
+unsigned long sbi_heap_free_space_from(struct sbi_heap_control *hpctrl);
+
+static inline unsigned long sbi_heap_free_space(void)
+{
+	return sbi_heap_free_space_from(&global_hpctrl);
+}
 
 /** Amount (in bytes) of used space in the heap area */
-unsigned long sbi_heap_used_space(void);
+unsigned long sbi_heap_used_space_from(struct sbi_heap_control *hpctrl);
+
+static inline unsigned long sbi_heap_used_space(void)
+{
+	return sbi_heap_used_space_from(&global_hpctrl);
+}
 
 /** Amount (in bytes) of reserved space in the heap area */
-unsigned long sbi_heap_reserved_space(void);
+unsigned long sbi_heap_reserved_space_from(struct sbi_heap_control *hpctrl);
+
+static inline unsigned long sbi_heap_reserved_space(void)
+{
+	return sbi_heap_reserved_space_from(&global_hpctrl);
+}
 
 /** Initialize heap area */
 int sbi_heap_init(struct sbi_scratch *scratch);
+int sbi_heap_init_new(struct sbi_heap_control *hpctrl, unsigned long base,
+		       unsigned long size);
+int sbi_heap_alloc_new(struct sbi_heap_control **hpctrl);
 
 #endif

include/sbi/sbi_platform.h

@@ -111,9 +111,6 @@ struct sbi_platform_operations {
 	/** Get platform specific mhpmevent value */
 	uint64_t (*pmu_xlate_to_mhpmevent)(uint32_t event_idx, uint64_t data);
 
-	/** Initialize the platform console */
-	int (*console_init)(void);
-
 	/** Initialize the platform interrupt controller for current HART */
 	int (*irqchip_init)(bool cold_boot);
 	/** Exit the platform interrupt controller for current HART */
@@ -155,7 +152,7 @@ struct sbi_platform_operations {
 
 /** Platform default heap size */
 #define SBI_PLATFORM_DEFAULT_HEAP_SIZE(__num_hart)	\
-					(0x8000 + 0x800 * (__num_hart))
+					(0x8000 + 0x1000 * (__num_hart))
 
 /** Representation of a platform */
 struct sbi_platform {
@@ -549,20 +546,6 @@ static inline uint64_t sbi_platform_pmu_xlate_to_mhpmevent(const struct sbi_plat
 	return 0;
 }
 
-/**
- * Initialize the platform console
- *
- * @param plat pointer to struct sbi_platform
- *
- * @return 0 on success and negative error code on failure
- */
-static inline int sbi_platform_console_init(const struct sbi_platform *plat)
-{
-	if (plat && sbi_platform_ops(plat)->console_init)
-		return sbi_platform_ops(plat)->console_init();
-	return 0;
-}
-
 /**
  * Initialize the platform interrupt controller for current HART
  *

lib/sbi/Kconfig

@@ -1,6 +1,10 @@
 # SPDX-License-Identifier: BSD-2-Clause
 
-menu "SBI Extension Support"
+menu "Generic SBI Support"
+
+config CONSOLE_EARLY_BUFFER_SIZE
+	int "Early console buffer size (bytes)"
+	default 256
 
 config SBI_ECALL_TIME
 	bool "Timer extension"

lib/sbi/objects.mk

@@ -13,7 +13,7 @@ libsbi-objs-y += riscv_hardfp.o
 libsbi-objs-y += riscv_locks.o
 
 libsbi-objs-y += sbi_ecall.o
-libsbi-objs-y += sbi_ecall_exts.o
+libsbi-objs-y += sbi_ecall_exts.carray.o
 
 # The order of below extensions is performance optimized
 carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_TIME) += ecall_time

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

@@ -9,6 +9,7 @@
 
 #include <sbi/riscv_locks.h>
 #include <sbi/sbi_console.h>
+#include <sbi/sbi_fifo.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_scratch.h>
@@ -21,6 +22,15 @@ static char console_tbuf[CONSOLE_TBUF_MAX];
 static u32 console_tbuf_len;
 static spinlock_t console_out_lock	       = SPIN_LOCK_INITIALIZER;
 
+#ifdef CONFIG_CONSOLE_EARLY_BUFFER_SIZE
+#define CONSOLE_EARLY_BUFFER_SIZE	CONFIG_CONSOLE_EARLY_BUFFER_SIZE
+#else
+#define CONSOLE_EARLY_BUFFER_SIZE	256
+#endif
+static char console_early_buffer[CONSOLE_EARLY_BUFFER_SIZE] = { 0 };
+static SBI_FIFO_DEFINE(console_early_fifo, console_early_buffer, \
+		       CONSOLE_EARLY_BUFFER_SIZE, sizeof(char));
+
 bool sbi_isprintable(char c)
 {
 	if (((31 < c) && (c < 127)) || (c == '\f') || (c == '\r') ||
@@ -39,6 +49,7 @@ int sbi_getc(void)
 
 static unsigned long nputs(const char *str, unsigned long len)
 {
+	char ch;
 	unsigned long i;
 
 	if (console_dev) {
@@ -51,6 +62,11 @@ static unsigned long nputs(const char *str, unsigned long len)
 				console_dev->console_putc(str[i]);
 			}
 		}
+	} else {
+		for (i = 0; i < len; i++) {
+			ch = str[i];
+			sbi_fifo_enqueue(&console_early_fifo, &ch, true);
+		}
 	}
 	return len;
 }
@@ -472,19 +488,19 @@ const struct sbi_console_device *sbi_console_get_device(void)
 
 void sbi_console_set_device(const struct sbi_console_device *dev)
 {
+	char ch;
+	bool flush_early_fifo = false;
+
 	if (!dev)
 		return;
 
+	if (!console_dev)
+		flush_early_fifo = true;
+
 	console_dev = dev;
-}
-
-int sbi_console_init(struct sbi_scratch *scratch)
-{
-	int rc = sbi_platform_console_init(sbi_platform_ptr(scratch));
-
-	/* console is not a necessary device */
-	if (rc == SBI_ENODEV)
-		return 0;
-
-	return rc;
+
+	if (flush_early_fifo) {
+		while (!sbi_fifo_dequeue(&console_early_fifo, &ch))
+			sbi_putc(ch);
+	}
 }

lib/sbi/sbi_dbtr.c

@@ -357,7 +357,7 @@ 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)))
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
 		return;
 
 	state = trig->state;
@@ -403,7 +403,7 @@ static void dbtr_trigger_disable(struct sbi_dbtr_trigger *trig)
 {
 	unsigned long tdata1;
 
-	if (!trig && !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
 		return;
 
 	tdata1 = trig->tdata1;
@@ -429,7 +429,7 @@ static void dbtr_trigger_disable(struct sbi_dbtr_trigger *trig)
 
 static void dbtr_trigger_clear(struct sbi_dbtr_trigger *trig)
 {
-	if (!trig && !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
+	if (!trig || !(trig->state & RV_DBTR_BIT_MASK(TS, MAPPED)))
 		return;
 
 	csr_write(CSR_TSELECT, trig->index);

lib/sbi/sbi_emulate_csr.c

@@ -10,7 +10,6 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/sbi_bitops.h>
-#include <sbi/sbi_console.h>
 #include <sbi/sbi_emulate_csr.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
@@ -151,10 +150,6 @@ int sbi_emulate_csr_read(int csr_num, struct sbi_trap_regs *regs,
 		break;
 	}
 
-	if (ret)
-		sbi_dprintf("%s: hartid%d: invalid csr_num=0x%x\n",
-			    __func__, current_hartid(), csr_num);
-
 	return ret;
 }
 
@@ -189,9 +184,5 @@ int sbi_emulate_csr_write(int csr_num, struct sbi_trap_regs *regs,
 		break;
 	}
 
-	if (ret)
-		sbi_dprintf("%s: hartid%d: invalid csr_num=0x%x\n",
-			    __func__, current_hartid(), csr_num);
-
 	return ret;
 }

lib/sbi/sbi_fifo.c

@@ -66,11 +66,63 @@ static inline void  __sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
 	if (head >= fifo->num_entries)
 		head = head - fifo->num_entries;
 
-	sbi_memcpy((char *)fifo->queue + head * fifo->entry_size, data, fifo->entry_size);
+	switch (fifo->entry_size) {
+	case 1:
+		*(char *)(fifo->queue + head * fifo->entry_size) = *(char *)data;
+		break;
+	case 2:
+		*(u16 *)(fifo->queue + head * fifo->entry_size) = *(u16 *)data;
+		break;
+	case 4:
+		*(u32 *)(fifo->queue + head * fifo->entry_size) = *(u32 *)data;
+		break;
+#if __riscv_xlen > 32
+	case 8:
+		*(u64 *)(fifo->queue + head * fifo->entry_size) = *(u64 *)data;
+		break;
+#endif
+	default:
+		sbi_memcpy(fifo->queue + head * fifo->entry_size,
+			   data, fifo->entry_size);
+		break;
+	}
 
 	fifo->avail++;
 }
 
+/* Note: must be called with fifo->qlock held */
+static inline void  __sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
+{
+	if (!data)
+		goto skip_data_copy;
+
+	switch (fifo->entry_size) {
+	case 1:
+		*(char *)data = *(char *)(fifo->queue + (u32)fifo->tail * fifo->entry_size);
+		break;
+	case 2:
+		*(u16 *)data = *(u16 *)(fifo->queue + (u32)fifo->tail * fifo->entry_size);
+		break;
+	case 4:
+		*(u32 *)data = *(u32 *)(fifo->queue + (u32)fifo->tail * fifo->entry_size);
+		break;
+#if __riscv_xlen > 32
+	case 8:
+		*(u64 *)data = *(u64 *)(fifo->queue + (u32)fifo->tail * fifo->entry_size);
+		break;
+#endif
+	default:
+		sbi_memcpy(data, fifo->queue + (u32)fifo->tail * fifo->entry_size,
+			   fifo->entry_size);
+		break;
+	}
+
+skip_data_copy:
+	fifo->avail--;
+	fifo->tail++;
+	if (fifo->tail >= fifo->num_entries)
+		fifo->tail = 0;
+}
 
 /* Note: must be called with fifo->qlock held */
 static inline bool __sbi_fifo_is_empty(struct sbi_fifo *fifo)
@@ -154,7 +206,7 @@ int sbi_fifo_inplace_update(struct sbi_fifo *fifo, void *in,
 	return ret;
 }
 
-int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
+int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data, bool force)
 {
 	if (!fifo || !data)
 		return SBI_EINVAL;
@@ -162,9 +214,13 @@ int sbi_fifo_enqueue(struct sbi_fifo *fifo, void *data)
 	spin_lock(&fifo->qlock);
 
 	if (__sbi_fifo_is_full(fifo)) {
-		spin_unlock(&fifo->qlock);
-		return SBI_ENOSPC;
+		if (!force) {
+			spin_unlock(&fifo->qlock);
+			return SBI_ENOSPC;
+		}
+		__sbi_fifo_dequeue(fifo, NULL);
 	}
+
 	__sbi_fifo_enqueue(fifo, data);
 
 	spin_unlock(&fifo->qlock);
@@ -184,13 +240,7 @@ int sbi_fifo_dequeue(struct sbi_fifo *fifo, void *data)
 		return SBI_ENOENT;
 	}
 
-	sbi_memcpy(data, (char *)fifo->queue + (u32)fifo->tail * fifo->entry_size,
-	       fifo->entry_size);
-
-	fifo->avail--;
-	fifo->tail++;
-	if (fifo->tail >= fifo->num_entries)
-		fifo->tail = 0;
+	__sbi_fifo_dequeue(fifo, data);
 
 	spin_unlock(&fifo->qlock);
 

lib/sbi/sbi_fwft.c

@@ -251,7 +251,7 @@ int sbi_fwft_init(struct sbi_scratch *scratch, bool cold_boot)
 
 	fhs = fwft_get_hart_state_ptr(scratch);
 	if (!fhs) {
-		fhs = sbi_zalloc(sizeof(fhs) + array_size(features) * sizeof(struct fwft_config));
+		fhs = sbi_zalloc(sizeof(*fhs) + array_size(features) * sizeof(struct fwft_config));
 		if (!fhs)
 			return SBI_ENOMEM;
 

lib/sbi/sbi_heap.c

@@ -24,7 +24,7 @@ struct heap_node {
 	unsigned long size;
 };
 
-struct heap_control {
+struct sbi_heap_control {
 	spinlock_t lock;
 	unsigned long base;
 	unsigned long size;
@@ -35,71 +35,135 @@ struct heap_control {
 	struct sbi_dlist used_space_list;
 };
 
-static struct heap_control hpctrl;
+struct sbi_heap_control global_hpctrl;
 
-void *sbi_malloc(size_t size)
+static void *alloc_with_align(struct sbi_heap_control *hpctrl,
+			      size_t align, size_t size)
 {
 	void *ret = NULL;
-	struct heap_node *n, *np;
+	struct heap_node *n, *np, *rem;
+	unsigned long lowest_aligned;
+	size_t pad;
 
 	if (!size)
 		return NULL;
 
-	size += HEAP_ALLOC_ALIGN - 1;
-	size &= ~((unsigned long)HEAP_ALLOC_ALIGN - 1);
+	size += align - 1;
+	size &= ~((unsigned long)align - 1);
 
-	spin_lock(&hpctrl.lock);
+	spin_lock(&hpctrl->lock);
 
 	np = NULL;
-	sbi_list_for_each_entry(n, &hpctrl.free_space_list, head) {
-		if (size <= n->size) {
+	sbi_list_for_each_entry(n, &hpctrl->free_space_list, head) {
+		lowest_aligned = ROUNDUP(n->addr, align);
+		pad = lowest_aligned - n->addr;
+
+		if (size + pad <= n->size) {
 			np = n;
 			break;
 		}
 	}
-	if (np) {
+	if (!np)
+		goto out;
+
+	if (pad) {
+		if (sbi_list_empty(&hpctrl->free_node_list)) {
+			goto out;
+		}
+
+		n = sbi_list_first_entry(&hpctrl->free_node_list,
+					 struct heap_node, head);
+		sbi_list_del(&n->head);
+
+		if ((size + pad < np->size) &&
+		    !sbi_list_empty(&hpctrl->free_node_list)) {
+			rem = sbi_list_first_entry(&hpctrl->free_node_list,
+						   struct heap_node, head);
+			sbi_list_del(&rem->head);
+			rem->addr = np->addr + (size + pad);
+			rem->size = np->size - (size + pad);
+			sbi_list_add_tail(&rem->head,
+					  &hpctrl->free_space_list);
+		} else if (size + pad != np->size) {
+			/* Can't allocate, return n */
+			sbi_list_add(&n->head, &hpctrl->free_node_list);
+			ret = NULL;
+			goto out;
+		}
+
+		n->addr = lowest_aligned;
+		n->size = size;
+		sbi_list_add_tail(&n->head, &hpctrl->used_space_list);
+
+		np->size = pad;
+		ret = (void *)n->addr;
+	} else {
 		if ((size < np->size) &&
-		    !sbi_list_empty(&hpctrl.free_node_list)) {
-			n = sbi_list_first_entry(&hpctrl.free_node_list,
+		    !sbi_list_empty(&hpctrl->free_node_list)) {
+			n = sbi_list_first_entry(&hpctrl->free_node_list,
 						 struct heap_node, head);
 			sbi_list_del(&n->head);
-			n->addr = np->addr + np->size - size;
+			n->addr = np->addr;
 			n->size = size;
+			np->addr += size;
 			np->size -= size;
-			sbi_list_add_tail(&n->head, &hpctrl.used_space_list);
+			sbi_list_add_tail(&n->head, &hpctrl->used_space_list);
 			ret = (void *)n->addr;
 		} else if (size == np->size) {
 			sbi_list_del(&np->head);
-			sbi_list_add_tail(&np->head, &hpctrl.used_space_list);
+			sbi_list_add_tail(&np->head, &hpctrl->used_space_list);
 			ret = (void *)np->addr;
 		}
 	}
 
-	spin_unlock(&hpctrl.lock);
+out:
+	spin_unlock(&hpctrl->lock);
 
 	return ret;
 }
 
-void *sbi_zalloc(size_t size)
+void *sbi_malloc_from(struct sbi_heap_control *hpctrl, size_t size)
 {
-	void *ret = sbi_malloc(size);
+	return alloc_with_align(hpctrl, HEAP_ALLOC_ALIGN, size);
+}
+
+void *sbi_aligned_alloc_from(struct sbi_heap_control *hpctrl,
+			     size_t alignment, size_t size)
+{
+	if (alignment < HEAP_ALLOC_ALIGN)
+		alignment = HEAP_ALLOC_ALIGN;
+
+	/* Make sure alignment is power of two */
+	if ((alignment & (alignment - 1)) != 0)
+		return NULL;
+
+	/* Make sure size is multiple of alignment */
+	if (size % alignment != 0)
+		return NULL;
+
+	return alloc_with_align(hpctrl, alignment, size);
+}
+
+void *sbi_zalloc_from(struct sbi_heap_control *hpctrl, size_t size)
+{
+	void *ret = sbi_malloc_from(hpctrl, size);
 
 	if (ret)
 		sbi_memset(ret, 0, size);
 	return ret;
 }
 
-void sbi_free(void *ptr)
+void sbi_free_from(struct sbi_heap_control *hpctrl, void *ptr)
 {
 	struct heap_node *n, *np;
 
 	if (!ptr)
 		return;
 
-	spin_lock(&hpctrl.lock);
+	spin_lock(&hpctrl->lock);
 
 	np = NULL;
-	sbi_list_for_each_entry(n, &hpctrl.used_space_list, head) {
+	sbi_list_for_each_entry(n, &hpctrl->used_space_list, head) {
 		if ((n->addr <= (unsigned long)ptr) &&
 		    ((unsigned long)ptr < (n->addr + n->size))) {
 			np = n;
@@ -107,22 +171,22 @@ void sbi_free(void *ptr)
 		}
 	}
 	if (!np) {
-		spin_unlock(&hpctrl.lock);
+		spin_unlock(&hpctrl->lock);
 		return;
 	}
 
 	sbi_list_del(&np->head);
 
-	sbi_list_for_each_entry(n, &hpctrl.free_space_list, head) {
+	sbi_list_for_each_entry(n, &hpctrl->free_space_list, head) {
 		if ((np->addr + np->size) == n->addr) {
 			n->addr = np->addr;
 			n->size += np->size;
-			sbi_list_add_tail(&np->head, &hpctrl.free_node_list);
+			sbi_list_add_tail(&np->head, &hpctrl->free_node_list);
 			np = NULL;
 			break;
 		} else if (np->addr == (n->addr + n->size)) {
 			n->size += np->size;
-			sbi_list_add_tail(&np->head, &hpctrl.free_node_list);
+			sbi_list_add_tail(&np->head, &hpctrl->free_node_list);
 			np = NULL;
 			break;
 		} else if ((n->addr + n->size) < np->addr) {
@@ -132,39 +196,72 @@ void sbi_free(void *ptr)
 		}
 	}
 	if (np)
-		sbi_list_add_tail(&np->head, &hpctrl.free_space_list);
+		sbi_list_add_tail(&np->head, &hpctrl->free_space_list);
 
-	spin_unlock(&hpctrl.lock);
+	spin_unlock(&hpctrl->lock);
 }
 
-unsigned long sbi_heap_free_space(void)
+unsigned long sbi_heap_free_space_from(struct sbi_heap_control *hpctrl)
 {
 	struct heap_node *n;
 	unsigned long ret = 0;
 
-	spin_lock(&hpctrl.lock);
-	sbi_list_for_each_entry(n, &hpctrl.free_space_list, head)
+	spin_lock(&hpctrl->lock);
+	sbi_list_for_each_entry(n, &hpctrl->free_space_list, head)
 		ret += n->size;
-	spin_unlock(&hpctrl.lock);
+	spin_unlock(&hpctrl->lock);
 
 	return ret;
 }
 
-unsigned long sbi_heap_used_space(void)
+unsigned long sbi_heap_used_space_from(struct sbi_heap_control *hpctrl)
 {
-	return hpctrl.size - hpctrl.hksize - sbi_heap_free_space();
+	return hpctrl->size - hpctrl->hksize - sbi_heap_free_space();
 }
 
-unsigned long sbi_heap_reserved_space(void)
+unsigned long sbi_heap_reserved_space_from(struct sbi_heap_control *hpctrl)
 {
-	return hpctrl.hksize;
+	return hpctrl->hksize;
 }
 
-int sbi_heap_init(struct sbi_scratch *scratch)
+int sbi_heap_init_new(struct sbi_heap_control *hpctrl, unsigned long base,
+		       unsigned long size)
 {
 	unsigned long i;
 	struct heap_node *n;
 
+	/* Initialize heap control */
+	SPIN_LOCK_INIT(hpctrl->lock);
+	hpctrl->base = base;
+	hpctrl->size = size;
+	hpctrl->hkbase = hpctrl->base;
+	hpctrl->hksize = hpctrl->size / HEAP_HOUSEKEEPING_FACTOR;
+	hpctrl->hksize &= ~((unsigned long)HEAP_BASE_ALIGN - 1);
+	SBI_INIT_LIST_HEAD(&hpctrl->free_node_list);
+	SBI_INIT_LIST_HEAD(&hpctrl->free_space_list);
+	SBI_INIT_LIST_HEAD(&hpctrl->used_space_list);
+
+	/* Prepare free node list */
+	for (i = 0; i < (hpctrl->hksize / sizeof(*n)); i++) {
+		n = (struct heap_node *)(hpctrl->hkbase + (sizeof(*n) * i));
+		SBI_INIT_LIST_HEAD(&n->head);
+		n->addr = n->size = 0;
+		sbi_list_add_tail(&n->head, &hpctrl->free_node_list);
+	}
+
+	/* Prepare free space list */
+	n = sbi_list_first_entry(&hpctrl->free_node_list,
+				 struct heap_node, head);
+	sbi_list_del(&n->head);
+	n->addr = hpctrl->hkbase + hpctrl->hksize;
+	n->size = hpctrl->size - hpctrl->hksize;
+	sbi_list_add_tail(&n->head, &hpctrl->free_space_list);
+
+	return 0;
+}
+
+int sbi_heap_init(struct sbi_scratch *scratch)
+{
 	/* Sanity checks on heap offset and size */
 	if (!scratch->fw_heap_size ||
 	    (scratch->fw_heap_size & (HEAP_BASE_ALIGN - 1)) ||
@@ -173,32 +270,13 @@ int sbi_heap_init(struct sbi_scratch *scratch)
 	    (scratch->fw_heap_offset & (HEAP_BASE_ALIGN - 1)))
 		return SBI_EINVAL;
 
-	/* Initialize heap control */
-	SPIN_LOCK_INIT(hpctrl.lock);
-	hpctrl.base = scratch->fw_start + scratch->fw_heap_offset;
-	hpctrl.size = scratch->fw_heap_size;
-	hpctrl.hkbase = hpctrl.base;
-	hpctrl.hksize = hpctrl.size / HEAP_HOUSEKEEPING_FACTOR;
-	hpctrl.hksize &= ~((unsigned long)HEAP_BASE_ALIGN - 1);
-	SBI_INIT_LIST_HEAD(&hpctrl.free_node_list);
-	SBI_INIT_LIST_HEAD(&hpctrl.free_space_list);
-	SBI_INIT_LIST_HEAD(&hpctrl.used_space_list);
-
-	/* Prepare free node list */
-	for (i = 0; i < (hpctrl.hksize / sizeof(*n)); i++) {
-		n = (struct heap_node *)(hpctrl.hkbase + (sizeof(*n) * i));
-		SBI_INIT_LIST_HEAD(&n->head);
-		n->addr = n->size = 0;
-		sbi_list_add_tail(&n->head, &hpctrl.free_node_list);
-	}
-
-	/* Prepare free space list */
-	n = sbi_list_first_entry(&hpctrl.free_node_list,
-				 struct heap_node, head);
-	sbi_list_del(&n->head);
-	n->addr = hpctrl.hkbase + hpctrl.hksize;
-	n->size = hpctrl.size - hpctrl.hksize;
-	sbi_list_add_tail(&n->head, &hpctrl.free_space_list);
+	return sbi_heap_init_new(&global_hpctrl,
+				  scratch->fw_start + scratch->fw_heap_offset,
+				  scratch->fw_heap_size);
+}
 
+int sbi_heap_alloc_new(struct sbi_heap_control **hpctrl)
+{
+	*hpctrl = sbi_calloc(1, sizeof(struct sbi_heap_control));
 	return 0;
 }

lib/sbi/sbi_hsm.c

@@ -423,10 +423,12 @@ 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);
+		hdata->saved_menvcfgh = csr_read(CSR_MENVCFGH);
 #endif
-	hdata->saved_menvcfg = csr_read(CSR_MENVCFG);
+		hdata->saved_menvcfg = csr_read(CSR_MENVCFG);
+	}
 }
 
 static void __sbi_hsm_suspend_non_ret_restore(struct sbi_scratch *scratch)
@@ -434,10 +436,12 @@ 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);
 
-	csr_write(CSR_MENVCFG, hdata->saved_menvcfg);
+	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);
+		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

@@ -48,9 +48,10 @@ static int misc_mem_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
 
 static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
 {
-	int do_write, rs1_num = (insn >> 15) & 0x1f;
-	ulong rs1_val = GET_RS1(insn, regs);
-	int csr_num   = (u32)insn >> 20;
+	bool do_write	= false;
+	int rs1_num	= GET_RS1_NUM(insn);
+	ulong rs1_val	= GET_RS1(insn, regs);
+	int csr_num	= GET_CSR_NUM((u32)insn);
 	ulong prev_mode = (regs->mstatus & MSTATUS_MPP) >> MSTATUS_MPP_SHIFT;
 	ulong csr_val, new_csr_val;
 
@@ -60,32 +61,41 @@ static int system_opcode_insn(ulong insn, struct sbi_trap_regs *regs)
 		return SBI_EFAIL;
 	}
 
-	/* TODO: Ensure that we got CSR read/write instruction */
+	/* Ensure that we got CSR read/write instruction */
+	int funct3 = GET_RM(insn);
+	if (funct3 == 0 || funct3 == 4) {
+		sbi_printf("%s: Invalid opcode for CSR read/write instruction",
+			   __func__);
+		return truly_illegal_insn(insn, regs);
+	}
 
 	if (sbi_emulate_csr_read(csr_num, regs, &csr_val))
 		return truly_illegal_insn(insn, regs);
 
-	do_write = rs1_num;
-	switch (GET_RM(insn)) {
-	case 1:
+	switch (funct3) {
+	case CSRRW:
 		new_csr_val = rs1_val;
-		do_write    = 1;
+		do_write    = true;
 		break;
-	case 2:
+	case CSRRS:
 		new_csr_val = csr_val | rs1_val;
+		do_write    = (rs1_num != 0);
 		break;
-	case 3:
+	case CSRRC:
 		new_csr_val = csr_val & ~rs1_val;
+		do_write    = (rs1_num != 0);
 		break;
-	case 5:
+	case CSRRWI:
 		new_csr_val = rs1_num;
-		do_write    = 1;
+		do_write    = true;
 		break;
-	case 6:
+	case CSRRSI:
 		new_csr_val = csr_val | rs1_num;
+		do_write    = (rs1_num != 0);
 		break;
-	case 7:
+	case CSRRCI:
 		new_csr_val = csr_val & ~rs1_num;
+		do_write    = (rs1_num != 0);
 		break;
 	default:
 		return truly_illegal_insn(insn, regs);

lib/sbi/sbi_init.c

@@ -261,10 +261,6 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	if (rc)
 		sbi_hart_hang();
 
-	rc = sbi_console_init(scratch);
-	if (rc)
-		sbi_hart_hang();
-
 	rc = sbi_sse_init(scratch, true);
 	if (rc) {
 		sbi_printf("%s: sse init failed (error %d)\n", __func__, rc);

lib/sbi/sbi_sse.c

@@ -667,7 +667,7 @@ static int sse_ipi_inject_send(unsigned long hartid, uint32_t event_id)
 	sse_inject_fifo_r =
 		sbi_scratch_offset_ptr(remote_scratch, sse_inject_fifo_off);
 
-	ret = sbi_fifo_enqueue(sse_inject_fifo_r, &evt);
+	ret = sbi_fifo_enqueue(sse_inject_fifo_r, &evt, false);
 	if (ret)
 		return SBI_EFAIL;
 

lib/sbi/sbi_tlb.c

@@ -79,6 +79,7 @@ static void sbi_tlb_local_hfence_gvma(struct sbi_tlb_info *tinfo)
 
 static void sbi_tlb_local_sfence_vma(struct sbi_tlb_info *tinfo)
 {
+#ifndef CONFIG_THEAD_C9XX_ERRATA_JTLB
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
 	unsigned long i;
@@ -96,6 +97,11 @@ static void sbi_tlb_local_sfence_vma(struct sbi_tlb_info *tinfo)
 				     : "r"(start + i)
 				     : "memory");
 	}
+#else
+	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_SFENCE_VMA_RCVD);
+
+	tlb_flush_all();
+#endif
 }
 
 static void sbi_tlb_local_hfence_vvma_asid(struct sbi_tlb_info *tinfo)
@@ -145,6 +151,7 @@ static void sbi_tlb_local_hfence_gvma_vmid(struct sbi_tlb_info *tinfo)
 
 static void sbi_tlb_local_sfence_vma_asid(struct sbi_tlb_info *tinfo)
 {
+#ifndef CONFIG_THEAD_C9XX_ERRATA_JTLB
 	unsigned long start = tinfo->start;
 	unsigned long size  = tinfo->size;
 	unsigned long asid  = tinfo->asid;
@@ -167,6 +174,16 @@ static void sbi_tlb_local_sfence_vma_asid(struct sbi_tlb_info *tinfo)
 				     : "r"(start + i), "r"(asid)
 				     : "memory");
 	}
+#else
+	unsigned long asid  = tinfo->asid;
+
+	sbi_pmu_ctr_incr_fw(SBI_PMU_FW_SFENCE_VMA_ASID_RCVD);
+
+	__asm__ __volatile__("sfence.vma x0, %0"
+			:
+			: "r"(asid)
+			: "memory");
+#endif
 }
 
 static void sbi_tlb_local_fence_i(struct sbi_tlb_info *tinfo)
@@ -351,7 +368,8 @@ static int tlb_update(struct sbi_scratch *scratch,
 
 	ret = sbi_fifo_inplace_update(tlb_fifo_r, data, tlb_update_cb);
 
-	if (ret == SBI_FIFO_UNCHANGED && sbi_fifo_enqueue(tlb_fifo_r, data) < 0) {
+	if (ret == SBI_FIFO_UNCHANGED &&
+	    sbi_fifo_enqueue(tlb_fifo_r, data, false) < 0) {
 		/**
 		 * For now, Busy loop until there is space in the fifo.
 		 * There may be case where target hart is also

lib/sbi/tests/objects.mk

@@ -1,5 +1,5 @@
 libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_unit_test.o
-libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_unit_tests.o
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_unit_tests.carray.o
 
 carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += bitmap_test_suite
 libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_bitmap_test.o
@@ -12,3 +12,6 @@ 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
+
+carray-sbi_unit_tests-$(CONFIG_SBIUNIT) += math_test_suite
+libsbi-objs-$(CONFIG_SBIUNIT) += tests/sbi_math_test.o

lib/sbi/tests/sbi_math_test.c

@@ -0,0 +1,46 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright  2024  Beijing ESWIN Computing Technology Co., Ltd.
+ *
+ * Author: Dongdong Zhang <zhangdongdong@eswincomputing.com>
+ */
+#include <sbi/sbi_math.h>
+#include <sbi/sbi_unit_test.h>
+
+static void log2roundup_test(struct sbiunit_test_case *test)
+{
+    struct {
+        unsigned long input;
+        unsigned long expected;
+    } cases[] = {
+        {1, 0},
+        {2, 1},
+        {3, 2},
+        {4, 2},
+        {5, 3},
+        {8, 3},
+        {9, 4},
+        {15, 4},
+        {16, 4},
+        {17, 5},
+        {31, 5},
+        {32, 5},
+        {33, 6},
+        {63, 6},
+        {64, 6},
+        {65, 7},
+    };
+
+    for (int i = 0; i < sizeof(cases)/sizeof(cases[0]); i++) {
+        unsigned long result = log2roundup(cases[i].input);
+        SBIUNIT_EXPECT_EQ(test, result, cases[i].expected);
+    }
+}
+
+static struct sbiunit_test_case math_test_cases[] = {
+    SBIUNIT_TEST_CASE(log2roundup_test),
+    SBIUNIT_END_CASE,
+};
+
+SBIUNIT_TEST_SUITE(math_test_suite, math_test_cases);

lib/sbi/tests/sbi_unit_test.c

@@ -7,6 +7,10 @@
 #include <sbi/sbi_types.h>
 #include <sbi/sbi_console.h>
 
+#define ANSI_COLOR_GREEN "\x1b[32m"
+#define ANSI_COLOR_RED "\x1b[31m"
+#define ANSI_COLOR_RESET "\x1b[0m"
+
 extern struct sbiunit_test_suite *sbi_unit_tests[];
 extern unsigned long sbi_unit_tests_size;
 
@@ -27,10 +31,13 @@ static void run_test_suite(struct sbiunit_test_suite *suite)
 			count_fail++;
 		else
 			count_pass++;
-		sbi_printf("[%s] %s\n", s_case->failed ? "FAILED" : "PASSED",
-			   s_case->name);
+		sbi_printf("%s[%s]%s %s\n",
+			   s_case->failed ? ANSI_COLOR_RED : ANSI_COLOR_GREEN,
+			   s_case->failed ? "FAILED" : "PASSED",
+			   ANSI_COLOR_RESET, s_case->name);
 		s_case++;
 	}
+
 	sbi_printf("%u PASSED / %u FAILED / %u TOTAL\n", count_pass, count_fail,
 		   count_pass + count_fail);
 }

lib/utils/fdt/fdt_domain.c

@@ -395,8 +395,7 @@ static int __fdt_parse_domain(void *fdt, int domain_offset, void *opaque)
 		val64 = fdt32_to_cpu(val[0]);
 		val64 = (val64 << 32) | fdt32_to_cpu(val[1]);
 	} else {
-		if (domain_offset == *cold_domain_offset)
-			val64 = sbi_scratch_thishart_ptr()->next_arg1;
+		val64 = sbi_scratch_thishart_ptr()->next_arg1;
 	}
 	dom->next_arg1 = val64;
 

lib/utils/fdt/fdt_helper.c

@@ -411,6 +411,18 @@ static int fdt_parse_isa_one_hart(const char *isa, unsigned long *extensions)
 	return 0;
 }
 
+static void fdt_parse_isa_extensions_one_hart(const char *isa,
+					      unsigned long *extensions,
+					      int len)
+{
+	size_t i;
+
+	for (i = 0; i < SBI_HART_EXT_MAX; i++) {
+		if (fdt_stringlist_contains(isa, len, sbi_hart_ext[i].name))
+			__set_bit(sbi_hart_ext[i].id, extensions);
+	}
+}
+
 static int fdt_parse_isa_all_harts(void *fdt)
 {
 	u32 hartid;
@@ -434,10 +446,6 @@ static int fdt_parse_isa_all_harts(void *fdt)
 		if (!fdt_node_is_enabled(fdt, cpu_offset))
 			continue;
 
-		val = fdt_getprop(fdt, cpu_offset, "riscv,isa", &len);
-		if (!val || len <= 0)
-			return SBI_ENOENT;
-
 		scratch = sbi_hartid_to_scratch(hartid);
 		if (!scratch)
 			return SBI_ENOENT;
@@ -445,6 +453,17 @@ static int fdt_parse_isa_all_harts(void *fdt)
 		hart_exts = sbi_scratch_offset_ptr(scratch,
 						   fdt_isa_bitmap_offset);
 
+		val = fdt_getprop(fdt, cpu_offset, "riscv,isa-extensions", &len);
+		if (val && len > 0) {
+			fdt_parse_isa_extensions_one_hart((const char *)val,
+							  hart_exts, len);
+			continue;
+		}
+
+		val = fdt_getprop(fdt, cpu_offset, "riscv,isa", &len);
+		if (!val || len <= 0)
+			return SBI_ENOENT;
+
 		err = fdt_parse_isa_one_hart((const char *)val, hart_exts);
 		if (err)
 			return err;

lib/utils/gpio/objects.mk

@@ -8,7 +8,7 @@
 #
 
 libsbiutils-objs-$(CONFIG_FDT_GPIO) += gpio/fdt_gpio.o
-libsbiutils-objs-$(CONFIG_FDT_GPIO) += gpio/fdt_gpio_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_GPIO) += gpio/fdt_gpio_drivers.carray.o
 
 carray-fdt_gpio_drivers-$(CONFIG_FDT_GPIO_DESIGNWARE) += fdt_gpio_designware
 libsbiutils-objs-$(CONFIG_FDT_GPIO_DESIGNWARE) += gpio/fdt_gpio_designware.o

lib/utils/i2c/objects.mk

@@ -10,7 +10,7 @@
 libsbiutils-objs-$(CONFIG_I2C) += i2c/i2c.o
 
 libsbiutils-objs-$(CONFIG_FDT_I2C) += i2c/fdt_i2c.o
-libsbiutils-objs-$(CONFIG_FDT_I2C) += i2c/fdt_i2c_adapter_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_I2C) += i2c/fdt_i2c_adapter_drivers.carray.o
 
 carray-fdt_i2c_adapter_drivers-$(CONFIG_FDT_I2C_SIFIVE) += fdt_i2c_adapter_sifive
 libsbiutils-objs-$(CONFIG_FDT_I2C_SIFIVE) += i2c/fdt_i2c_sifive.o

lib/utils/ipi/fdt_ipi_mswi.c

@@ -56,6 +56,7 @@ static const struct fdt_match ipi_mswi_match[] = {
 	{ .compatible = "riscv,clint0", .data = &clint_offset },
 	{ .compatible = "sifive,clint0", .data = &clint_offset },
 	{ .compatible = "thead,c900-clint", .data = &clint_offset },
+	{ .compatible = "thead,c900-clint-mswi", .data = &clint_offset },
 	{ .compatible = "thead,c900-aclint-mswi" },
 	{ .compatible = "riscv,aclint-mswi" },
 	{ },

lib/utils/ipi/objects.mk

@@ -11,7 +11,7 @@ libsbiutils-objs-$(CONFIG_IPI_MSWI) += ipi/aclint_mswi.o
 libsbiutils-objs-$(CONFIG_IPI_PLICSW) += ipi/andes_plicsw.o
 
 libsbiutils-objs-$(CONFIG_FDT_IPI) += ipi/fdt_ipi.o
-libsbiutils-objs-$(CONFIG_FDT_IPI) += ipi/fdt_ipi_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_IPI) += ipi/fdt_ipi_drivers.carray.o
 
 carray-fdt_ipi_drivers-$(CONFIG_FDT_IPI_MSWI) += fdt_ipi_mswi
 libsbiutils-objs-$(CONFIG_FDT_IPI_MSWI) += ipi/fdt_ipi_mswi.o

lib/utils/irqchip/objects.mk

@@ -8,7 +8,7 @@
 #
 
 libsbiutils-objs-$(CONFIG_FDT_IRQCHIP) += irqchip/fdt_irqchip.o
-libsbiutils-objs-$(CONFIG_FDT_IRQCHIP) += irqchip/fdt_irqchip_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_IRQCHIP) += irqchip/fdt_irqchip_drivers.carray.o
 
 carray-fdt_irqchip_drivers-$(CONFIG_FDT_IRQCHIP_APLIC) += fdt_irqchip_aplic
 libsbiutils-objs-$(CONFIG_FDT_IRQCHIP_APLIC) += irqchip/fdt_irqchip_aplic.o

lib/utils/regmap/objects.mk

@@ -8,7 +8,7 @@
 #
 
 libsbiutils-objs-$(CONFIG_FDT_REGMAP) += regmap/fdt_regmap.o
-libsbiutils-objs-$(CONFIG_FDT_REGMAP) += regmap/fdt_regmap_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_REGMAP) += regmap/fdt_regmap_drivers.carray.o
 
 carray-fdt_regmap_drivers-$(CONFIG_FDT_REGMAP_SYSCON) += fdt_regmap_syscon
 libsbiutils-objs-$(CONFIG_FDT_REGMAP_SYSCON) += regmap/fdt_regmap_syscon.o

lib/utils/reset/objects.mk

@@ -8,7 +8,7 @@
 #
 
 libsbiutils-objs-$(CONFIG_FDT_RESET) += reset/fdt_reset.o
-libsbiutils-objs-$(CONFIG_FDT_RESET) += reset/fdt_reset_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_RESET) += reset/fdt_reset_drivers.carray.o
 
 carray-fdt_reset_drivers-$(CONFIG_FDT_RESET_ATCWDT200) += fdt_reset_atcwdt200
 libsbiutils-objs-$(CONFIG_FDT_RESET_ATCWDT200) += reset/fdt_reset_atcwdt200.o

lib/utils/serial/objects.mk

@@ -8,7 +8,7 @@
 #
 
 libsbiutils-objs-$(CONFIG_FDT_SERIAL) += serial/fdt_serial.o
-libsbiutils-objs-$(CONFIG_FDT_SERIAL) += serial/fdt_serial_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_SERIAL) += serial/fdt_serial_drivers.carray.o
 
 carray-fdt_serial_drivers-$(CONFIG_FDT_SERIAL_CADENCE) += fdt_serial_cadence
 libsbiutils-objs-$(CONFIG_FDT_SERIAL_CADENCE) += serial/fdt_serial_cadence.o

lib/utils/timer/fdt_timer_mtimer.c

@@ -156,6 +156,8 @@ static const struct fdt_match timer_mtimer_match[] = {
 	{ .compatible = "riscv,clint0", .data = &sifive_clint_quirks },
 	{ .compatible = "sifive,clint0", .data = &sifive_clint_quirks },
 	{ .compatible = "thead,c900-clint", .data = &thead_clint_quirks },
+	{ .compatible = "thead,c900-clint-mtimer",
+	  .data = &thead_clint_quirks },
 	{ .compatible = "thead,c900-aclint-mtimer",
 	  .data = &thead_aclint_quirks },
 	{ .compatible = "riscv,aclint-mtimer" },

lib/utils/timer/objects.mk

@@ -11,7 +11,7 @@ libsbiutils-objs-$(CONFIG_TIMER_MTIMER) += timer/aclint_mtimer.o
 libsbiutils-objs-$(CONFIG_TIMER_PLMT) += timer/andes_plmt.o
 
 libsbiutils-objs-$(CONFIG_FDT_TIMER) += timer/fdt_timer.o
-libsbiutils-objs-$(CONFIG_FDT_TIMER) += timer/fdt_timer_drivers.o
+libsbiutils-objs-$(CONFIG_FDT_TIMER) += timer/fdt_timer_drivers.carray.o
 
 carray-fdt_timer_drivers-$(CONFIG_FDT_TIMER_MTIMER) += fdt_timer_mtimer
 libsbiutils-objs-$(CONFIG_FDT_TIMER_MTIMER) += timer/fdt_timer_mtimer.o

platform/fpga/ariane/platform.c

@@ -7,7 +7,6 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/riscv_io.h>
-#include <sbi/sbi_console.h>
 #include <sbi/sbi_const.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_platform.h>
@@ -67,8 +66,15 @@ static struct aclint_mtimer_data mtimer = {
  */
 static int ariane_early_init(bool cold_boot)
 {
-	/* For now nothing to do. */
-	return 0;
+	if (!cold_boot)
+		return 0;
+
+	return uart8250_init(ARIANE_UART_ADDR,
+			     ARIANE_UART_FREQ,
+			     ARIANE_UART_BAUDRATE,
+			     ARIANE_UART_REG_SHIFT,
+			     ARIANE_UART_REG_WIDTH,
+			     ARIANE_UART_REG_OFFSET);
 }
 
 /*
@@ -87,19 +93,6 @@ static int ariane_final_init(bool cold_boot)
 	return 0;
 }
 
-/*
- * Initialize the ariane console.
- */
-static int ariane_console_init(void)
-{
-	return uart8250_init(ARIANE_UART_ADDR,
-			     ARIANE_UART_FREQ,
-			     ARIANE_UART_BAUDRATE,
-			     ARIANE_UART_REG_SHIFT,
-			     ARIANE_UART_REG_WIDTH,
-			     ARIANE_UART_REG_OFFSET);
-}
-
 static int plic_ariane_warm_irqchip_init(int m_cntx_id, int s_cntx_id)
 {
 	int ret;
@@ -175,7 +168,6 @@ static int ariane_timer_init(bool cold_boot)
 const struct sbi_platform_operations platform_ops = {
 	.early_init = ariane_early_init,
 	.final_init = ariane_final_init,
-	.console_init = ariane_console_init,
 	.irqchip_init = ariane_irqchip_init,
 	.ipi_init = ariane_ipi_init,
 	.timer_init = ariane_timer_init,

platform/fpga/openpiton/platform.c

@@ -6,7 +6,6 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
 #include <sbi/riscv_io.h>
-#include <sbi/sbi_console.h>
 #include <sbi/sbi_const.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_platform.h>
@@ -103,7 +102,10 @@ static int openpiton_early_init(bool cold_boot)
 				    ACLINT_DEFAULT_MTIMECMP_OFFSET;
 	}
 
-	return 0;
+	return uart8250_init(uart.addr, uart.freq, uart.baud,
+			     OPENPITON_DEFAULT_UART_REG_SHIFT,
+			     OPENPITON_DEFAULT_UART_REG_WIDTH,
+			     OPENPITON_DEFAULT_UART_REG_OFFSET);
 }
 
 /*
@@ -122,19 +124,6 @@ static int openpiton_final_init(bool cold_boot)
 	return 0;
 }
 
-/*
- * Initialize the openpiton console.
- */
-static int openpiton_console_init(void)
-{
-	return uart8250_init(uart.addr,
-			     uart.freq,
-			     uart.baud,
-			     OPENPITON_DEFAULT_UART_REG_SHIFT,
-			     OPENPITON_DEFAULT_UART_REG_WIDTH,
-			     OPENPITON_DEFAULT_UART_REG_OFFSET);
-}
-
 static int plic_openpiton_warm_irqchip_init(int m_cntx_id, int s_cntx_id)
 {
 	int ret;
@@ -210,7 +199,6 @@ static int openpiton_timer_init(bool cold_boot)
 const struct sbi_platform_operations platform_ops = {
 	.early_init = openpiton_early_init,
 	.final_init = openpiton_final_init,
-	.console_init = openpiton_console_init,
 	.irqchip_init = openpiton_irqchip_init,
 	.ipi_init = openpiton_ipi_init,
 	.timer_init = openpiton_timer_init,

platform/generic/Kconfig

@@ -33,6 +33,7 @@ config PLATFORM_ANDES_AE350
 	bool "Andes AE350 support"
 	select SYS_ATCSMU
 	select ANDES_PMU
+	select ANDES_PMA
 	default n
 
 config PLATFORM_RENESAS_RZFIVE
@@ -57,6 +58,13 @@ config PLATFORM_SOPHGO_SG2042
 	select THEAD_C9XX_PMU
 	default n
 
+config PLATFORM_SOPHGO_SG2044
+	bool "Sophgo sg2044 support"
+	select THEAD_C9XX_ERRATA
+	select THEAD_C9XX_ERRATA_JTLB
+	select THEAD_C9XX_PMU
+	default n
+
 config PLATFORM_STARFIVE_JH7110
 	bool "StarFive JH7110 support"
 	default n

platform/generic/andes/Kconfig

@@ -3,6 +3,11 @@
 config ANDES_PMA
 	bool "Andes PMA support"
 	default n
+	help
+	  Programmable PMA(PPMA) is a feature for Andes. PPMA allows
+	  dynamic adjustment of memory attributes in the runtime.
+	  It contains a configurable amount of PMA entries implemented
+	  as CSRs to control the attributes of memory locations in interest.
 
 config ANDES_SBI
 	bool "Andes SBI support"

platform/generic/andes/ae350.c

@@ -19,6 +19,7 @@
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_init.h>
 #include <andes/andes.h>
+#include <andes/andes_sbi.h>
 
 static struct smu_data smu = { 0 };
 extern void __ae350_enable_coherency_warmboot(void);
@@ -120,4 +121,5 @@ const struct platform_override andes_ae350 = {
 	.final_init  = ae350_final_init,
 	.extensions_init = andes_pmu_extensions_init,
 	.pmu_init = andes_pmu_init,
+	.vendor_ext_provider = andes_sbi_vendor_ext_provider,
 };

platform/generic/andes/andes_pma.c

@@ -94,35 +94,108 @@ static inline bool not_napot(unsigned long addr, unsigned long size)
 	return ((size & (size - 1)) || (addr & (size - 1)));
 }
 
+static inline bool is_pma_entry_disable(char pmaxcfg)
+{
+	return (pmaxcfg & ANDES_PMACFG_ETYP_MASK) == ANDES_PMACFG_ETYP_OFF ?
+	       true : false;
+}
+
+static char get_pmaxcfg(int entry_id)
+{
+	unsigned int pmacfg_addr;
+	unsigned long pmacfg_val;
+	char *pmaxcfg;
+
+#if __riscv_xlen == 64
+	pmacfg_addr = CSR_PMACFG0 + ((entry_id / 8) ? 2 : 0);
+	pmacfg_val = andes_pma_read_num(pmacfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 8);
+#elif __riscv_xlen == 32
+	pmacfg_addr = CSR_PMACFG0 + (entry_id / 4);
+	pmacfg_val = andes_pma_read_num(pmacfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 4);
+#else
+#error "Unexpected __riscv_xlen"
+#endif
+	return *pmaxcfg;
+}
+
+static void set_pmaxcfg(int entry_id, char flags)
+{
+	unsigned int pmacfg_addr;
+	unsigned long pmacfg_val;
+	char *pmaxcfg;
+
+#if __riscv_xlen == 64
+	pmacfg_addr = CSR_PMACFG0 + ((entry_id / 8) ? 2 : 0);
+	pmacfg_val = andes_pma_read_num(pmacfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 8);
+#elif __riscv_xlen == 32
+	pmacfg_addr = CSR_PMACFG0 + (entry_id / 4);
+	pmacfg_val = andes_pma_read_num(pmacfg_addr);
+	pmaxcfg = (char *)&pmacfg_val + (entry_id % 4);
+#else
+#error "Unexpected __riscv_xlen"
+#endif
+	*pmaxcfg = flags;
+	andes_pma_write_num(pmacfg_addr, pmacfg_val);
+}
+
+static void decode_pmaaddrx(int entry_id, unsigned long *start,
+			    unsigned long *size)
+{
+	unsigned long pmaaddr;
+	int k;
+
+	/**
+	 * Given $pmaaddr, let k = # of trailing 1s of $pmaaddr
+	 * size = 2 ^ (k + 3)
+	 * start = 4 * ($pmaaddr - (size / 8) + 1)
+	 */
+	pmaaddr = andes_pma_read_num(CSR_PMAADDR0 + entry_id);
+	k = sbi_ffz(pmaaddr);
+	*size = 1 << (k + 3);
+	*start = (pmaaddr - (1 << k) + 1) << 2;
+}
+
+static bool has_pma_region_overlap(unsigned long start, unsigned long size)
+{
+	unsigned long _start, _size, _end, end;
+	char pmaxcfg;
+
+	end = start + size - 1;
+	for (int i = 0; i < ANDES_MAX_PMA_REGIONS; i++) {
+		pmaxcfg = get_pmaxcfg(i);
+		if (is_pma_entry_disable(pmaxcfg))
+			continue;
+
+		decode_pmaaddrx(i, &_start, &_size);
+		_end = _start + _size - 1;
+
+		if (MAX(start, _start) <= MIN(end, _end)) {
+			sbi_printf(
+				"ERROR %s(): %#lx ~ %#lx overlaps with PMA%d: %#lx ~ %#lx\n",
+				__func__, start, end, i, _start, _end);
+			return true;
+		}
+	}
+
+	return false;
+}
+
 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);
+	set_pmaxcfg(entry_id, pma_region->flags);
 
 	pmaaddr = (addr >> 2) + (size >> 3) - 1;
 
@@ -241,7 +314,6 @@ static int andes_fdt_reserved_memory_fixup(void *fdt,
 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;
@@ -254,7 +326,7 @@ int andes_pma_setup_regions(const struct andes_pma_region *pma_regions,
 	if (pma_regions_count > ANDES_MAX_PMA_REGIONS)
 		return SBI_EINVAL;
 
-	if ((mmsc & MMSC_CFG_PPMA_MASK) == 0)
+	if (!andes_sbi_probe_pma())
 		return SBI_ENOTSUPP;
 
 	/* Configure the PMA regions */
@@ -290,3 +362,82 @@ int andes_pma_setup_regions(const struct andes_pma_region *pma_regions,
 
 	return 0;
 }
+
+bool andes_sbi_probe_pma(void)
+{
+	return (csr_read(CSR_MMSC_CFG) & MMSC_CFG_PPMA_MASK) ? true : false;
+}
+
+int andes_sbi_set_pma(unsigned long pa, unsigned long size, u8 flags)
+{
+	unsigned int entry_id;
+	unsigned long rc;
+	char pmaxcfg;
+	struct andes_pma_region region;
+
+	if (!andes_sbi_probe_pma()) {
+		sbi_printf("ERROR %s(): Platform does not support PPMA.\n",
+			   __func__);
+		return SBI_ERR_NOT_SUPPORTED;
+	}
+
+	if (has_pma_region_overlap(pa, size))
+		return SBI_ERR_INVALID_PARAM;
+
+	for (entry_id = 0; entry_id < ANDES_MAX_PMA_REGIONS; entry_id++) {
+		pmaxcfg = get_pmaxcfg(entry_id);
+		if (is_pma_entry_disable(pmaxcfg)) {
+			region.pa = pa;
+			region.size = size;
+			region.flags = flags;
+			break;
+		}
+	}
+
+	if (entry_id == ANDES_MAX_PMA_REGIONS) {
+		sbi_printf("ERROR %s(): All PMA entries have run out\n",
+			   __func__);
+		return SBI_ERR_FAILED;
+	}
+
+	rc = andes_pma_setup(&region, entry_id);
+	if (rc == SBI_EINVAL) {
+		sbi_printf("ERROR %s(): Failed to set PMAADDR%d\n",
+			   __func__, entry_id);
+		return SBI_ERR_FAILED;
+	}
+
+	return SBI_SUCCESS;
+}
+
+int andes_sbi_free_pma(unsigned long pa)
+{
+	unsigned long start, size;
+	char pmaxcfg;
+
+	if (!andes_sbi_probe_pma()) {
+		sbi_printf("ERROR %s(): Platform does not support PPMA.\n",
+			   __func__);
+		return SBI_ERR_NOT_SUPPORTED;
+	}
+
+	for (int i = 0; i < ANDES_MAX_PMA_REGIONS; i++) {
+		pmaxcfg = get_pmaxcfg(i);
+		if (is_pma_entry_disable(pmaxcfg))
+			continue;
+
+		decode_pmaaddrx(i, &start, &size);
+		if (start != pa)
+			continue;
+
+		set_pmaxcfg(i, ANDES_PMACFG_ETYP_OFF);
+		andes_pma_write_num(CSR_PMAADDR0 + i, 0);
+
+		return SBI_SUCCESS;
+	}
+
+	sbi_printf("ERROR %s(): Failed to find the entry with PA %#lx\n",
+		   __func__, pa);
+
+	return SBI_ERR_FAILED;
+}

platform/generic/andes/andes_sbi.c

@@ -5,12 +5,16 @@
  */
 #include <andes/andes.h>
 #include <andes/andes_sbi.h>
+#include <andes/andes_pma.h>
 #include <sbi/riscv_asm.h>
 #include <sbi/sbi_error.h>
 
 enum sbi_ext_andes_fid {
 	SBI_EXT_ANDES_FID0 = 0, /* Reserved for future use */
 	SBI_EXT_ANDES_IOCP_SW_WORKAROUND,
+	SBI_EXT_ANDES_PMA_PROBE,
+	SBI_EXT_ANDES_PMA_SET,
+	SBI_EXT_ANDES_PMA_FREE,
 };
 
 static bool andes_cache_controllable(void)
@@ -37,14 +41,24 @@ int andes_sbi_vendor_ext_provider(long funcid,
 				  struct sbi_ecall_return *out,
 				  const struct fdt_match *match)
 {
+	int ret = 0;
+
 	switch (funcid) {
 	case SBI_EXT_ANDES_IOCP_SW_WORKAROUND:
 		out->value = andes_apply_iocp_sw_workaround();
 		break;
-
+	case SBI_EXT_ANDES_PMA_PROBE:
+		out->value = andes_sbi_probe_pma();
+		break;
+	case SBI_EXT_ANDES_PMA_SET:
+		ret = andes_sbi_set_pma(regs->a0, regs->a1, regs->a2);
+		break;
+	case SBI_EXT_ANDES_PMA_FREE:
+		ret = andes_sbi_free_pma(regs->a0);
+		break;
 	default:
-		return SBI_EINVAL;
+		ret = SBI_ENOTSUPP;
 	}
 
-	return 0;
+	return ret;
 }

platform/generic/configs/defconfig

@@ -4,6 +4,7 @@ CONFIG_PLATFORM_RENESAS_RZFIVE=y
 CONFIG_PLATFORM_SIFIVE_FU540=y
 CONFIG_PLATFORM_SIFIVE_FU740=y
 CONFIG_PLATFORM_SOPHGO_SG2042=y
+CONFIG_PLATFORM_SOPHGO_SG2044=y
 CONFIG_PLATFORM_STARFIVE_JH7110=y
 CONFIG_PLATFORM_THEAD=y
 CONFIG_FDT_GPIO=y

platform/generic/include/andes/andes_pma.h

@@ -12,11 +12,15 @@
 
 #define ANDES_PMA_GRANULARITY			(1 << 12)
 
+#define ANDES_PMACFG_ETYP_OFFSET		0
+#define ANDES_PMACFG_ETYP_MASK			(3 << ANDES_PMACFG_ETYP_OFFSET)
+#define ANDES_PMACFG_ETYP_OFF			(0 << ANDES_PMACFG_ETYP_OFFSET)
 /* Naturally aligned power of 2 region */
-#define ANDES_PMACFG_ETYP_NAPOT			3
+#define ANDES_PMACFG_ETYP_NAPOT			(3 << ANDES_PMACFG_ETYP_OFFSET)
 
+#define ANDES_PMACFG_MTYP_OFFSET		2
 /* Memory, Non-cacheable, Bufferable */
-#define ANDES_PMACFG_MTYP_MEM_NON_CACHE_BUF	(3 << 2)
+#define ANDES_PMACFG_MTYP_MEM_NON_CACHE_BUF	(3 << ANDES_PMACFG_MTYP_OFFSET)
 
 /**
  * struct andes_pma_region - Describes PMA regions
@@ -47,4 +51,39 @@ struct andes_pma_region {
 int andes_pma_setup_regions(const struct andes_pma_region *pma_regions,
 			    unsigned int pma_regions_count);
 
+/**
+ * Programmable PMA(PPMA) is a feature for Andes. PPMA allows dynamic adjustment
+ * of memory attributes in the runtime. It contains a configurable amount of PMA
+ * entries implemented as CSRs to control the attributes of memory locations.
+ *
+ * Check if hardware supports PPMA
+ *
+ * @return true if PPMA is supported
+ * @return false if PPMA is not supported
+ */
+bool andes_sbi_probe_pma(void);
+
+/**
+ * Set a NAPOT region with given memory attributes
+ * @param pa: Start address of the NAPOT region
+ * @param size: Size of the NAPOT region
+ * @param flags: Memory attributes set to the NAPOT region
+ *
+ * @return SBI_SUCCESS on success
+ * @return SBI_ERR_NOT_SUPPORTED if hardware does not support PPMA features
+ * @return SBI_ERR_INVALID_PARAM if the given region is overlapped with the
+ *	   region that has been set already
+ * @return SBI_ERR_FAILED if available entries have run out or setup fails
+ */
+int andes_sbi_set_pma(unsigned long pa, unsigned long size, u8 flags);
+
+/**
+ * Reset the memory attribute of a NAPOT region
+ * @param pa Start address of the NAPOT region
+ *
+ * @return SBI_SUCCESS on success
+ * @return SBI_ERR_FAILED if the given region is not set before
+ */
+int andes_sbi_free_pma(unsigned long pa);
+
 #endif /* _ANDES_PMA_H_ */

platform/generic/objects.mk

@@ -19,7 +19,7 @@ platform-runcmd = qemu-system-riscv$(PLATFORM_RISCV_XLEN) -M virt -m 256M \
 
 # Objects to build
 platform-objs-y += platform.o
-platform-objs-y += platform_override_modules.o
+platform-objs-y += platform_override_modules.carray.o
 
 # Blobs to build
 FW_DYNAMIC=y

platform/generic/platform.c

@@ -221,9 +221,23 @@ static int generic_nascent_init(void)
 
 static int generic_early_init(bool cold_boot)
 {
-	if (cold_boot)
+	int rc;
+
+	if (cold_boot) {
 		fdt_reset_init();
 
+		if (semihosting_enabled())
+			rc = semihosting_init();
+		else
+			rc = fdt_serial_init();
+
+		/* console is not a necessary device for tp scalar*/
+		if (rc == SBI_ENODEV)
+			return 0;
+		else if (rc)
+			return rc;
+	}
+
 	if (!generic_plat || !generic_plat->early_init)
 		return 0;
 
@@ -378,14 +392,6 @@ static uint64_t generic_pmu_xlate_to_mhpmevent(uint32_t event_idx,
 	return evt_val;
 }
 
-static int generic_console_init(void)
-{
-	if (semihosting_enabled())
-		return semihosting_init();
-	else
-		return fdt_serial_init();
-}
-
 const struct sbi_platform_operations platform_ops = {
 	.cold_boot_allowed	= generic_cold_boot_allowed,
 	.nascent_init		= generic_nascent_init,
@@ -395,7 +401,6 @@ const struct sbi_platform_operations platform_ops = {
 	.final_exit		= generic_final_exit,
 	.extensions_init	= generic_extensions_init,
 	.domains_init		= generic_domains_init,
-	.console_init		= generic_console_init,
 	.irqchip_init		= fdt_irqchip_init,
 	.irqchip_exit		= fdt_irqchip_exit,
 	.ipi_init		= fdt_ipi_init,

platform/generic/sophgo/objects.mk

@@ -7,3 +7,5 @@
 
 carray-platform_override_modules-$(CONFIG_PLATFORM_SOPHGO_SG2042) += sophgo_sg2042
 platform-objs-$(CONFIG_PLATFORM_SOPHGO_SG2042) += sophgo/sg2042.o
+carray-platform_override_modules-$(CONFIG_PLATFORM_SOPHGO_SG2044) += sophgo_sg2044
+platform-objs-$(CONFIG_PLATFORM_SOPHGO_SG2044) += sophgo/sg2044.o

platform/generic/sophgo/sg2044.c

@@ -0,0 +1,49 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c)
+ *
+ * Authors:
+ *   Inochi Amaoto <inochiama@outlook.com>
+ *   YuQing Cai <caiyuqing_hz@163.com>
+ *   ZhenYu Zhang <1204122531@qq.com>
+ */
+
+#include <platform_override.h>
+#include <thead/c9xx_pmu.h>
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_const.h>
+#include <sbi/sbi_ecall_interface.h>
+#include <sbi/sbi_pmu.h>
+#include <sbi/sbi_platform.h>
+#include <sbi_utils/fdt/fdt_helper.h>
+
+static u32 selected_hartid = -1;
+
+static bool sg2044_cold_boot_allowed(u32 hartid,
+                                   const struct fdt_match *match)
+{
+	if (selected_hartid != -1)
+		return (selected_hartid == hartid);
+
+	return true;
+}
+
+static int sg2044_extensions_init(const struct fdt_match *match,
+				  struct sbi_hart_features *hfeatures)
+{
+	thead_c9xx_register_pmu_device();
+	return 0;
+}
+
+static const struct fdt_match sophgo_sg2044_match[] = {
+	{ .compatible = "sophgo,sg2044" },
+	{ },
+};
+
+const struct platform_override sophgo_sg2044 = {
+	.match_table		= sophgo_sg2044_match,
+	.cold_boot_allowed 	= sg2044_cold_boot_allowed,
+	.extensions_init	= sg2044_extensions_init,
+};

platform/generic/thead/Kconfig

@@ -7,3 +7,7 @@ config THEAD_C9XX_PMU
 config THEAD_C9XX_ERRATA
 	bool "T-HEAD c9xx errata support"
 	default n
+
+config THEAD_C9XX_ERRATA_JTLB
+	bool "T-HEAD c9xx errata(JTLB) support"
+	default n

platform/kendryte/k210/platform.c

@@ -9,7 +9,6 @@
 
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_encoding.h>
-#include <sbi/sbi_console.h>
 #include <sbi/sbi_const.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_system.h>
@@ -109,10 +108,13 @@ static struct sbi_system_reset_device k210_reset = {
 
 static int k210_early_init(bool cold_boot)
 {
-	if (cold_boot)
-		sbi_system_reset_add_device(&k210_reset);
+	if (!cold_boot)
+		return 0;
 
-	return 0;
+	sbi_system_reset_add_device(&k210_reset);
+
+	return sifive_uart_init(K210_UART_BASE_ADDR, k210_get_clk_freq(),
+				K210_UART_BAUDRATE);
 }
 
 static int k210_final_init(bool cold_boot)
@@ -130,12 +132,6 @@ static int k210_final_init(bool cold_boot)
 	return 0;
 }
 
-static int k210_console_init(void)
-{
-	return sifive_uart_init(K210_UART_BASE_ADDR, k210_get_clk_freq(),
-				K210_UART_BAUDRATE);
-}
-
 static int k210_irqchip_init(bool cold_boot)
 {
 	int rc;
@@ -181,8 +177,6 @@ const struct sbi_platform_operations platform_ops = {
 
 	.final_init	= k210_final_init,
 
-	.console_init	= k210_console_init,
-
 	.irqchip_init = k210_irqchip_init,
 
 	.ipi_init  = k210_ipi_init,

platform/nuclei/ux600/platform.c

@@ -11,7 +11,6 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_io.h>
 #include <sbi/riscv_encoding.h>
-#include <sbi/sbi_console.h>
 #include <sbi/sbi_const.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_system.h>
@@ -150,8 +149,10 @@ static int ux600_early_init(bool cold_boot)
 {
 	u32 regval;
 
-	if (cold_boot)
-		sbi_system_reset_add_device(&ux600_reset);
+	if (!cold_boot)
+		return 0;
+
+	sbi_system_reset_add_device(&ux600_reset);
 
 	/* Measure CPU Frequency using Timer */
 	ux600_clk_freq = ux600_get_clk_freq();
@@ -163,7 +164,9 @@ static int ux600_early_init(bool cold_boot)
 	regval = readl((void *)(UX600_GPIO_ADDR + UX600_GPIO_IOF_EN_OFS)) |
 		UX600_GPIO_IOF_UART0_MASK;
 	writel(regval, (void *)(UX600_GPIO_ADDR + UX600_GPIO_IOF_EN_OFS));
-	return 0;
+
+	return sifive_uart_init(UX600_DEBUG_UART, ux600_clk_freq,
+				UX600_UART_BAUDRATE);
 }
 
 static void ux600_modify_dt(void *fdt)
@@ -184,12 +187,6 @@ static int ux600_final_init(bool cold_boot)
 	return 0;
 }
 
-static int ux600_console_init(void)
-{
-	return sifive_uart_init(UX600_DEBUG_UART, ux600_clk_freq,
-				UX600_UART_BAUDRATE);
-}
-
 static int ux600_irqchip_init(bool cold_boot)
 {
 	int rc;
@@ -234,7 +231,6 @@ static int ux600_timer_init(bool cold_boot)
 const struct sbi_platform_operations platform_ops = {
 	.early_init		= ux600_early_init,
 	.final_init		= ux600_final_init,
-	.console_init		= ux600_console_init,
 	.irqchip_init		= ux600_irqchip_init,
 	.ipi_init		= ux600_ipi_init,
 	.timer_init		= ux600_timer_init,

platform/template/platform.c

@@ -64,7 +64,12 @@ static struct aclint_mtimer_data mtimer = {
  */
 static int platform_early_init(bool cold_boot)
 {
-	return 0;
+	if (!cold_boot)
+		return 0;
+
+	/* Example if the generic UART8250 driver is used */
+	return uart8250_init(PLATFORM_UART_ADDR, PLATFORM_UART_INPUT_FREQ,
+			     PLATFORM_UART_BAUDRATE, 0, 1, 0);
 }
 
 /*
@@ -75,16 +80,6 @@ static int platform_final_init(bool cold_boot)
 	return 0;
 }
 
-/*
- * Initialize the platform console.
- */
-static int platform_console_init(void)
-{
-	/* Example if the generic UART8250 driver is used */
-	return uart8250_init(PLATFORM_UART_ADDR, PLATFORM_UART_INPUT_FREQ,
-			     PLATFORM_UART_BAUDRATE, 0, 1, 0);
-}
-
 /*
  * Initialize the platform interrupt controller for current HART.
  */
@@ -143,7 +138,6 @@ static int platform_timer_init(bool cold_boot)
 const struct sbi_platform_operations platform_ops = {
 	.early_init		= platform_early_init,
 	.final_init		= platform_final_init,
-	.console_init		= platform_console_init,
 	.irqchip_init		= platform_irqchip_init,
 	.ipi_init		= platform_ipi_init,
 	.timer_init		= platform_timer_init

scripts/carray.sh

@@ -61,6 +61,7 @@ if [ -z "${ARRAY_NAME}" ]; then
 	usage
 fi
 
+printf "// Generated with $(basename $0) from $(basename ${CONFIG_FILE})\n"
 printf "#include <%s>\n\n" "${TYPE_HEADER}"
 
 for VAR in ${VAR_LIST}; do